target/cortex_a: Extract code to read/write from/to register to/from DCC

[openocd.git] / src / target / cortex_a.c

diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 9fc265288c392c761a7cf13531975d417794301c..8773ea1607a1019a6c778bc5e14088e2e4008259 100644 (file)
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -55,6 +55,7 @@
 #include "target_type.h"
 #include "arm_opcodes.h"
 #include "arm_semihosting.h"
+#include "jtag/interface.h"
 #include "transport/transport.h"
 #include "smp.h"
 #include <helper/time_support.h>
@@ -424,22 +425,35 @@ static int cortex_a_instr_write_data_dcc(struct arm_dpm *dpm,
                        &dscr);
 }
 
-static int cortex_a_instr_write_data_r0(struct arm_dpm *dpm,
-       uint32_t opcode, uint32_t data)
+static int cortex_a_instr_write_data_rt_dcc(struct arm_dpm *dpm,
+       uint8_t rt, uint32_t data)
 {
        struct cortex_a_common *a = dpm_to_a(dpm);
        uint32_t dscr = DSCR_INSTR_COMP;
        int retval;
 
+       if (rt > 15)
+               return ERROR_TARGET_INVALID;
+
        retval = cortex_a_write_dcc(a, data);
        if (retval != ERROR_OK)
                return retval;
 
-       /* DCCRX to R0, "MCR p14, 0, R0, c0, c5, 0", 0xEE000E15 */
-       retval = cortex_a_exec_opcode(
+       /* DCCRX to Rt, "MCR p14, 0, R0, c0, c5, 0", 0xEE000E15 */
+       return cortex_a_exec_opcode(
                        a->armv7a_common.arm.target,
-                       ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
+                       ARMV4_5_MRC(14, 0, rt, 0, 5, 0),
                        &dscr);
+}
+
+static int cortex_a_instr_write_data_r0(struct arm_dpm *dpm,
+       uint32_t opcode, uint32_t data)
+{
+       struct cortex_a_common *a = dpm_to_a(dpm);
+       uint32_t dscr = DSCR_INSTR_COMP;
+       int retval;
+
+       retval = cortex_a_instr_write_data_rt_dcc(dpm, 0, data);
        if (retval != ERROR_OK)
                return retval;
 
@@ -481,31 +495,43 @@ static int cortex_a_instr_read_data_dcc(struct arm_dpm *dpm,
        return cortex_a_read_dcc(a, data, &dscr);
 }
 
-
-static int cortex_a_instr_read_data_r0(struct arm_dpm *dpm,
-       uint32_t opcode, uint32_t *data)
+static int cortex_a_instr_read_data_rt_dcc(struct arm_dpm *dpm,
+       uint8_t rt, uint32_t *data)
 {
        struct cortex_a_common *a = dpm_to_a(dpm);
        uint32_t dscr = DSCR_INSTR_COMP;
        int retval;
 
-       /* the opcode, writing data to R0 */
+       if (rt > 15)
+               return ERROR_TARGET_INVALID;
+
        retval = cortex_a_exec_opcode(
                        a->armv7a_common.arm.target,
-                       opcode,
+                       ARMV4_5_MCR(14, 0, rt, 0, 5, 0),
                        &dscr);
        if (retval != ERROR_OK)
                return retval;
 
-       /* write R0 to DCC */
+       return cortex_a_read_dcc(a, data, &dscr);
+}
+
+static int cortex_a_instr_read_data_r0(struct arm_dpm *dpm,
+       uint32_t opcode, uint32_t *data)
+{
+       struct cortex_a_common *a = dpm_to_a(dpm);
+       uint32_t dscr = DSCR_INSTR_COMP;
+       int retval;
+
+       /* the opcode, writing data to R0 */
        retval = cortex_a_exec_opcode(
                        a->armv7a_common.arm.target,
-                       ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
+                       opcode,
                        &dscr);
        if (retval != ERROR_OK)
                return retval;
 
-       return cortex_a_read_dcc(a, data, &dscr);
+       /* write R0 to DCC */
+       return cortex_a_instr_read_data_rt_dcc(dpm, 0, data);
 }
 
 static int cortex_a_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
@@ -1659,7 +1685,7 @@ static int cortex_a_assert_reset(struct target *target)
                 */
                if (transport_is_swd() ||
                                (target->reset_halt && (jtag_get_reset_config() & RESET_SRST_NO_GATING)))
-                       jtag_add_reset(0, 1);
+                       adapter_assert_reset();
 
        } else {
                LOG_ERROR("%s: how to reset?", target_name(target));
@@ -1682,7 +1708,7 @@ static int cortex_a_deassert_reset(struct target *target)
        LOG_DEBUG(" ");
 
        /* be certain SRST is off */
-       jtag_add_reset(0, 0);
+       adapter_deassert_reset();
 
        if (target_was_examined(target)) {
                retval = cortex_a_poll(target);
@@ -1892,7 +1918,8 @@ static int cortex_a_write_cpu_memory_slow(struct target *target,
 {
        /* Writes count objects of size size from *buffer. Old value of DSCR must
         * be in *dscr; updated to new value. This is slow because it works for
-        * non-word-sized objects and (maybe) unaligned accesses. If size == 4 and
+        * non-word-sized objects. Avoid unaligned accesses as they do not work
+        * on memory address space without "Normal" attribute. If size == 4 and
         * the address is aligned, cortex_a_write_cpu_memory_fast should be
         * preferred.
         * Preconditions:
@@ -2049,7 +2076,22 @@ static int cortex_a_write_cpu_memory(struct target *target,
                /* We are doing a word-aligned transfer, so use fast mode. */
                retval = cortex_a_write_cpu_memory_fast(target, count, buffer, &dscr);
        } else {
-               /* Use slow path. */
+               /* Use slow path. Adjust size for aligned accesses */
+               switch (address % 4) {
+                       case 1:
+                       case 3:
+                               count *= size;
+                               size = 1;
+                               break;
+                       case 2:
+                               if (size == 4) {
+                                       count *= 2;
+                                       size = 2;
+                               }
+                       case 0:
+                       default:
+                               break;
+               }
                retval = cortex_a_write_cpu_memory_slow(target, size, count, buffer, &dscr);
        }
 
@@ -2135,7 +2177,8 @@ static int cortex_a_read_cpu_memory_slow(struct target *target,
 {
        /* Reads count objects of size size into *buffer. Old value of DSCR must be
         * in *dscr; updated to new value. This is slow because it works for
-        * non-word-sized objects and (maybe) unaligned accesses. If size == 4 and
+        * non-word-sized objects. Avoid unaligned accesses as they do not work
+        * on memory address space without "Normal" attribute. If size == 4 and
         * the address is aligned, cortex_a_read_cpu_memory_fast should be
         * preferred.
         * Preconditions:
@@ -2351,7 +2394,23 @@ static int cortex_a_read_cpu_memory(struct target *target,
                /* We are doing a word-aligned transfer, so use fast mode. */
                retval = cortex_a_read_cpu_memory_fast(target, count, buffer, &dscr);
        } else {
-               /* Use slow path. */
+               /* Use slow path. Adjust size for aligned accesses */
+               switch (address % 4) {
+                       case 1:
+                       case 3:
+                               count *= size;
+                               size = 1;
+                               break;
+                       case 2:
+                               if (size == 4) {
+                                       count *= 2;
+                                       size = 2;
+                               }
+                               break;
+                       case 0:
+                       default:
+                               break;
+               }
                retval = cortex_a_read_cpu_memory_slow(target, size, count, buffer, &dscr);
        }
 
@@ -2922,7 +2981,7 @@ static int cortex_a_virt2phys(struct target *target,
        if (retval != ERROR_OK)
                return retval;
        return armv7a_mmu_translate_va_pa(target, (uint32_t)virt,
-                                                   (uint32_t *)phys, 1);
+                                                   phys, 1);
 }
 
 COMMAND_HANDLER(cortex_a_handle_cache_info_command)