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.

Change-Id: I0f662cad0df81cbe5866219ad0fef980dcb3e44f
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/3002
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins

diff --git a/src/rtos/ThreadX.c b/src/rtos/ThreadX.c
index c033125455f3995d3df63a1e579e4477dd97daea..16df7a4b791943a35bce704a962e6ecdf3f872b3 100644 (file)
--- 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 */
        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 */
        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 */
 },
 };
        rtos_threadx_arm926ejs_stack_offsets_interrupt  /* register_offsets */
 },
 };

diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 3420d06b7520b919113f0f67340f6ffa98de8915..528546eaa3473f6700a3c1e442257e628df4d674 100644 (file)
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -485,13 +485,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;
                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;
        }
        for (i = 0; i < stacking->num_output_registers; i++) {
                int j;

diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 7750f3c7fa8ee4c8227bff53b1b146e839002560..d082fb7a72eedfac16b8b1bf9c405cd7d4b3e2d2 100644 (file)
--- 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_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;
 };
 
        const struct stack_register_offset *register_offsets;
 };
 

diff --git a/src/rtos/rtos_chibios_stackings.c b/src/rtos/rtos_chibios_stackings.c
index ed485616d3ba16401a82ba38e24446a95be9ddd8..e138b0684a0c22e5eb22232d608821b37494ed9f 100644 (file)
--- 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 */
        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 */
 };
 
        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 */
        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 */
 };
        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 53ba171f15c95bd4c00582f649d963764dd7324b..cc924ae22c69354fa840e3f0fd16d9b125827b26 100644 (file)
--- a/src/rtos/rtos_ecos_stackings.c
+++ b/src/rtos/rtos_ecos_stackings.c
@@ -21,6 +21,7 @@
 #endif
 
 #include "rtos.h"
 #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] = {
 #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 */
        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 */
 };
        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 315da301f9d8836caf8fc4a069fb77adfab0a0bc..8171957fdf96cc2e9816910cda06b81043202bb3 100644 (file)
--- 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.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   */
 
 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 */
        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 */
 };
 
        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 8f2d67ea9c03f616bf06c38c4da636ccef62fb77..fac18b6062b2cdaa57a2922d5ae942cb8c48a5f5 100644 (file)
--- 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 */
        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 */
 };
 
        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 2eab86f934984202a6f33b9535cb5fd4a175158c..fed393b958991e7d672561e246f076b6a5f12b11 100644 (file)
--- 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 */
 };
 
        { 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 */
 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 */
 };
 
        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 */
        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 */
 };
 
        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 */
        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 */
 };
        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 b76e2bb6dfd532e307b60bc6cc497dd416c05dca..c64a4be0cce300b1e2e801670fe2cef00cfe4ed7 100644 (file)
--- 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;
 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_ */
 
 #endif /* ifndef INCLUDED_RTOS_STANDARD_STACKINGS_H_ */