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 d95c25ee72eaaf2f699726e7d4522242942a56c3..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) {
@@ -521,15 +521,8 @@ static int cortex_m_poll(struct target *target)
        }
 
        if (cortex_m->dcb_dhcsr & S_RESET_ST) {
-               /* check if still in reset */
-               retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
-               if (retval != ERROR_OK)
-                       return retval;
-
-               if (cortex_m->dcb_dhcsr & S_RESET_ST) {
-                       target->state = TARGET_RESET;
-                       return ERROR_OK;
-               }
+               target->state = TARGET_RESET;
+               return ERROR_OK;
        }
 
        if (target->state == TARGET_RESET) {
@@ -538,7 +531,11 @@ static int cortex_m_poll(struct target *target)
                 */
                LOG_DEBUG("Exit from reset with dcb_dhcsr 0x%" PRIx32,
                        cortex_m->dcb_dhcsr);
-               cortex_m_endreset_event(target);
+               retval = cortex_m_endreset_event(target);
+               if (retval != ERROR_OK) {
+                       target->state = TARGET_UNKNOWN;
+                       return retval;
+               }
                target->state = TARGET_RUNNING;
                prev_target_state = TARGET_RUNNING;
        }
@@ -1069,6 +1066,19 @@ static int cortex_m_assert_reset(struct target *target)
                                "handler to reset any peripherals or configure hardware srst support.");
                }
 
+               /*
+                 SAM4L needs to execute security initalization
+                 startup sequence before AP access would be enabled.
+                 During the intialization CDBGPWRUPACK is pulled low and we
+                 need to wait for it to be set to 1 again.
+               */
+               retval = dap_dp_poll_register(swjdp, DP_CTRL_STAT,
+                                             CDBGPWRUPACK, CDBGPWRUPACK, 100);
+               if (retval != ERROR_OK) {
+                       LOG_ERROR("Failed waitnig for CDBGPWRUPACK");
+                       return ERROR_FAIL;
+               }
+
                {
                        /* I do not know why this is necessary, but it
                         * fixes strange effects (step/resume cause NMI
@@ -1252,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;
@@ -1483,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:
@@ -1546,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:
@@ -1631,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)
@@ -1693,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;
 }
@@ -1728,7 +1795,7 @@ fail1:
                free(cm->dwt_comparator_list);
                goto fail0;
        }
-       cache->name = "cortex-m3 dwt registers";
+       cache->name = "Cortex-M DWT registers";
        cache->num_regs = 2 + cm->dwt_num_comp * 3;
        cache->reg_list = calloc(cache->num_regs, sizeof *cache->reg_list);
        if (!cache->reg_list) {
@@ -1900,7 +1967,9 @@ static int cortex_m_target_request_data(struct target *target,
        uint32_t i;
 
        for (i = 0; i < (size * 4); i++) {
-               cortex_m_dcc_read(target, &data, &ctrl);
+               int retval = cortex_m_dcc_read(target, &data, &ctrl);
+               if (retval != ERROR_OK)
+                       return retval;
                buffer[i] = data;
        }
 
@@ -1919,8 +1988,11 @@ static int cortex_m_handle_target_request(void *priv)
        if (target->state == TARGET_RUNNING) {
                uint8_t data;
                uint8_t ctrl;
+               int retval;
 
-               cortex_m_dcc_read(target, &data, &ctrl);
+               retval = cortex_m_dcc_read(target, &data, &ctrl);
+               if (retval != ERROR_OK)
+                       return retval;
 
                /* check if we have data */
                if (ctrl & (1 << 0)) {
@@ -1928,12 +2000,12 @@ static int cortex_m_handle_target_request(void *priv)
 
                        /* we assume target is quick enough */
                        request = data;
-                       cortex_m_dcc_read(target, &data, &ctrl);
-                       request |= (data << 8);
-                       cortex_m_dcc_read(target, &data, &ctrl);
-                       request |= (data << 16);
-                       cortex_m_dcc_read(target, &data, &ctrl);
-                       request |= (data << 24);
+                       for (int i = 1; i <= 3; i++) {
+                               retval = cortex_m_dcc_read(target, &data, &ctrl);
+                               if (retval != ERROR_OK)
+                                       return retval;
+                               request |= ((uint32_t)data << (i * 8));
+                       }
                        target_request(target, request);
                }
        }