stacking adjustment code

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 */
 },
 };

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;

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;
 };
 

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 */
 };

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 */
 };

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 */
 };
 

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 */
 };
 

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 */
 };

src/rtos/rtos_standard_stackings.c

@@ -113,26 +113,103 @@ 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);
+}
+
+/* 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 */
-	8,						/* stack_alignment */
+	rtos_standard_Cortex_M3_stack_align,	/* stack_alignment */
 	rtos_standard_Cortex_M3_stack_offsets	/* register_offsets */
 };
 
 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 */
 };
 
 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 */
 };

src/rtos/rtos_standard_stackings.h

@@ -30,5 +30,11 @@
 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);
+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_ */