From: Christian Gudrian <christian.gudrian@gmx.de>
Date: Fri, 24 Oct 2014 21:22:40 +0000 (+0200)
Subject: Added FPU support for ChibiOS/RT
X-Git-Tag: v0.9.0-rc1~226
X-Git-Url: https://git.sur5r.net/?a=commitdiff_plain;h=f83e1dc13fb79957dcb7de668628bf9e0762a2c1;p=openocd

Added FPU support for ChibiOS/RT

When an enabled FPU is detected we now use an appropriate stacking.

Change-Id: I1b0f43ec22e1c55c4f10e2ffa97d4aaa77bca5ee
Signed-off-by: Christian Gudrian <christian.gudrian@gmx.de>
Reviewed-on: http://openocd.zylin.com/2354
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
---

diff --git a/src/rtos/ChibiOS.c b/src/rtos/ChibiOS.c
index da5dc9fc..cd77f2ae 100644
--- a/src/rtos/ChibiOS.c
+++ b/src/rtos/ChibiOS.c
@@ -264,11 +264,9 @@ static int ChibiOS_update_stacking(struct rtos *rtos)
 			/* Check if CP10 and CP11 are set to full access.
 			 * In ChibiOS this is done in ResetHandler() in crt0.c */
 			if (cpacr & 0x00F00000) {
-				/* Found target with enabled FPU */
-				/* FIXME: Need to figure out how to specify the FPU registers */
-				LOG_ERROR("ChibiOS ARM v7m targets with enabled FPU "
-						  " are NOT supported");
-				return -1;
+				LOG_DEBUG("Enabled FPU detected.");
+				param->stacking_info = &rtos_chibios_arm_v7m_stacking_w_fpu;
+				return 0;
 			}
 		}
 
diff --git a/src/rtos/rtos_chibios_stackings.c b/src/rtos/rtos_chibios_stackings.c
index 0b5d0632..ed485616 100644
--- a/src/rtos/rtos_chibios_stackings.c
+++ b/src/rtos/rtos_chibios_stackings.c
@@ -55,3 +55,31 @@ const struct rtos_register_stacking rtos_chibios_arm_v7m_stacking = {
 	0,						/* stack_alignment */
 	rtos_chibios_arm_v7m_stack_offsets	/* register_offsets */
 };
+
+static const struct stack_register_offset rtos_chibios_arm_v7m_stack_offsets_w_fpu[ARMV7M_NUM_CORE_REGS] = {
+	{ -1,   32 },		/* r0   */
+	{ -1,   32 },		/* r1   */
+	{ -1,   32 },		/* r2   */
+	{ -1,   32 },		/* r3   */
+	{ 0x40, 32 },		/* r4   */
+	{ 0x44, 32 },		/* r5   */
+	{ 0x48, 32 },		/* r6   */
+	{ 0x4c, 32 },		/* r7   */
+	{ 0x50, 32 },		/* r8   */
+	{ 0x54, 32 },		/* r9   */
+	{ 0x58, 32 },		/* r10  */
+	{ 0x5c, 32 },		/* r11  */
+	{ -1,   32 },		/* r12  */
+	{ -2,   32 },		/* sp   */
+	{ -1,   32 },		/* lr   */
+	{ 0x60, 32 },		/* pc   */
+	{ -1,   32 },		/* xPSR */
+};
+
+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 */
+	rtos_chibios_arm_v7m_stack_offsets_w_fpu	/* register_offsets */
+};
diff --git a/src/rtos/rtos_chibios_stackings.h b/src/rtos/rtos_chibios_stackings.h
index b5eea647..8ba2a6ca 100644
--- a/src/rtos/rtos_chibios_stackings.h
+++ b/src/rtos/rtos_chibios_stackings.h
@@ -28,5 +28,6 @@
 #include "rtos.h"
 
 extern const struct rtos_register_stacking rtos_chibios_arm_v7m_stacking;
+extern const struct rtos_register_stacking rtos_chibios_arm_v7m_stacking_w_fpu;
 
 #endif	/* ifndef INCLUDED_RTOS_CHIBIOS_STACKINGS_H_ */