Add stack alignment support to RTOS awareness - needed for ARM ABI processors

Change-Id: I69a2f3d0606a97d48b7738561a85da87f458b82b
Signed-off-by: Evan Hunter <ehunter@broadcom.com>
Reviewed-on: http://openocd.zylin.com/238
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
Reviewed-by: Mathias Küster <kesmtp@freenet.de>
Reviewed-by: Øyvind Harboe <oyvindharboe@gmail.com>

4 files changed, 71 insertions, 62 deletions

diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 3c029f5..ac7f89c 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -490,6 +490,12 @@ int rtos_generic_stack_read( struct target * target, const struct rtos_register_
 	*hex_reg_list = (char*)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);
+	}
 	for( i = 0; i < stacking->num_output_registers; i++ )
 	{
 		int j;
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 1a73bd7..269ae8d 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -91,6 +91,7 @@ struct rtos_register_stacking
 	unsigned char                       stack_registers_size;
 	signed   char                       stack_growth_direction;
 	unsigned char                       num_output_registers;
+	unsigned char                       stack_alignment;
 	const struct stack_register_offset* register_offsets;
 };
 
diff --git a/src/rtos/rtos_ecos_stackings.c b/src/rtos/rtos_ecos_stackings.c
index a81b477..995c113 100644
--- a/src/rtos/rtos_ecos_stackings.c
+++ b/src/rtos/rtos_ecos_stackings.c
@@ -22,42 +22,43 @@
 
 #include "rtos.h"
 
