Skip to content

Commit f541a44

Browse files
jservjserv
committed
Fix Cortex-R5 preemption path register corruption
The preemption path in __tx_preempt_load_context corrupted the thread's r1, r2, and r3 during minimal-to-full frame conversion. The old code read r0-r3 from the minimal frame into physical registers, then overwrote r1 with PC, r2 with SPSR, and r3 with a mode constant before pushing all four to the full frame. This replaces broken STMDB-based approach with an indexed LDR/STR strategy that avoids register conflicts: - Save the thread's banked LR into r0 before mode switch (lr is not in the minimal frame and must be captured while still in the thread's mode) - Use a frame base pointer (r3) for deferred reads from the minimal frame - Compute SP as SUB sp, r3, eclipse-threadx#28 instead of SUB sp, sp, eclipse-threadx#60 to be mode-independent (sp is banked between SYS and SVC) - Use CPS for register-free mode switch to SVC - Fill r0-r3 from the minimal frame BEFORE callee-saved writes, since for SVC-mode threads the full frame overlaps the minimal frame on the same stack (r7-r10 slots alias r0-r3 slots)
1 parent 72e590c commit f541a44

1 file changed

Lines changed: 68 additions & 13 deletions

File tree

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

Lines changed: 68 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -222,23 +222,78 @@ __tx_preempt_svc_mode:
222222
@
223223
__tx_preempt_load_context:
224224
LDR sp, [r0, #8] @ Restore thread stack pointer
225+
MOV r0, lr @ Save thread's LR (SYS or SVC) in r0
226+
@ (r0 is scratch here, holding thread ptr
227+
@ which we reload later)
228+
229+
@
230+
@ /* Read metadata from the minimal interrupt frame using indexed loads.
231+
@ r1-r3 are scratch at this point.
232+
@ Minimal frame layout: [r0][r1][r2][r3][r10][r12][PC][SPSR]
233+
@ +0 +4 +8 +12 +16 +20 +24 +28 */
225234
@
226-
@ /* Read the interrupt frame without destroying it yet. */
235+
LDR r1, [sp, #24] @ r1 = PC (point of interrupt)
236+
LDR r2, [sp, #28] @ r2 = SPSR
237+
MOV r3, sp @ r3 = minimal frame base address
238+
LDR r10, [sp, #16] @ r10 = thread's r10 (from frame)
239+
LDR r12, [sp, #20] @ r12 = thread's r12 (from frame)
227240
@
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
241+
@ /* Advance thread's SP past the consumed minimal frame. */
242+
ADD sp, sp, #32 @ Thread's SP restored to pre-interrupt value
233243
@
234-
@ /* Now build full thread context in SVC mode. */
244+
@ /* Switch to SVC mode to build the full interrupt frame.
245+
@ Use CPS to avoid consuming a register for the mode constant.
246+
@ Interrupts remain disabled (I/F bits unchanged by CPS). */
247+
#ifdef TX_ENABLE_FIQ_SUPPORT
248+
CPSID if, #0x13 @ Disable IRQ+FIQ, enter SVC mode
249+
#else
250+
CPSID i, #0x13 @ Disable IRQ, enter SVC mode
251+
#endif
235252
@
236-
MOV r3, #SVC_MODE @ Build SVC mode CPSR
237-
MSR CPSR_c, r3 @ Enter SVC mode
238-
STR r1, [sp, #-4]! @ Save point of interrupt
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
253+
@ /* Build the full interrupt frame on the SVC stack.
254+
@ The frame must be: [r0-r12, lr, pc] = 15 registers = 60 bytes.
255+
@ For SVC-mode threads, this frame overlaps with the minimal frame in
256+
@ memory (same stack), so r0-r3 must be read from the frame BEFORE
257+
@ callee-saved writes overwrite those addresses.
258+
@
259+
@ Calculated SP:
260+
@ Old SP (r3) points to base of minimal frame.
261+
@ New SP should be (Old SP + 32) - 60 = Old SP - 28.
262+
@ We use r3 to calculate this to ensure correctness regardless of
263+
@ whether we came from SYS or SVC mode (sp is banked). */
264+
@
265+
SUB sp, r3, #28 @ Allocate full frame (15 regs)
266+
@
267+
@ /* Save the thread's LR (saved in r0) to the stack.
268+
@ We do this early to free up r0 for use as scratch in the copy loop.
269+
@ Offset 52 corresponds to LR slot. */
270+
STR r0, [sp, #52] @ Save LR
271+
@
272+
@ /* Fill r0-r3 from minimal frame FIRST (before overlap can occur). */
273+
LDR r0, [r3, #0] @ Thread's r0
274+
STR r0, [sp, #0] @ -> frame slot r0
275+
LDR r0, [r3, #4] @ Thread's r1
276+
STR r0, [sp, #4] @ -> frame slot r1
277+
LDR r0, [r3, #8] @ Thread's r2
278+
STR r0, [sp, #8] @ -> frame slot r2
279+
LDR r0, [r3, #12] @ Thread's r3
280+
STR r0, [sp, #12] @ -> frame slot r3
281+
@
282+
@ /* Fill callee-saved registers from physical registers. */
283+
STR r4, [sp, #16] @ r4 (callee-saved, in physical reg)
284+
STR r5, [sp, #20] @ r5
285+
STR r6, [sp, #24] @ r6
286+
STR r7, [sp, #28] @ r7
287+
STR r8, [sp, #32] @ r8
288+
STR r9, [sp, #36] @ r9
289+
STR r10, [sp, #40] @ r10 (loaded from frame)
290+
STR r11, [sp, #44] @ r11 (callee-saved, in physical reg)
291+
STR r12, [sp, #48] @ r12 (loaded from frame)
292+
@ STR lr, [sp, #52] @ (LR already saved above)
293+
STR r1, [sp, #56] @ pc (point of interrupt)
294+
@
295+
@ /* Prepare SPSR in r4 for the type/SPSR header push below. */
296+
MOV r4, r2 @ r4 = SPSR
242297
@
243298
LDR r1, =_tx_thread_current_ptr @ Pickup address of current thread ptr
244299
LDR r0, [r1] @ Pickup current thread pointer

0 commit comments

Comments
 (0)