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 103ce502a1820bf9f7f9a2ec31e4c80b7a6f21d6..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;
        }
@@ -1265,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;
@@ -1913,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;
        }
 
@@ -1932,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)) {
@@ -1941,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);
                }
        }