From 735cd9a9853e2d7a566a7018cd4d8e47cc097c13 Mon Sep 17 00:00:00 2001 From: Andrew Ruder Date: Mon, 5 Oct 2015 13:51:10 -0500 Subject: [PATCH 1/3] rtos: turn stack alignment into a function pointer Some targets (Cortex M) require more complicated calculations for turning the stored stack pointer back into a process stack pointer. For example, the Cortex M stores a bit in the auto-stacked xPSR indicating that alignment had to be performed and an additional 4 byte padding is present before the exception stacking. This change only sets up the framework for Cortex-M unstacking and does not add Cortex-M support. Note: this also fixes the alignment calculation nearly addressed by change #2301 entitled rtos/rtos.c: fix stack alignment calculation. Updated calculation is in rtos_generic_stack_align. Signed-off-by: Andrew Ruder Cc: Paul Fertser Cc: Andreas Fritiofson Cc: Evan Hunter Cc: Jon Burgess --- src/rtos/ThreadX.c | 4 ++-- src/rtos/rtos.c | 13 +++++------ src/rtos/rtos.h | 10 ++++++++- src/rtos/rtos_chibios_stackings.c | 4 ++-- src/rtos/rtos_ecos_stackings.c | 3 ++- src/rtos/rtos_embkernel_stackings.c | 3 ++- src/rtos/rtos_mqx_stackings.c | 2 +- src/rtos/rtos_standard_stackings.c | 34 ++++++++++++++++++++++++++--- src/rtos/rtos_standard_stackings.h | 3 +++ 9 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/rtos/ThreadX.c b/src/rtos/ThreadX.c index c0331254..16df7a4b 100644 --- a/src/rtos/ThreadX.c +++ b/src/rtos/ThreadX.c @@ -117,14 +117,14 @@ const struct rtos_register_stacking rtos_threadx_arm926ejs_stacking[] = { ARM926EJS_REGISTERS_SIZE_SOLICITED, /* stack_registers_size */ -1, /* stack_growth_direction */ 17, /* num_output_registers */ - 0, /* stack_alignment */ + NULL, /* stack_alignment */ rtos_threadx_arm926ejs_stack_offsets_solicited /* register_offsets */ }, { ARM926EJS_REGISTERS_SIZE_INTERRUPT, /* stack_registers_size */ -1, /* stack_growth_direction */ 17, /* num_output_registers */ - 0, /* stack_alignment */ + NULL, /* stack_alignment */ rtos_threadx_arm926ejs_stack_offsets_interrupt /* register_offsets */ }, }; diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c index af95da30..50770d47 100644 --- a/src/rtos/rtos.c +++ b/src/rtos/rtos.c @@ -487,13 +487,12 @@ int rtos_generic_stack_read(struct target *target, list_size += stacking->register_offsets[i].width_bits/8; *hex_reg_list = malloc(list_size*2 + 1); tmp_str_ptr = *hex_reg_list; - new_stack_ptr = stack_ptr - stacking->stack_growth_direction * - stacking->stack_registers_size; - if (stacking->stack_alignment != 0) { - /* Align new stack pointer to x byte boundary */ - new_stack_ptr = - (new_stack_ptr & (~((int64_t) stacking->stack_alignment - 1))) + - ((stacking->stack_growth_direction == -1) ? stacking->stack_alignment : 0); + if (stacking->calculate_process_stack != NULL) { + new_stack_ptr = stacking->calculate_process_stack(target, + stack_data, stacking, stack_ptr); + } else { + new_stack_ptr = stack_ptr - stacking->stack_growth_direction * + stacking->stack_registers_size; } for (i = 0; i < stacking->num_output_registers; i++) { int j; diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h index 7750f3c7..d082fb7a 100644 --- a/src/rtos/rtos.h +++ b/src/rtos/rtos.h @@ -83,7 +83,15 @@ struct rtos_register_stacking { unsigned char stack_registers_size; signed char stack_growth_direction; unsigned char num_output_registers; - unsigned char stack_alignment; + /* Some targets require evaluating the stack to determine the + * actual stack pointer for a process. If this field is NULL, + * just use stacking->stack_registers_size * stack_growth_direction + * to calculate adjustment. + */ + int64_t (*calculate_process_stack)(struct target *target, + const uint8_t *stack_data, + const struct rtos_register_stacking *stacking, + int64_t stack_ptr); const struct stack_register_offset *register_offsets; }; diff --git a/src/rtos/rtos_chibios_stackings.c b/src/rtos/rtos_chibios_stackings.c index ed485616..e138b068 100644 --- a/src/rtos/rtos_chibios_stackings.c +++ b/src/rtos/rtos_chibios_stackings.c @@ -52,7 +52,7 @@ const struct rtos_register_stacking rtos_chibios_arm_v7m_stacking = { 0x24, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 0, /* stack_alignment */ + NULL, /* stack_alignment */ rtos_chibios_arm_v7m_stack_offsets /* register_offsets */ }; @@ -80,6 +80,6 @@ const struct rtos_register_stacking rtos_chibios_arm_v7m_stacking_w_fpu = { 0x64, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 0, /* stack_alignment */ + NULL, /* stack_alignment */ rtos_chibios_arm_v7m_stack_offsets_w_fpu /* register_offsets */ }; diff --git a/src/rtos/rtos_ecos_stackings.c b/src/rtos/rtos_ecos_stackings.c index 53ba171f..cc924ae2 100644 --- a/src/rtos/rtos_ecos_stackings.c +++ b/src/rtos/rtos_ecos_stackings.c @@ -21,6 +21,7 @@ #endif #include "rtos.h" +#include "rtos_standard_stackings.h" #include "target/armv7m.h" static const struct stack_register_offset rtos_eCos_Cortex_M3_stack_offsets[ARMV7M_NUM_CORE_REGS] = { @@ -47,6 +48,6 @@ const struct rtos_register_stacking rtos_eCos_Cortex_M3_stacking = { 0x44, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 8, /* stack_alignment */ + rtos_generic_stack_align8, /* stack_alignment */ rtos_eCos_Cortex_M3_stack_offsets /* register_offsets */ }; diff --git a/src/rtos/rtos_embkernel_stackings.c b/src/rtos/rtos_embkernel_stackings.c index 315da301..8171957f 100644 --- a/src/rtos/rtos_embkernel_stackings.c +++ b/src/rtos/rtos_embkernel_stackings.c @@ -24,6 +24,7 @@ #include "rtos.h" #include "target/armv7m.h" +#include "rtos_standard_stackings.h" static const struct stack_register_offset rtos_embkernel_Cortex_M_stack_offsets[ARMV7M_NUM_CORE_REGS] = { { 0x24, 32 }, /* r0 */ @@ -49,7 +50,7 @@ const struct rtos_register_stacking rtos_embkernel_Cortex_M_stacking = { 0x40, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 8, /* stack_alignment */ + rtos_generic_stack_align8, /* stack_alignment */ rtos_embkernel_Cortex_M_stack_offsets /* register_offsets */ }; diff --git a/src/rtos/rtos_mqx_stackings.c b/src/rtos/rtos_mqx_stackings.c index 8f2d67ea..fac18b60 100644 --- a/src/rtos/rtos_mqx_stackings.c +++ b/src/rtos/rtos_mqx_stackings.c @@ -75,7 +75,7 @@ const struct rtos_register_stacking rtos_mqx_arm_v7m_stacking = { 0x4C, /* stack_registers_size, calculate offset base address */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 0, /* stack_alignment */ + NULL, /* stack_alignment */ rtos_mqx_arm_v7m_stack_offsets /* register_offsets */ }; diff --git a/src/rtos/rtos_standard_stackings.c b/src/rtos/rtos_standard_stackings.c index 2eab86f9..2543ec5d 100644 --- a/src/rtos/rtos_standard_stackings.c +++ b/src/rtos/rtos_standard_stackings.c @@ -113,11 +113,39 @@ static const struct stack_register_offset rtos_standard_NDS32_N1068_stack_offset { 0x10, 32 }, /* IFC_LP */ }; +static int64_t rtos_generic_stack_align(struct target *target, + const uint8_t *stack_data, const struct rtos_register_stacking *stacking, + int64_t stack_ptr, int align) +{ + int64_t new_stack_ptr; + int64_t aligned_stack_ptr; + new_stack_ptr = stack_ptr - stacking->stack_growth_direction * + stacking->stack_registers_size; + aligned_stack_ptr = new_stack_ptr & ((int64_t)align - 1); + if (aligned_stack_ptr != new_stack_ptr && + stacking->stack_growth_direction == -1) { + /* If we have a downward growing stack, the simple alignment code + * above results in a wrong result (since it rounds down to nearest + * alignment). We want to round up so add an extra align. + */ + aligned_stack_ptr += (int64_t)align; + } + return aligned_stack_ptr; +} + +int64_t rtos_generic_stack_align8(struct target *target, + const uint8_t *stack_data, const struct rtos_register_stacking *stacking, + int64_t stack_ptr) +{ + return rtos_generic_stack_align(target, stack_data, + stacking, stack_ptr, 8); +} + const struct rtos_register_stacking rtos_standard_Cortex_M3_stacking = { 0x40, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 8, /* stack_alignment */ + rtos_generic_stack_align8, /* stack_alignment */ rtos_standard_Cortex_M3_stack_offsets /* register_offsets */ }; @@ -125,7 +153,7 @@ const struct rtos_register_stacking rtos_standard_Cortex_R4_stacking = { 0x48, /* stack_registers_size */ -1, /* stack_growth_direction */ 26, /* num_output_registers */ - 8, /* stack_alignment */ + rtos_generic_stack_align8, /* stack_alignment */ rtos_standard_Cortex_R4_stack_offsets /* register_offsets */ }; @@ -133,6 +161,6 @@ const struct rtos_register_stacking rtos_standard_NDS32_N1068_stacking = { 0x90, /* stack_registers_size */ -1, /* stack_growth_direction */ 32, /* num_output_registers */ - 8, /* stack_alignment */ + rtos_generic_stack_align8, /* stack_alignment */ rtos_standard_NDS32_N1068_stack_offsets /* register_offsets */ }; diff --git a/src/rtos/rtos_standard_stackings.h b/src/rtos/rtos_standard_stackings.h index b76e2bb6..c64a4be0 100644 --- a/src/rtos/rtos_standard_stackings.h +++ b/src/rtos/rtos_standard_stackings.h @@ -30,5 +30,8 @@ extern const struct rtos_register_stacking rtos_standard_Cortex_M3_stacking; extern const struct rtos_register_stacking rtos_standard_Cortex_R4_stacking; extern const struct rtos_register_stacking rtos_standard_NDS32_N1068_stacking; +int64_t rtos_generic_stack_align8(struct target *target, + const uint8_t *stack_data, const struct rtos_register_stacking *stacking, + int64_t stack_ptr); #endif /* ifndef INCLUDED_RTOS_STANDARD_STACKINGS_H_ */ From cc4d19cb8a9decbfb90bdbb03a1360bc4a7c5052 Mon Sep 17 00:00:00 2001 From: Andrew Ruder Date: Mon, 5 Oct 2015 13:52:43 -0500 Subject: [PATCH 2/3] rtos: handle STKALIGN adjustments on cortex m In the case that the STKALIGN bit is set on Cortex M processors, on entry to an exception - the processor can store an additional 4 bytes of padding before regular stacking to achieve 8-byte alignment on exception entry. In the case that this padding is present, the processor will set bit (1 << 9) in the stacked xPSR register. Use the new calculate_process_stack callback to take into account the xPSR register and use it on the standard Cortex_M3 stacking. Note: Change #2301 had some misinformation regarding the padding. On Cortex-M the padding is stored BEFORE stacking so xPSR is always available at a fixed offset. Tested on a Cortex-M0+ (Atmel SAMR21) board which has STKALIGN fixed to a '1' such that this alignment always occurs on non-aligned stacks. Behavior of xPSR verified via the (bad-sorry) assembly program below by setting a breakpoint on the SVC_Handler symbol. The first time SVC_Handler is triggered the stack was 0x20000ff8, the second time SVC_Handler is triggered the stack was 0x20000ffc. Note that in both cases the interrupt handler gets 0x20000fd8 for a stack pointer. GDB exerpt: Breakpoint 1, 0x000040b6 in Reset_Handler () (gdb) hbreak SVC_Handler Hardware assisted breakpoint 2 at 0x40f8 (gdb) cont Continuing. Breakpoint 2, 0x000040f8 in SVC_Handler () (gdb) print $msp $3 = (void *) 0x20000fd8 (gdb) x/9w $msp 0x20000fd8: 0x1 0x2 0x3 0x4 0x20000fe8: 0x88160082 0xa53 0x40ce 0x21000000 0x20000ff8: 0x0 (gdb) cont Continuing. Breakpoint 2, 0x000040f8 in SVC_Handler () (gdb) print $msp $4 = (void *) 0x20000fd8 (gdb) x/9w $msp 0x20000fd8: 0x1 0x2 0x3 0x4 0x20000fe8: 0x88160082 0xa53 0x40e8 0x21000200 0x20000ff8: 0x0 Assembly program: .cpu cortex-m0plus .fpu softvfp .thumb .syntax unified .section .vectors @ pvStack: .word 0x20001000 @ pfnReset_Handler: .word Reset_Handler + 1 @ pfnNMI_Handler: .word 0 @ pfnHardFault_Handler: .word 0 @ pfnReservedM12: .word 0 @ pfnReservedM11: .word 0 @ pfnReservedM10: .word 0 @ pfnReservedM9: .word 0 @ pfnReservedM8: .word 0 @ pfnReservedM7: .word 0 @ pfnReservedM6: .word 0 @ pfnSVC_Handler: .word SVC_Handler + 1 .section .text .global Reset_Handler Reset_Handler: cpsie i ldr r0, .stack_start ldr r2, .stack_last eors r1, r1 .loop_clear: str r1, [r0] adds r0, r0, #4 cmp r0, r2 bne .loop_clear subs r2, r2, #4 mov sp, r2 movs r0, #1 movs r1, #2 movs r2, #3 movs r3, #4 svc #0 ldr r0, .stack_start ldr r2, .stack_last eors r1, r1 .loop_clear2: str r1, [r0] adds r0, r0, #4 cmp r0, r2 bne .loop_clear2 mov sp, r2 movs r0, #1 movs r1, #2 movs r2, #3 movs r3, #4 svc #0 .loop: b .loop .align 4 .stack_start: .word 0x20000f00 .stack_last: .word 0x20000ffc @ first call - 0x2000fff8 -- should already be aligned @ second call - 0x2000fffc -- should hit the alignment code .global SVC_Handler SVC_Handler: bx lr Signed-off-by: Andrew Ruder Cc: Paul Fertser Cc: Andreas Fritiofson Cc: Evan Hunter Cc: Jon Burgess --- src/rtos/rtos_standard_stackings.c | 51 +++++++++++++++++++++++++++++- src/rtos/rtos_standard_stackings.h | 3 ++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/src/rtos/rtos_standard_stackings.c b/src/rtos/rtos_standard_stackings.c index 2543ec5d..e166b553 100644 --- a/src/rtos/rtos_standard_stackings.c +++ b/src/rtos/rtos_standard_stackings.c @@ -141,11 +141,60 @@ int64_t rtos_generic_stack_align8(struct target *target, stacking, stack_ptr, 8); } +/* The Cortex M3 will indicate that an alignment adjustment + * has been done on the stack by setting bit 9 of the stacked xPSR + * register. In this case, we can just add an extra 4 bytes to get + * to the program stack. Note that some places in the ARM documentation + * make this a little unclear but the padding takes place before the + * normal exception stacking - so xPSR is always available at a fixed + * location. + * + * Relevant documentation: + * Cortex-M series processors -> Cortex-M3 -> Revision: xxx -> + * Cortex-M3 Devices Generic User Guide -> The Cortex-M3 Processor -> + * Exception Model -> Exception entry and return -> Exception entry + * Cortex-M series processors -> Cortex-M3 -> Revision: xxx -> + * Cortex-M3 Devices Generic User Guide -> Cortex-M3 Peripherals -> + * System control block -> Configuration and Control Register (STKALIGN) + * + * This is just a helper function for use in the calculate_process_stack + * function for a given architecture/rtos. + */ +int64_t rtos_Cortex_M_stack_align(struct target *target, + const uint8_t *stack_data, const struct rtos_register_stacking *stacking, + int64_t stack_ptr, size_t xpsr_offset) +{ + const uint32_t ALIGN_NEEDED = (1 << 9); + uint32_t xpsr; + int64_t new_stack_ptr; + + new_stack_ptr = stack_ptr - stacking->stack_growth_direction * + stacking->stack_registers_size; + xpsr = (target->endianness == TARGET_LITTLE_ENDIAN) ? + le_to_h_u32(&stack_data[xpsr_offset]) : + be_to_h_u32(&stack_data[xpsr_offset]); + if ((xpsr & ALIGN_NEEDED) != 0) { + LOG_DEBUG("XPSR(0x%08" PRIx32 ") indicated stack alignment was necessary\r\n", + xpsr); + new_stack_ptr -= (stacking->stack_growth_direction * 4); + } + return new_stack_ptr; +} + +static int64_t rtos_standard_Cortex_M3_stack_align(struct target *target, + const uint8_t *stack_data, const struct rtos_register_stacking *stacking, + int64_t stack_ptr) +{ + const int XPSR_OFFSET = 0x3c; + return rtos_Cortex_M_stack_align(target, stack_data, stacking, + stack_ptr, XPSR_OFFSET); +} + const struct rtos_register_stacking rtos_standard_Cortex_M3_stacking = { 0x40, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - rtos_generic_stack_align8, /* stack_alignment */ + rtos_standard_Cortex_M3_stack_align, /* stack_alignment */ rtos_standard_Cortex_M3_stack_offsets /* register_offsets */ }; diff --git a/src/rtos/rtos_standard_stackings.h b/src/rtos/rtos_standard_stackings.h index c64a4be0..f931bb9f 100644 --- a/src/rtos/rtos_standard_stackings.h +++ b/src/rtos/rtos_standard_stackings.h @@ -33,5 +33,8 @@ extern const struct rtos_register_stacking rtos_standard_NDS32_N1068_stacking; int64_t rtos_generic_stack_align8(struct target *target, const uint8_t *stack_data, const struct rtos_register_stacking *stacking, int64_t stack_ptr); +int64_t rtos_Cortex_M_stack_align(struct target *target, + const uint8_t *stack_data, const struct rtos_register_stacking *stacking, + int64_t stack_ptr, size_t xpsr_offset); #endif /* ifndef INCLUDED_RTOS_STANDARD_STACKINGS_H_ */ From bb69aa340970e7caaefba878f86a9900a71ea854 Mon Sep 17 00:00:00 2001 From: Andrew Ruder Date: Mon, 5 Oct 2015 14:59:35 -0500 Subject: [PATCH 3/3] riot: support latest cortex m stack adjustment code --- src/rtos/rtos_riot_stackings.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/rtos/rtos_riot_stackings.c b/src/rtos/rtos_riot_stackings.c index 94e3669e..15dce4f4 100644 --- a/src/rtos/rtos_riot_stackings.c +++ b/src/rtos/rtos_riot_stackings.c @@ -22,6 +22,19 @@ #include "rtos.h" #include "target/armv7m.h" +#include "rtos_standard_stackings.h" + +/* This works for the M0 and M34 stackings as xPSR is in a fixed + * location + */ +static int64_t rtos_riot_Cortex_M_stack_align(struct target *target, + const uint8_t *stack_data, const struct rtos_register_stacking *stacking, + int64_t stack_ptr) +{ + const int XPSR_OFFSET = 0x40; + return rtos_Cortex_M_stack_align(target, stack_data, stacking, + stack_ptr, XPSR_OFFSET); +} /* see thread_arch.c */ static const struct stack_register_offset rtos_riot_Cortex_M0_stack_offsets[ARMV7M_NUM_CORE_REGS] = { @@ -48,7 +61,7 @@ const struct rtos_register_stacking rtos_riot_Cortex_M0_stacking = { 0x44, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 8, /* stack_alignment */ + rtos_riot_Cortex_M_stack_align, /* stack_alignment */ rtos_riot_Cortex_M0_stack_offsets /* register_offsets */ }; @@ -77,6 +90,6 @@ const struct rtos_register_stacking rtos_riot_Cortex_M34_stacking = { 0x44, /* stack_registers_size */ -1, /* stack_growth_direction */ ARMV7M_NUM_CORE_REGS, /* num_output_registers */ - 8, /* stack_alignment */ + rtos_riot_Cortex_M_stack_align, /* stack_alignment */ rtos_riot_Cortex_M34_stack_offsets /* register_offsets */ };