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

Change-Id: Id0940e6bbd6a59adee1378c0e86fe86830f0c8fc
Signed-off-by: Andrew Ruder <andrew.ruder@elecsyscorp.com>
Cc: Paul Fertser <fercerpav@gmail.com>
Cc: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Cc: Evan Hunter <evanhunter920@gmail.com>
Cc: Jon Burgess <jburgess777@gmail.com>
Reviewed-on: http://openocd.zylin.com/3003
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>

diff --git a/src/rtos/rtos_standard_stackings.c b/src/rtos/rtos_standard_stackings.c
index fed393b958991e7d672561e246f076b6a5f12b11..1a58870fc5dc826b5c4241a2582583eaa1fc4f90 100644 (file)
--- 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 c64a4be0cce300b1e2e801670fe2cef00cfe4ed7..f931bb9f2695eb3572bcb2bc9456eb0c46c545df 100644 (file)
--- 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_ */