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

9 files changed, 58 insertions, 18 deletions

diff --git a/src/rtos/ThreadX.c b/src/rtos/ThreadX.c
index c033125..16df7a4 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 3420d06..528546e 100644
--- 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;
-	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 7750f3c..d082fb7 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 ed48561..e138b06 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 53ba171..cc924ae 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 315da30..8171957 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 8f2d67e..fac18b6 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 2eab86f..fed393b 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 b76e2bb..c64a4be 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_ */