armv7m: add FPU registers support

[openocd] / src / target / cortex_m.c

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index d3b8f4e080f5c18583a191024e398e4b62990e8d..b194c33e6f1baa07e9f83a481391e5250cf87219 100644 (file)
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1498,6 +1498,29 @@ static int cortex_m_load_core_reg_u32(struct target *target,
                        LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
                        break;
 
+               case ARMV7M_FPSCR:
+                       /* Floating-point Status and Registers */
+                       retval = target_write_u32(target, DCB_DCRSR, 0x21);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       retval = target_read_u32(target, DCB_DCRDR, value);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       LOG_DEBUG("load from FPSCR  value 0x%" PRIx32, *value);
+                       break;
+
+               case ARMV7M_S0 ... ARMV7M_S31:
+                       /* Floating-point Status and Registers */
+                       retval = target_write_u32(target, DCB_DCRSR, num - ARMV7M_S0 + 0x40);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       retval = target_read_u32(target, DCB_DCRDR, value);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       LOG_DEBUG("load from FPU reg S%d  value 0x%" PRIx32,
+                                 (int)(num - ARMV7M_S0), *value);
+                       break;
+
                case ARMV7M_PRIMASK:
                case ARMV7M_BASEPRI:
                case ARMV7M_FAULTMASK:
@@ -1561,6 +1584,29 @@ static int cortex_m_store_core_reg_u32(struct target *target,
                        LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
                        break;
 
+               case ARMV7M_FPSCR:
+                       /* Floating-point Status and Registers */
+                       retval = target_write_u32(target, DCB_DCRDR, value);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       retval = target_write_u32(target, DCB_DCRSR, 0x21 | (1<<16));
+                       if (retval != ERROR_OK)
+                               return retval;
+                       LOG_DEBUG("write FPSCR value 0x%" PRIx32, value);
+                       break;
+
+               case ARMV7M_S0 ... ARMV7M_S31:
+                       /* Floating-point Status and Registers */
+                       retval = target_write_u32(target, DCB_DCRDR, value);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       retval = target_write_u32(target, DCB_DCRSR, (num - ARMV7M_S0 + 0x40) | (1<<16));
+                       if (retval != ERROR_OK)
+                               return retval;
+                       LOG_DEBUG("write FPU reg S%d  value 0x%" PRIx32,
+                                 (int)(num - ARMV7M_S0), value);
+                       break;
+
                case ARMV7M_PRIMASK:
                case ARMV7M_BASEPRI:
                case ARMV7M_FAULTMASK: