cortex_m.c: Use two byte breakpoint for 32bit Thumb-2 request

[openocd] / src / target / cortex_m.c

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index dc8d344401abe7d83f0db75d7b379b4f45489f61..a5230b75c737adabe6b8874e215282705ec693e5 100644 (file)
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -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;
@@ -1900,7 +1915,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 +1936,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 +1948,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);
                }
        }