|
| 1 | +diff --git a/mpfs_hal/common/mss_mtrap.c b/mpfs_hal/common/mss_mtrap.c |
| 2 | +index 2226d7b..92f3b4a 100644 |
| 3 | +--- a/mpfs_hal/common/mss_mtrap.c |
| 4 | ++++ b/mpfs_hal/common/mss_mtrap.c |
| 5 | +@@ -5,6 +5,7 @@ |
| 6 | + * |
| 7 | + * MPFS HAL Embedded Software |
| 8 | + * |
| 9 | ++ * Modifications copyright (C) 2021 Hex Five Security, Inc.* |
| 10 | + */ |
| 11 | + |
| 12 | + /*************************************************************************** |
| 13 | +@@ -20,8 +21,6 @@ |
| 14 | + extern "C" { |
| 15 | + #endif |
| 16 | + |
| 17 | +- |
| 18 | +- |
| 19 | + void handle_local_interrupt(uint8_t interrupt_no); |
| 20 | + void handle_m_soft_interrupt(void); |
| 21 | + void handle_m_timer_interrupt(void); |
| 22 | +@@ -32,6 +31,12 @@ void pmp_trap(uintptr_t * regs, uintptr_t mcause, uintptr_t mepc); |
| 23 | + void trap_from_machine_mode(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc); |
| 24 | + void bad_trap(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc); |
| 25 | + |
| 26 | ++void handle_m_trap_h0(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc); /* Hex Five mod */ |
| 27 | ++void handle_m_trap_h1(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc); /* Hex Five mod */ |
| 28 | ++void handle_m_trap_h2(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc); /* Hex Five mod */ |
| 29 | ++void handle_m_trap_h3(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc); /* Hex Five mod */ |
| 30 | ++void handle_m_trap_h4(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc); /* Hex Five mod */ |
| 31 | ++void handle_m_trap(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc); /* Hex Five mod */ |
| 32 | + |
| 33 | + void bad_trap(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc) |
| 34 | + { |
| 35 | +@@ -748,33 +753,63 @@ void trap_from_machine_mode(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc) |
| 36 | + } |
| 37 | + else |
| 38 | + { |
| 39 | +- uint32_t i = 0U; |
| 40 | +- while(1) |
| 41 | ++ |
| 42 | ++ switch(read_csr(mhartid)) { |
| 43 | ++ case 0U: handle_m_trap_h0(regs, mcause, mepc); break; |
| 44 | ++ case 1U: handle_m_trap_h1(regs, mcause, mepc); break; |
| 45 | ++ case 2U: handle_m_trap_h2(regs, mcause, mepc); break; |
| 46 | ++ case 3U: handle_m_trap_h3(regs, mcause, mepc); break; |
| 47 | ++ case 4U: handle_m_trap_h4(regs, mcause, mepc); break; |
| 48 | ++ default: handle_m_trap(regs, mcause, mepc); |
| 49 | ++ } |
| 50 | ++ |
| 51 | ++ } |
| 52 | ++} |
| 53 | ++ |
| 54 | ++__attribute__((weak)) void handle_m_trap_h0(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc){ |
| 55 | ++ handle_m_trap(regs, dummy, mepc); |
| 56 | ++} |
| 57 | ++__attribute__((weak)) void handle_m_trap_h1(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc){ |
| 58 | ++ handle_m_trap(regs, dummy, mepc); |
| 59 | ++} |
| 60 | ++__attribute__((weak)) void handle_m_trap_h2(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc){ |
| 61 | ++ handle_m_trap(regs, dummy, mepc); |
| 62 | ++} |
| 63 | ++__attribute__((weak)) void handle_m_trap_h3(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc){ |
| 64 | ++ handle_m_trap(regs, dummy, mepc); |
| 65 | ++} |
| 66 | ++__attribute__((weak)) void handle_m_trap_h4(uintptr_t * regs, uintptr_t dummy, uintptr_t mepc){ |
| 67 | ++ handle_m_trap(regs, dummy, mepc); |
| 68 | ++} |
| 69 | ++ |
| 70 | ++void handle_m_trap(uintptr_t * regs, uintptr_t mcause, uintptr_t mepc) /* Hex Five mod */ |
| 71 | ++{ |
| 72 | ++ uint32_t i = 0U; |
| 73 | ++ while(1) |
| 74 | ++ { |
| 75 | ++ /* wait for watchdog */ |
| 76 | ++ i++; /* added some code as SC debugger hangs if in loop doing nothing */ |
| 77 | ++ if(i == 0x1000U) |
| 78 | + { |
| 79 | +- /* wait for watchdog */ |
| 80 | +- i++; /* added some code as SC debugger hangs if in loop doing nothing */ |
| 81 | +- if(i == 0x1000U) |
| 82 | +- { |
| 83 | + i = mcause; /* so mcause is not optimised out */ |
| 84 | +- } |
| 85 | + } |
| 86 | +- switch(mcause) |
| 87 | +- { |
| 88 | ++ } |
| 89 | ++ switch(mcause) |
| 90 | ++ { |
| 91 | + |
| 92 | +- case CAUSE_LOAD_PAGE_FAULT: |
| 93 | +- break; |
| 94 | +- case CAUSE_STORE_PAGE_FAULT: |
| 95 | +- break; |
| 96 | +- case CAUSE_FETCH_ACCESS: |
| 97 | +- break; |
| 98 | +- case CAUSE_LOAD_ACCESS: |
| 99 | +- break; |
| 100 | +- case CAUSE_STORE_ACCESS: |
| 101 | +- break; |
| 102 | +- default: |
| 103 | +- bad_trap(regs, dummy, mepc); |
| 104 | +- break; |
| 105 | +- } |
| 106 | ++ case CAUSE_LOAD_PAGE_FAULT: |
| 107 | ++ break; |
| 108 | ++ case CAUSE_STORE_PAGE_FAULT: |
| 109 | ++ break; |
| 110 | ++ case CAUSE_FETCH_ACCESS: |
| 111 | ++ break; |
| 112 | ++ case CAUSE_LOAD_ACCESS: |
| 113 | ++ break; |
| 114 | ++ case CAUSE_STORE_ACCESS: |
| 115 | ++ break; |
| 116 | ++ default: |
| 117 | ++ bad_trap(regs, mcause, mepc); |
| 118 | ++ break; |
| 119 | + } |
| 120 | + } |
| 121 | + |
| 122 | +diff --git a/mpfs_hal/startup_gcc/mss_entry.S b/mpfs_hal/startup_gcc/mss_entry.S |
| 123 | +index 5ff6d95..f2b2ea3 100644 |
| 124 | +--- a/mpfs_hal/startup_gcc/mss_entry.S |
| 125 | ++++ b/mpfs_hal/startup_gcc/mss_entry.S |
| 126 | +@@ -5,6 +5,7 @@ |
| 127 | + * |
| 128 | + * MPFS HAL Embedded Software |
| 129 | + * |
| 130 | ++ * Modifications copyright (C) 2021 Hex Five Security, Inc. |
| 131 | + */ |
| 132 | + |
| 133 | + /******************************************************************************* |
| 134 | +@@ -52,7 +53,7 @@ _start: |
| 135 | + csrw medeleg, 0 |
| 136 | + .skip_e51: |
| 137 | + # mscratch must be init to zero- we are not using scratch memory |
| 138 | +- csrw mscratch, zero |
| 139 | ++ #csrw mscratch, zero /* Hex Five mod*/ |
| 140 | + csrw mcause, zero |
| 141 | + csrw mepc, zero |
| 142 | + /* |
0 commit comments