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 72dbe9ee6085a2c5a83a69091ba125af1282d93a..b194c33e6f1baa07e9f83a481391e5250cf87219 100644 (file)
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -473,7 +473,7 @@ static int cortex_m_debug_entry(struct target *target)
 
        LOG_DEBUG("entered debug state in core mode: %s at PC 0x%" PRIx32 ", target->state: %s",
                arm_mode_name(arm->core_mode),
-               *(uint32_t *)(arm->pc->value),
+               buf_get_u32(arm->pc->value, 0, 32),
                target_state_name(target));
 
        if (armv7m->post_debug_entry) {
@@ -1262,6 +1262,11 @@ int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }
 
+       if (breakpoint->length == 3) {
+               LOG_DEBUG("Using a two byte breakpoint for 32bit Thumb-2 request");
+               breakpoint->length = 2;
+       }
+
        if ((breakpoint->length != 2)) {
                LOG_INFO("only breakpoints of two bytes length supported");
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
@@ -1493,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:
@@ -1556,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:
@@ -1641,14 +1692,20 @@ static int cortex_m_init_target(struct command_context *cmd_ctx,
 struct dwt_reg_state {
        struct target *target;
        uint32_t addr;
-       uint32_t value;         /* scratch/cache */
+       uint8_t value[4];               /* scratch/cache */
 };
 
 static int cortex_m_dwt_get_reg(struct reg *reg)
 {
        struct dwt_reg_state *state = reg->arch_info;
 
-       return target_read_u32(state->target, state->addr, &state->value);
+       uint32_t tmp;
+       int retval = target_read_u32(state->target, state->addr, &tmp);
+       if (retval != ERROR_OK)
+               return retval;
+
+       buf_set_u32(state->value, 0, 32, tmp);
+       return ERROR_OK;
 }
 
 static int cortex_m_dwt_set_reg(struct reg *reg, uint8_t *buf)
@@ -1703,7 +1760,7 @@ static void cortex_m_dwt_addreg(struct target *t, struct reg *r, struct dwt_reg
 
        r->name = d->name;
        r->size = d->size;
-       r->value = &state->value;
+       r->value = state->value;
        r->arch_info = state;
        r->type = &dwt_reg_type;
 }