-static const struct stack_register_offset rtos_eCos_Cortex_M3_stack_offsets [] =
-{ { 0x0c, 32 },       // r0
-  { 0x10, 32 },       // r1
-  { 0x14, 32 },       // r2
-  { 0x18, 32 },       // r3
-  { 0x1c, 32 },       // r4
-  { 0x20, 32 },       // r5
-  { 0x24, 32 },       // r6
-  { 0x28, 32 },       // r7
-  { 0x2c, 32 },       // r8
-  { 0x30, 32 },       // r9
-  { 0x34, 32 },       // r10
-  { 0x38, 32 },       // r11
-  { 0x3c, 32 },       // r12
-  { -2,   32 },       // sp
-  { -1,   32 },       // lr
-  { 0x40, 32 },       // pc
-  { -1,   96 },       // FPA1
-  { -1,   96 },       // FPA2
-  { -1,   96 },       // FPA3
-  { -1,   96 },       // FPA4
-  { -1,   96 },       // FPA5
-  { -1,   96 },       // FPA6
-  { -1,   96 },       // FPA7
-  { -1,   96 },       // FPA8
-  { -1,   32 },       // FPS
-  { -1,   32 },       // xPSR
+static const struct stack_register_offset rtos_eCos_Cortex_M3_stack_offsets[] = {
+	{ 0x0c, 32 },       /* r0   */
+	{ 0x10, 32 },       /* r1   */
+	{ 0x14, 32 },       /* r2   */
+	{ 0x18, 32 },       /* r3   */
+	{ 0x1c, 32 },       /* r4   */
+	{ 0x20, 32 },       /* r5   */
+	{ 0x24, 32 },       /* r6   */
+	{ 0x28, 32 },       /* r7   */
+	{ 0x2c, 32 },       /* r8   */
+	{ 0x30, 32 },       /* r9   */
+	{ 0x34, 32 },       /* r10  */
+	{ 0x38, 32 },       /* r11  */
+	{ 0x3c, 32 },       /* r12  */
+	{ -2,   32 },       /* sp   */
+	{ -1,   32 },       /* lr   */
+	{ 0x40, 32 },       /* pc   */
+	{ -1,   96 },       /* FPA1 */
+	{ -1,   96 },       /* FPA2 */
+	{ -1,   96 },       /* FPA3 */
+	{ -1,   96 },       /* FPA4 */
+	{ -1,   96 },       /* FPA5 */
+	{ -1,   96 },       /* FPA6 */
+	{ -1,   96 },       /* FPA7 */
+	{ -1,   96 },       /* FPA8 */
+	{ -1,   32 },       /* FPS  */
+	{ -1,   32 },       /* xPSR */
 };
 
 
 const struct rtos_register_stacking rtos_eCos_Cortex_M3_stacking =
 {
-          0x44,                                 // stack_registers_size
-          -1,                                   // stack_growth_direction
-          26,                                   // num_output_registers
-          rtos_eCos_Cortex_M3_stack_offsets     // register_offsets
+	0x44,                                 /* stack_registers_size */
+	-1,                                   /* stack_growth_direction */
+	26,                                   /* num_output_registers */
+	8,                                    /* stack_alignment */
+	rtos_eCos_Cortex_M3_stack_offsets     /* register_offsets */
 };
 
 
diff --git a/src/rtos/rtos_standard_stackings.c b/src/rtos/rtos_standard_stackings.c
index e15b8b3..278d3ef 100644
--- a/src/rtos/rtos_standard_stackings.c
+++ b/src/rtos/rtos_standard_stackings.c
@@ -24,42 +24,43 @@
 
 #include "rtos.h"
 
-static const struct stack_register_offset rtos_standard_Cortex_M3_stack_offsets [] =
-{ { 0x20, 32 },       // r0
-  { 0x24, 32 },       // r1
-  { 0x28, 32 },       // r2
-  { 0x2c, 32 },       // r3
-  { 0x00, 32 },       // r4
-  { 0x04, 32 },       // r5
-  { 0x08, 32 },       // r6
-  { 0x0c, 32 },       // r7
-  { 0x10, 32 },       // r8
-  { 0x14, 32 },       // r9
-  { 0x18, 32 },       // r10
-  { 0x1c, 32 },       // r11
-  { 0x30, 32 },       // r12
-  { -2,   32 },       // sp
-  { 0x34, 32 },       // lr
-  { 0x38, 32 },       // pc
-  { -1,   96 },       // FPA1
-  { -1,   96 },       // FPA2
-  { -1,   96 },       // FPA3
-  { -1,   96 },       // FPA4
-  { -1,   96 },       // FPA5
-  { -1,   96 },       // FPA6
-  { -1,   96 },       // FPA7
-  { -1,   96 },       // FPA8
-  { -1,   32 },       // FPS
-  { 0x3c, 32 },       // xPSR
+static const struct stack_register_offset rtos_standard_Cortex_M3_stack_offsets[] = {
+	{ 0x20, 32 },       /* r0   */
+	{ 0x24, 32 },       /* r1   */
+	{ 0x28, 32 },       /* r2   */
+	{ 0x2c, 32 },       /* r3   */
+	{ 0x00, 32 },       /* r4   */
+	{ 0x04, 32 },       /* r5   */
+	{ 0x08, 32 },       /* r6   */
+	{ 0x0c, 32 },       /* r7   */
+	{ 0x10, 32 },       /* r8   */
+	{ 0x14, 32 },       /* r9   */
+	{ 0x18, 32 },       /* r10  */
+	{ 0x1c, 32 },       /* r11  */
+	{ 0x30, 32 },       /* r12  */
+	{ -2,   32 },       /* sp   */
+	{ 0x34, 32 },       /* lr   */
+	{ 0x38, 32 },       /* pc   */
+	{ -1,   96 },       /* FPA1 */
+	{ -1,   96 },       /* FPA2 */
+	{ -1,   96 },       /* FPA3 */
+	{ -1,   96 },       /* FPA4 */
+	{ -1,   96 },       /* FPA5 */
+	{ -1,   96 },       /* FPA6 */
+	{ -1,   96 },       /* FPA7 */
+	{ -1,   96 },       /* FPA8 */
+	{ -1,   32 },       /* FPS  */
+	{ 0x3c, 32 },       /* xPSR */
 };
 
 
 const struct rtos_register_stacking rtos_standard_Cortex_M3_stacking =
 {
-          0x40,                                 // stack_registers_size
-          -1,                                   // stack_growth_direction
-          26,                                   // num_output_registers
-          rtos_standard_Cortex_M3_stack_offsets // register_offsets
+	0x40,                                 /* stack_registers_size */
+	-1,                                   /* stack_growth_direction */
+	26,                                   /* num_output_registers */
+	8,                                    /* stack_alignment */
+	rtos_standard_Cortex_M3_stack_offsets /* register_offsets */
 };