cortex_m: Fix possible endianness problem in emulated DCC channel

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

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 3b4629730f27d3535a4da800bcfd45c32f70f5a9..a224788526480c74d8cb43876103ffc7968ece28 100644 (file)
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -214,6 +214,24 @@ static int cortex_m_single_step_core(struct target *target)
        return ERROR_OK;
 }
 
+static int cortex_m_enable_fpb(struct target *target)
+{
+       int retval = target_write_u32(target, FP_CTRL, 3);
+       if (retval != ERROR_OK)
+               return retval;
+
+       /* check the fpb is actually enabled */
+       uint32_t fpctrl;
+       retval = target_read_u32(target, FP_CTRL, &fpctrl);
+       if (retval != ERROR_OK)
+               return retval;
+
+       if (fpctrl & 1)
+               return ERROR_OK;
+
+       return ERROR_FAIL;
+}
+
 static int cortex_m_endreset_event(struct target *target)
 {
        int i;
@@ -265,9 +283,11 @@ static int cortex_m_endreset_event(struct target *target)
         */
 
        /* Enable FPB */
-       retval = target_write_u32(target, FP_CTRL, 3);
-       if (retval != ERROR_OK)
+       retval = cortex_m_enable_fpb(target);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Failed to enable the FPB");
                return retval;
+       }
 
        cortex_m->fpb_enabled = 1;
 
@@ -751,7 +771,7 @@ static int cortex_m_resume(struct target *target, int current,
                /* Single step past breakpoint at current address */
                breakpoint = breakpoint_find(target, resume_pc);
                if (breakpoint) {
-                       LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %d)",
+                       LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %" PRIu32 ")",
                                breakpoint->address,
                                breakpoint->unique_id);
                        cortex_m_unset_breakpoint(target, breakpoint);
@@ -1109,7 +1129,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
        struct cortex_m_fp_comparator *comparator_list = cortex_m->fp_comparator_list;
 
        if (breakpoint->set) {
-               LOG_WARNING("breakpoint (BPID: %d) already set", breakpoint->unique_id);
+               LOG_WARNING("breakpoint (BPID: %" PRIu32 ") already set", breakpoint->unique_id);
                return ERROR_OK;
        }
 
@@ -1134,7 +1154,13 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
                        comparator_list[fp_num].fpcr_value);
                if (!cortex_m->fpb_enabled) {
                        LOG_DEBUG("FPB wasn't enabled, do it now");
-                       target_write_u32(target, FP_CTRL, 3);
+                       retval = cortex_m_enable_fpb(target);
+                       if (retval != ERROR_OK) {
+                               LOG_ERROR("Failed to enable the FPB");
+                               return retval;
+                       }
+
+                       cortex_m->fpb_enabled = 1;
                }
        } else if (breakpoint->type == BKPT_SOFT) {
                uint8_t code[4];
@@ -1159,7 +1185,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
                breakpoint->set = true;
        }
 
-       LOG_DEBUG("BPID: %d, Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+       LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
                breakpoint->unique_id,
                (int)(breakpoint->type),
                breakpoint->address,
@@ -1180,7 +1206,7 @@ int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoi
                return ERROR_OK;
        }
 
-       LOG_DEBUG("BPID: %d, Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+       LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
                breakpoint->unique_id,
                (int)(breakpoint->type),
                breakpoint->address,
@@ -1853,12 +1879,19 @@ int cortex_m_examine(struct target *target)
        return ERROR_OK;
 }
 
-static int cortex_m_dcc_read(struct adiv5_dap *swjdp, uint8_t *value, uint8_t *ctrl)
+static int cortex_m_dcc_read(struct target *target, uint8_t *value, uint8_t *ctrl)
 {
+       struct armv7m_common *armv7m = target_to_armv7m(target);
+       struct adiv5_dap *swjdp = armv7m->arm.dap;
        uint16_t dcrdr;
+       uint8_t buf[2];
        int retval;
 
-       mem_ap_read_buf_u16(swjdp, (uint8_t *)&dcrdr, 2, DCB_DCRDR);
+       retval = mem_ap_read(swjdp, buf, 2, 1, DCB_DCRDR, false);
+       if (retval != ERROR_OK)
+               return retval;
+
+       dcrdr = target_buffer_get_u16(target, buf);
        *ctrl = (uint8_t)dcrdr;
        *value = (uint8_t)(dcrdr >> 8);
 
@@ -1867,8 +1900,8 @@ static int cortex_m_dcc_read(struct adiv5_dap *swjdp, uint8_t *value, uint8_t *c
        /* write ack back to software dcc register
         * signify we have read data */
        if (dcrdr & (1 << 0)) {
-               dcrdr = 0;
-               retval = mem_ap_write_buf_u16(swjdp, (uint8_t *)&dcrdr, 2, DCB_DCRDR);
+               target_buffer_set_u16(target, buf, 0);
+               retval = mem_ap_write(swjdp, buf, 2, 1, DCB_DCRDR, false);
                if (retval != ERROR_OK)
                        return retval;
        }
@@ -1879,14 +1912,12 @@ static int cortex_m_dcc_read(struct adiv5_dap *swjdp, uint8_t *value, uint8_t *c
 static int cortex_m_target_request_data(struct target *target,
        uint32_t size, uint8_t *buffer)
 {
-       struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct adiv5_dap *swjdp = armv7m->arm.dap;
        uint8_t data;
        uint8_t ctrl;
        uint32_t i;
 
        for (i = 0; i < (size * 4); i++) {
-               cortex_m_dcc_read(swjdp, &data, &ctrl);
+               cortex_m_dcc_read(target, &data, &ctrl);
                buffer[i] = data;
        }
 
@@ -1898,8 +1929,6 @@ static int cortex_m_handle_target_request(void *priv)
        struct target *target = priv;
        if (!target_was_examined(target))
                return ERROR_OK;
-       struct armv7m_common *armv7m = target_to_armv7m(target);
-       struct adiv5_dap *swjdp = armv7m->arm.dap;
 
        if (!target->dbg_msg_enabled)
                return ERROR_OK;
@@ -1908,7 +1937,7 @@ static int cortex_m_handle_target_request(void *priv)
                uint8_t data;
                uint8_t ctrl;
 
-               cortex_m_dcc_read(swjdp, &data, &ctrl);
+               cortex_m_dcc_read(target, &data, &ctrl);
 
                /* check if we have data */
                if (ctrl & (1 << 0)) {
@@ -1916,11 +1945,11 @@ static int cortex_m_handle_target_request(void *priv)
 
                        /* we assume target is quick enough */
                        request = data;
-                       cortex_m_dcc_read(swjdp, &data, &ctrl);
+                       cortex_m_dcc_read(target, &data, &ctrl);
                        request |= (data << 8);
-                       cortex_m_dcc_read(swjdp, &data, &ctrl);
+                       cortex_m_dcc_read(target, &data, &ctrl);
                        request |= (data << 16);
-                       cortex_m_dcc_read(swjdp, &data, &ctrl);
+                       cortex_m_dcc_read(target, &data, &ctrl);
                        request |= (data << 24);
                        target_request(target, request);
                }