rtos: Add FPU detection to ChibiOS/RT

The stacking of ChibiOS/RT depends on the usage of an FPU. If the
FPU is enabled the FPU registers are also saved on context switch.

This patch adds automatic detection of FPU for armv7m targets.

Note: With this patch, openocd will only output an error message
      warning that the FPU is enabled.

      For further FPU support, the correct stacking information
      also needs to be added.

Change-Id: I0984cbd9180f247ba2fa610e74a6413cc54239ea
Signed-off-by: Matthias Blaicher <matthias@blaicher.com>
Reviewed-on: http://openocd.zylin.com/961
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 be5fe8472d29f0ce339ec9b96efcfa4b4ecc1cf1..71f929fe7b52bc8cc4235b56cd9371a3fa56046c 100644 (file)
--- a/src/rtos/ChibiOS.c
+++ b/src/rtos/ChibiOS.c
@@ -29,6 +29,8 @@
 #include <jtag/jtag.h>
 #include "target/target.h"
 #include "target/target_type.h"
+#include "target/armv7m.h"
+#include "target/cortex_m.h"
 #include "rtos.h"
 #include "helper/log.h"
 #include "helper/types.h"
@@ -94,12 +96,12 @@ struct ChibiOS_params ChibiOS_params_list[] = {
        {
        "cortex_m3",                                                    /* target_name */
        0,
-       &rtos_chibios_arm_v7m_stacking,         /* stacking_info */
+       NULL,                                                                   /* stacking_info */
        },
        {
        "stm32_stlink",                                                 /* target_name */
        0,
-       &rtos_chibios_arm_v7m_stacking,         /* stacking_info */
+       NULL,                                                                   /* stacking_info */
        }
 };
 #define CHIBIOS_NUM_PARAMS ((int)(sizeof(ChibiOS_params_list)/sizeof(struct ChibiOS_params)))
@@ -219,8 +221,43 @@ static int ChibiOS_update_stacking(struct rtos *rtos)
         *    available than the current execution. In which case
         *    ChibiOS_get_thread_reg_list is called.
         */
+       int retval;
+
+       if (!rtos->rtos_specific_params)
+               return -1;
+
+       struct ChibiOS_params *param;
+       param = (struct ChibiOS_params *) rtos->rtos_specific_params;
+
+       /* Check for armv7m with *enabled* FPU, i.e. a Cortex M4  */
+       struct armv7m_common *armv7m_target = target_to_armv7m(rtos->target);
+       if (is_armv7m(armv7m_target)) {
+               if (armv7m_target->fp_feature == FPv4_SP) {
+                       /* Found ARM v7m target which includes a FPU */
+                       uint32_t cpacr;
+
+                       retval = target_read_u32(rtos->target, FPU_CPACR, &cpacr);
+                       if (retval != ERROR_OK) {
+                               LOG_ERROR("Could not read CPACR register to check FPU state");
+                               return -1;
+                       }
+
+                       /* 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;
+                       }
+               }
+
+               /* Found ARM v7m target with no or disabled FPU */
+               param->stacking_info = &rtos_chibios_arm_v7m_stacking;
+               return 0;
+       }
 
-       /* TODO: Add actual detection, currently it will not work  with FPU enabled.*/
        return -1;
 }
 

diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index a10368d5240f0d3c72b14ec6d9ab61580a07b30a..743bd13db9790392d468b7b8f74ec45c1456f190 100644 (file)
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -59,6 +59,11 @@
 #define FP_COMP6       0xE0002020
 #define FP_COMP7       0xE0002024
 
+#define FPU_CPACR      0xE000ED88
+#define FPU_FPCCR      0xE000EF34
+#define FPU_FPCAR      0xE000EF38
+#define FPU_FPDSCR     0xE000EF3C
+
 /* DCB_DHCSR bit and field definitions */
 #define DBGKEY         (0xA05F << 16)
 #define C_DEBUGEN      (1 << 0)