|
26 | 26 | .equ SHPR3, 0xE000ED20 /* system priority register (3) */ |
27 | 27 | .equ PENDSV_PRI_LOWEST, 0xFFFF0000 /* PendSV and SysTick priority value (lowest) */ |
28 | 28 |
|
| 29 | + .equ EXC_RETURN, 0x7FFFFFF |
| 30 | + |
29 | 31 | /* |
30 | 32 | * void rt_hw_context_switch(rt_uint32 from, rt_uint32 to); |
31 | | - * R0 --> from |
32 | | - * R1 --> to |
| 33 | + * R0 --> from_thread->sp |
| 34 | + * R1 --> to_thread->sp |
33 | 35 | */ |
34 | 36 | .global rt_hw_context_switch_interrupt |
35 | 37 | .type rt_hw_context_switch_interrupt, %function |
|
57 | 59 | STR R1, [R0] |
58 | 60 | BX LR |
59 | 61 |
|
60 | | -/* R0 --> switch from thread stack |
61 | | - * R1 --> switch to thread stack |
| 62 | +/* R0 --> from_thread->sp |
| 63 | + * R1 --> to_thread->sp |
62 | 64 | * psr, pc, LR, R12, R3, R2, R1, R0 are pushed into [from] stack |
63 | 65 | */ |
64 | 66 | .global pendSVHook |
|
98 | 100 | /* restore interrupt */ |
99 | 101 | MSR PRIMASK, R2 |
100 | 102 |
|
101 | | - ORR LR, LR, #0x04 |
| 103 | + LDR R2, =EXC_RETURN |
| 104 | + CMP R2, LR, LSR #5 |
| 105 | + BNE check_stack |
| 106 | + ORR LR, LR, #4 |
| 107 | + BX LR |
| 108 | +check_stack: |
| 109 | + MOV R0, SP |
| 110 | + LDR R1, [R0] |
| 111 | + CMP R2, R1, LSR #5 |
| 112 | + BEQ fix_exc_return |
| 113 | + ADD R0, R0, #4 |
| 114 | + LDR R1, [R0] |
| 115 | + CMP R2, R1, LSR #5 |
| 116 | + BNE _pendsv_exit |
| 117 | +fix_exc_return: |
| 118 | + ORR R1, R1, #4 |
| 119 | + STR R1, [R0] |
| 120 | +_pendsv_exit: |
102 | 121 | BX LR |
103 | 122 |
|
104 | 123 | /* |
|
133 | 152 | STR R1, [R0] |
134 | 153 |
|
135 | 154 | /* restore MSP */ |
136 | | - LDR r0, =SCB_VTOR |
137 | | - LDR r0, [r0] |
138 | | - LDR r0, [r0] |
139 | | - NOP |
140 | | - MSR msp, r0 |
| 155 | + // LDR r0, =SCB_VTOR |
| 156 | + // LDR r0, [r0] |
| 157 | + // LDR r0, [r0] |
| 158 | + // NOP |
| 159 | + // MSR msp, r0 |
141 | 160 |
|
142 | 161 | /* enable interrupts at processor level */ |
143 | 162 | CPSIE F |
|
0 commit comments