Skip to content

Commit b0c54c6

Browse files
jservjserv
committed
Fix Cortex-R5 IRQ/FIQ context switching and SP banking
This fixes critical bugs in context save/restore for ARM Cortex-R5: - Mode-aware stack pointer banking (sp_svc vs sp_sys) - Traditional exception return (MOVS pc,lr) for QEMU compatibility - Correct ARM register ordering in LDMIA operations - Stack frame alignment and leak prevention - Register preservation across mode switches Resolves system reset on custom interrupts.
1 parent 22399e6 commit b0c54c6

File tree

4 files changed

+207
-72
lines changed

4 files changed

+207
-72
lines changed

‎ports/cortex_r5/gnu/src/tx_thread_context_restore.S‎

Lines changed: 72 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,11 @@
2626

2727
#ifdef TX_ENABLE_FIQ_SUPPORT
2828
SVC_MODE = 0xD3 @ Disable IRQ/FIQ, SVC mode
29+
SYS_MODE = 0xDF @ Disable IRQ/FIQ, SYS mode
2930
IRQ_MODE = 0xD2 @ Disable IRQ/FIQ, IRQ mode
3031
#else
3132
SVC_MODE = 0x93 @ Disable IRQ, SVC mode
33+
SYS_MODE = 0x9F @ Disable IRQ, SYS mode
3234
IRQ_MODE = 0x92 @ Disable IRQ, IRQ mode
3335
#endif
3436
@
@@ -156,37 +158,87 @@ __tx_thread_no_preempt_restore:
156158
@
157159
@ /* Restore interrupted thread or ISR. */
158160
@
159-
@ /* Pickup the saved stack pointer. */
161+
@ /* Switch to SYS mode and pickup the saved stack pointer. */
160162
@ tmp_ptr = _tx_thread_current_ptr -> tx_thread_stack_ptr;
161163
@
162-
LDR sp, [r0, #8] @ Restore thread stack pointer
164+
@ /* Load PC and CPSR from thread stack while in IRQ mode */
165+
LDR r1, [r0, #8] @ r1 = thread stack pointer
166+
ADD r2, r1, #24 @ r2 = address of saved PC (skip 6 regs)
167+
LDMIA r2, {r3, r12} @ r3 = PC, r12 = CPSR
163168
@
164-
@ /* Recover the saved context and return to the point of interrupt. */
169+
@ /* Load CPSR into SPSR_irq and PC into lr */
170+
MSR SPSR_cxsf, r12 @ Load CPSR into SPSR_irq
171+
MOV lr, r3 @ Load return address
165172
@
166-
LDMIA sp!, {r0, r10, r12, lr} @ Recover SPSR, POI, and scratch regs
167-
MSR SPSR_cxsf, r0 @ Put SPSR back
168-
LDMIA sp!, {r0-r3} @ Recover r0-r3
169-
MOVS pc, lr @ Return to point of interrupt
173+
@ /* Switch to thread's actual mode (not blindly SYS) */
174+
MOV r3, r12 @ Copy CPSR to r3
175+
AND r3, r3, #0x1F @ Isolate mode bits
176+
CMP r3, #0x13 @ Is thread in SVC mode?
177+
BEQ __tx_restore_svc_mode @ Yes, use SVC mode
178+
@
179+
@ /* Thread in SYS/USR mode (0x1F or 0x10) */
180+
CPS #0x1F @ Switch to SYS mode
181+
MOV sp, r1 @ Set SP_sys to thread stack
182+
B __tx_restore_continue
183+
@
184+
__tx_restore_svc_mode:
185+
@ /* Thread in SVC mode (0x13) */
186+
CPS #0x13 @ Switch to SVC mode
187+
MOV sp, r1 @ Set SP_svc to thread stack
188+
@
189+
__tx_restore_continue:
190+
LDMIA sp!, {r0-r3, r10, r12} @ Restore r0-r3, r10, r12
191+
ADD sp, sp, #8 @ Skip saved PC and CPSR (8 bytes)
192+
@
193+
@ /* Switch back to IRQ mode for proper exception return */
194+
CPS #0x12 @ Switch to IRQ mode
195+
@
196+
@ /* Return from exception (SPSR_irq->CPSR, lr->PC) */
197+
MOVS pc, lr @ Return from exception
170198
@
171199
@ }
172200
@ else
173201
@ {
174202
__tx_thread_preempt_restore:
175203
@
176-
LDR sp, [r0, #8] @ Restore thread stack pointer
177-
LDMIA sp!, {r3, r10, r12, lr} @ Recover temporarily saved registers
178-
MOV r1, lr @ Save lr (point of interrupt)
179-
MOV r2, #SVC_MODE @ Build SVC mode CPSR
180-
MSR CPSR_c, r2 @ Enter SVC mode
204+
@ /* Restore context and convert to full thread frame for scheduler. */
205+
@ /* Context on thread stack: r0-r3, r10, r12, LR, SPSR (RFE-compatible). */
206+
@
207+
@ /* Determine thread's mode from saved CPSR */
208+
LDR r3, [r0, #8] @ r3 = thread stack pointer
209+
LDR r12, [r3, #28] @ r12 = saved CPSR
210+
AND r12, r12, #0x1F @ r12 = mode bits
211+
CMP r12, #0x13 @ SVC mode?
212+
BEQ __tx_preempt_svc_mode
213+
@
214+
@ /* Thread was in SYS mode */
215+
MOV r12, #SYS_MODE @ SYS mode + interrupts disabled
216+
MSR CPSR_c, r12 @ Switch to SYS mode
217+
B __tx_preempt_load_context
218+
@
219+
__tx_preempt_svc_mode:
220+
MOV r12, #SVC_MODE @ SVC mode + interrupts disabled
221+
MSR CPSR_c, r12 @ Switch to SVC mode
222+
@
223+
__tx_preempt_load_context:
224+
LDR sp, [r0, #8] @ Restore thread stack pointer
225+
@
226+
@ /* Read the interrupt frame without destroying it yet. */
227+
@
228+
LDM sp, {r0-r3} @ Read r0-r3 (without popping)
229+
ADD sp, sp, #16 @ Move past r0-r3 (4 regs * 4 bytes)
230+
LDMIA sp!, {r10, r12} @ Load r10, r12 (ascending order: 10 < 12)
231+
LDMIA sp!, {r1} @ Load LR->r1
232+
LDR r2, [sp], #4 @ Load SPSR->r2 and advance SP
233+
@
234+
@ /* Now build full thread context in SVC mode. */
235+
@
236+
MOV r3, #SVC_MODE @ Build SVC mode CPSR
237+
MSR CPSR_c, r3 @ Enter SVC mode
181238
STR r1, [sp, #-4]! @ Save point of interrupt
182-
STMDB sp!, {r4-r12, lr} @ Save upper half of registers
183-
MOV r4, r3 @ Save SPSR in r4
184-
MOV r2, #IRQ_MODE @ Build IRQ mode CPSR
185-
MSR CPSR_c, r2 @ Enter IRQ mode
186-
LDMIA sp!, {r0-r3} @ Recover r0-r3
187-
MOV r5, #SVC_MODE @ Build SVC mode CPSR
188-
MSR CPSR_c, r5 @ Enter SVC mode
189-
STMDB sp!, {r0-r3} @ Save r0-r3 on thread's stack
239+
STMDB sp!, {r4-r12, lr} @ Save upper half of registers (thread's r4-r12!)
240+
MOV r4, r2 @ Move SPSR to r4 (thread's r4 already saved!)
241+
STMDB sp!, {r0-r3} @ Save r0-r3 on SVC stack
190242
@
191243
LDR r1, =_tx_thread_current_ptr @ Pickup address of current thread ptr
192244
LDR r0, [r1] @ Pickup current thread pointer

‎ports/cortex_r5/gnu/src/tx_thread_context_save.S‎

Lines changed: 61 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,16 @@
2222
#include "tx_user.h"
2323
#endif
2424

25+
#ifdef TX_ENABLE_FIQ_SUPPORT
26+
SVC_MODE = 0xD3 @ Disable IRQ/FIQ, SVC mode
27+
SYS_MODE = 0xDF @ Disable IRQ/FIQ, SYS mode
28+
IRQ_MODE = 0xD2 @ Disable IRQ/FIQ, IRQ mode
29+
#else
30+
SVC_MODE = 0x93 @ Disable IRQ, SVC mode
31+
SYS_MODE = 0x9F @ Disable IRQ, SYS mode
32+
IRQ_MODE = 0x92 @ Disable IRQ, IRQ mode
33+
#endif
34+
@
2535
.global _tx_thread_system_state
2636
.global _tx_thread_current_ptr
2737
.global _tx_irq_processing_return
@@ -120,7 +130,7 @@ _tx_thread_context_save:
120130
POP {lr} @ Recover ISR lr
121131
#endif
122132

123-
B __tx_irq_processing_return @ Continue IRQ processing
133+
B __tx_irq_processing_return @ Continue IRQ processing
124134
@
125135
__tx_thread_not_nested_save:
126136
@ }
@@ -134,26 +144,61 @@ __tx_thread_not_nested_save:
134144
LDR r1, =_tx_thread_current_ptr @ Pickup address of current thread ptr
135145
LDR r0, [r1] @ Pickup current thread pointer
136146
CMP r0, #0 @ Is it NULL?
137-
BEQ __tx_thread_idle_system_save @ If so, interrupt occurred in
147+
BEQ __tx_thread_idle_system_save @ If so, interrupt occurred in
138148
@ scheduling loop - nothing needs saving!
139149
@
140-
@ /* Save minimal context of interrupted thread. */
150+
@ /* Save minimal context of interrupted thread on thread's stack. */
151+
@ /* Stack frame: r0-r3, r10, r12, PC, CPSR (32 bytes) */
141152
@
142-
MRS r2, SPSR @ Pickup saved SPSR
143-
SUB lr, lr, #4 @ Adjust point of interrupt
144-
STMDB sp!, {r2, r10, r12, lr} @ Store other registers
153+
MOV r2, sp @ r2 = IRQ stack pointer (has saved r0-r3)
145154
@
146-
@ /* Save the current stack pointer in the thread's control block. */
147-
@ _tx_thread_current_ptr -> tx_thread_stack_ptr = sp;
155+
@ /* Pickup SPSR and adjust return address BEFORE switching modes */
156+
MRS r3, SPSR @ r3 = saved SPSR (thread's CPSR)
157+
SUB lr, lr, #4 @ Adjust point of interrupt (lr_irq)
158+
MOV r1, lr @ Save lr_irq in r1 (before mode switch!)
159+
@
160+
@ /* Switch to the thread's actual mode (from SPSR) to access its stack */
161+
@ /* r3 contains SPSR_irq = thread's saved CPSR */
162+
AND r4, r3, #0x1F @ r4 = thread's mode bits
163+
CMP r4, #0x13 @ Was thread in SVC mode?
164+
BEQ __tx_save_svc_mode
165+
@
166+
@ /* Thread was in SYS/USR mode - switch to SYS mode */
167+
@ /* Note: USR and SYS share the same sp/lr register bank */
168+
MOV r4, #SYS_MODE @ SYS mode + interrupts disabled
169+
MSR CPSR_c, r4 @ Switch to SYS mode
170+
B __tx_save_context_on_stack
171+
@
172+
__tx_save_svc_mode:
173+
@ /* Thread was in SVC mode - switch to SVC mode */
174+
MOV r4, #SVC_MODE @ SVC mode + interrupts disabled
175+
MSR CPSR_c, r4 @ Switch to SVC mode
176+
@
177+
__tx_save_context_on_stack:
178+
@ /* Now sp = thread's actual stack pointer (sp_sys or sp_svc) */
179+
@
180+
@ /* Save SPSR and PC to stack */
181+
STR r3, [sp, #-4]! @ Push SPSR at [SP-4]
182+
STR r1, [sp, #-4]! @ Push lr_irq (as PC) at [SP-8]
148183
@
149-
LDR r1, =_tx_thread_system_stack_ptr @ Pickup system stack pointer address
150-
STR sp, [r0, #8] @ Save thread stack pointer
184+
@ /* Recover original r0-r3 from IRQ stack (r2 still points to IRQ stack) */
185+
LDMIA r2, {r0-r3} @ Recover thread's r0-r3 from IRQ stack
151186
@
152-
@ /* Switch to the system stack. */
153-
@ sp = _tx_thread_system_stack_ptr@
187+
@ /* Save remaining registers */
188+
STMDB sp!, {r0-r3, r10, r12} @ Save r0-r3, r10, r12 (final SP = base-32)
154189
@
155-
LDR sp, [r1] @ Switch to system stack
156-
MOV r10, #0 @ Clear stack limit
190+
@ /* Save the thread's stack pointer in the thread's control block. */
191+
@ _tx_thread_current_ptr -> tx_thread_stack_ptr = sp;
192+
@
193+
LDR r1, =_tx_thread_current_ptr @ Reload current thread pointer address
194+
LDR r0, [r1] @ Get current thread pointer
195+
STR sp, [r0, #8] @ Save updated thread stack pointer
196+
@
197+
@ /* Switch to IRQ mode to use dedicated IRQ stack for ISR execution. */
198+
MOV r4, #IRQ_MODE @ IRQ mode + interrupts disabled
199+
MSR CPSR_c, r4 @ Switch to IRQ mode
200+
ADD sp, sp, #16 @ Pop stale r0-r3 from IRQ stack (4 regs * 4 bytes)
201+
MOV r10, #0 @ Clear stack limit
157202

158203
#ifdef TX_ENABLE_EXECUTION_CHANGE_NOTIFY
159204
@
@@ -164,7 +209,7 @@ __tx_thread_not_nested_save:
164209
POP {lr} @ Recover ISR lr
165210
#endif
166211

167-
B __tx_irq_processing_return @ Continue IRQ processing
212+
B __tx_irq_processing_return @ Continue IRQ processing
168213
@
169214
@ }
170215
@ else
@@ -173,9 +218,6 @@ __tx_thread_not_nested_save:
173218
__tx_thread_idle_system_save:
174219
@
175220
@ /* Interrupt occurred in the scheduling loop. */
176-
@
177-
@ /* Not much to do here, just adjust the stack pointer, and return to IRQ
178-
@ processing. */
179221
@
180222
MOV r10, #0 @ Clear stack limit
181223

@@ -189,10 +231,7 @@ __tx_thread_idle_system_save:
189231
#endif
190232

191233
ADD sp, sp, #16 @ Recover saved registers
192-
B __tx_irq_processing_return @ Continue IRQ processing
234+
B __tx_irq_processing_return @ Continue IRQ processing
193235
@
194236
@ }
195237
@}
196-
197-
198-

‎ports/cortex_r5/gnu/src/tx_thread_fiq_context_restore.S‎

Lines changed: 38 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -158,36 +158,59 @@ __tx_thread_fiq_no_preempt_restore:
158158
@
159159
@ /* Restore interrupted thread or ISR. */
160160
@
161-
@ /* Pickup the saved stack pointer. */
161+
@ /* Use traditional exception return from FIQ mode */
162+
@ /* Stack frame: r0-r3, r10, r12, PC, CPSR */
162163
@ tmp_ptr = _tx_thread_current_ptr -> tx_thread_stack_ptr;
163164
@
164-
LDR sp, [r0, #8] @ Restore thread stack pointer
165+
@ /* Load PC and CPSR from thread stack while in FIQ mode */
166+
LDR r1, [r0, #8] @ r1 = thread stack pointer
167+
ADD r2, r1, #24 @ r2 = address of saved PC (skip 6 regs)
168+
LDMIA r2, {r3, r12} @ r3 = PC, r12 = CPSR
165169
@
166-
@ /* Recover the saved context and return to the point of interrupt. */
170+
@ /* Load CPSR into SPSR_fiq and PC into lr */
171+
MSR SPSR_cxsf, r12 @ Load CPSR into SPSR_fiq
172+
MOV lr, r3 @ Load return address
167173
@
168-
LDMIA sp!, {r0, lr} @ Recover SPSR, POI, and scratch regs
169-
MSR SPSR_cxsf, r0 @ Put SPSR back
170-
LDMIA sp!, {r0-r3} @ Recover r0-r3
171-
MOVS pc, lr @ Return to point of interrupt
174+
@ /* Switch to thread's actual mode (not blindly SYS) */
175+
MOV r3, r12 @ Copy CPSR to r3
176+
AND r3, r3, #0x1F @ Isolate mode bits
177+
CMP r3, #0x13 @ Is thread in SVC mode?
178+
BEQ __tx_fiq_restore_svc_mode @ Yes, use SVC mode
179+
@
180+
@ /* Thread in SYS/USR mode (0x1F or 0x10) */
181+
CPS #0x1F @ Switch to SYS mode
182+
MOV sp, r1 @ Set SP_sys to thread stack
183+
B __tx_fiq_restore_continue
184+
@
185+
__tx_fiq_restore_svc_mode:
186+
@ /* Thread in SVC mode (0x13) */
187+
CPS #0x13 @ Switch to SVC mode
188+
MOV sp, r1 @ Set SP_svc to thread stack
189+
@
190+
__tx_fiq_restore_continue:
191+
LDMIA sp!, {r0-r3, r10, r12} @ Restore r0-r3, r10, r12
192+
ADD sp, sp, #8 @ Skip saved PC and CPSR (8 bytes)
193+
@
194+
@ /* Switch back to FIQ mode for proper exception return */
195+
CPS #0x11 @ Switch to FIQ mode
196+
@
197+
@ /* Return from exception (SPSR_fiq->CPSR, lr->PC) */
198+
MOVS pc, lr @ Return from exception
172199
@
173200
@ }
174201
@ else
175202
@ {
176203
__tx_thread_fiq_preempt_restore:
177204
@
205+
CPS #0x1F @ Switch to SYS mode
178206
LDR sp, [r0, #8] @ Restore thread stack pointer
179-
LDMIA sp!, {r3, lr} @ Recover temporarily saved registers
180-
MOV r1, lr @ Save lr (point of interrupt)
207+
LDM sp, {r0-r3, r10, r12} @ Read registers (without popping)
208+
ADD sp, sp, #24 @ Move past saved registers
209+
LDMIA sp!, {r1, r4} @ r1 = LR, r4 = SPSR
181210
MOV r2, #SVC_MODE @ Build SVC mode CPSR
182211
MSR CPSR_c, r2 @ Enter SVC mode
183212
STR r1, [sp, #-4]! @ Save point of interrupt
184213
STMDB sp!, {r4-r12, lr} @ Save upper half of registers
185-
MOV r4, r3 @ Save SPSR in r4
186-
MOV r2, #FIQ_MODE @ Build FIQ mode CPSR
187-
MSR CPSR_c, r2 @ Reenter FIQ mode
188-
LDMIA sp!, {r0-r3} @ Recover r0-r3
189-
MOV r5, #SVC_MODE @ Build SVC mode CPSR
190-
MSR CPSR_c, r5 @ Enter SVC mode
191214
STMDB sp!, {r0-r3} @ Save r0-r3 on thread's stack
192215
MOV r3, #1 @ Build interrupt stack type
193216
STMDB sp!, {r3, r4} @ Save interrupt stack type and SPSR

‎ports/cortex_r5/gnu/src/tx_thread_fiq_context_save.S‎

Lines changed: 36 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -134,27 +134,48 @@ __tx_thread_fiq_not_nested_save:
134134
BEQ __tx_thread_fiq_idle_system_save @ If so, interrupt occurred in
135135
@ @ scheduling loop - nothing needs saving!
136136
@
137-
@ /* Save minimal context of interrupted thread. */
137+
@ /* Save minimal context of interrupted thread on thread's stack. */
138+
@ /* Must switch to SYS mode and set SP before saving context */
139+
@ /* Stack frame ordered for RFE compatibility: r0-r3, r10, r12, PC, CPSR */
140+
@ /* Note: FIQ mode has banked r8-r12, so we save thread's r10/r12 from SYS mode. */
138141
@
139-
MRS r2, SPSR @ Pickup saved SPSR
140-
SUB lr, lr, #4 @ Adjust point of interrupt
141-
STMDB sp!, {r2, lr} @ Store other registers, Note that we don't
142-
@ @ need to save sl and ip since FIQ has
143-
@ @ copies of these registers. Nested
144-
@ @ interrupt processing does need to save
145-
@ @ these registers.
142+
@ /* Get thread's stack pointer from TCB (r0 = current thread ptr) */
143+
MOV r2, sp @ r2 = FIQ stack pointer (has saved r0-r3)
144+
LDR r1, [r0, #8] @ r1 = thread stack pointer from TCB
146145
@
147-
@ /* Save the current stack pointer in the thread's control block. */
146+
@ /* Pickup SPSR and adjust return address BEFORE switching modes */
147+
MRS r3, SPSR @ r3 = saved SPSR
148+
SUB lr, lr, #4 @ Adjust point of interrupt (lr_fiq)
149+
MOV r1, lr @ Save lr_fiq in r1 (before mode switch!)
150+
@
151+
@ /* Switch to SYS mode and set SP to thread's stack pointer */
152+
CPS #0x1F @ Switch to SYS mode
153+
LDR sp, [r0, #8] @ Load thread SP directly into sp
154+
@
155+
@ /* Save SPSR and LR to final stack positions (r2 still points to FIQ stack) */
156+
STR r3, [sp, #-4]! @ Push SPSR at [SP-4]
157+
STR r1, [sp, #-4]! @ Push lr_fiq (as PC) at [SP-8]
158+
@
159+
@ /* Now recover original r0-r3 from exception stack (r2 reg not yet modified) */
160+
LDMIA r2, {r0-r3} @ Recover thread's r0-r3 from FIQ stack
161+
@
162+
@ /* Save remaining registers below LR */
163+
STMDB sp!, {r0-r3, r10, r12} @ Save r0-r3, r10, r12 (final SP = base-32)
164+
@
165+
@ /* Save the thread's stack pointer in the thread's control block. */
148166
@ _tx_thread_current_ptr -> tx_thread_stack_ptr = sp;
149167
@
150-
LDR r1, =_tx_thread_system_stack_ptr @ Pickup system stack pointer address
151-
STR sp, [r0, #8] @ Save thread stack pointer
168+
LDR r1, =_tx_thread_current_ptr @ Reload current thread pointer address
169+
LDR r0, [r1] @ Get current thread pointer
170+
STR sp, [r0, #8] @ Save updated thread stack pointer
152171
@
153-
@ /* Switch to the system stack. */
154-
@ sp = _tx_thread_system_stack_ptr;
172+
@ /* Switch to FIQ mode to use dedicated FIQ stack for ISR execution. */
173+
@ /* This prevents thread stack overflow during deep interrupt nesting. */
174+
@ /* Note: r0-r3 on FIQ stack are now stale but will be popped on return. */
155175
@
156-
LDR sp, [r1] @ Switch to system stack
157-
MOV r10, #0 @ Clear stack limit
176+
CPS #0x11 @ Switch to FIQ mode
177+
ADD sp, sp, #16 @ Pop stale r0-r3 from FIQ stack (4 regs * 4 bytes)
178+
MOV r10, #0 @ Clear stack limit (also saved to thread stack)
158179

159180
#ifdef TX_ENABLE_EXECUTION_CHANGE_NOTIFY
160181
@

0 commit comments

Comments
 (0)