armv7m: do not access FPU registers when not present

[openocd.git] / src / target / cortex_m.c

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index d3b8f4e080f5c18583a191024e398e4b62990e8d..38ed4c3a1f5f6ab32ed6460266482d89336331ab 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:
@@ -1839,6 +1885,17 @@ int cortex_m_examine(struct target *target)
                        armv7m->arm.is_armv6m = true;
                }
 
+               if (armv7m->fp_feature != FPv4_SP &&
+                   armv7m->arm.core_cache->num_regs > ARMV7M_NUM_CORE_REGS_NOFP) {
+                       /* free unavailable FPU registers */
+                       size_t idx;
+                       for (idx = ARMV7M_NUM_CORE_REGS_NOFP;
+                            idx < armv7m->arm.core_cache->num_regs;
+                            idx++)
+                               free(armv7m->arm.core_cache->reg_list[idx].value);
+                       armv7m->arm.core_cache->num_regs = ARMV7M_NUM_CORE_REGS_NOFP;
+               }
+
                if (i == 4 || i == 3) {
                        /* Cortex-M3/M4 has 4096 bytes autoincrement range */
                        armv7m->dap.tar_autoincr_block = (1 << 12);