aarch64: allow reading system control register when halted in EL0
[openocd.git] / src / target / aarch64.c
index ae7f5a18e4ed0c8cddeef81f3262539701e6a384..41bea2e93dca22531055a6d25004f86039e966cd 100644 (file)
@@ -27,6 +27,7 @@
 #include "target_request.h"
 #include "target_type.h"
 #include "armv8_opcodes.h"
+#include "armv8_cache.h"
 #include <helper/time_support.h>
 
 static int aarch64_poll(struct target *target);
@@ -45,8 +46,6 @@ static int aarch64_virt2phys(struct target *target,
        target_addr_t virt, target_addr_t *phys);
 static int aarch64_read_apb_ap_memory(struct target *target,
        uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
-static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
-       uint32_t opcode, uint32_t data);
 
 static int aarch64_restore_system_control_reg(struct target *target)
 {
@@ -89,7 +88,10 @@ static int aarch64_restore_system_control_reg(struct target *target)
                                        return retval;
                        break;
                        default:
-                               LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+                               retval = armv8->arm.mcr(target, 15, 0, 0, 1, 0, aarch64->system_control_reg);
+                               if (retval != ERROR_OK)
+                                       return retval;
+                               break;
                        }
        }
        return retval;
@@ -211,19 +213,6 @@ static int aarch64_init_debug_access(struct target *target)
 
        LOG_DEBUG(" ");
 
-       /* Unlocking the debug registers for modification
-        * The debugport might be uninitialised so try twice */
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                            armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
-       if (retval != ERROR_OK) {
-               /* try again */
-               retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                            armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
-               if (retval == ERROR_OK)
-                       LOG_USER("Locking debug access failed on first, but succeeded on second try.");
-       }
-       if (retval != ERROR_OK)
-               return retval;
        /* Clear Sticky Power Down status Bit in PRSR to enable access to
           the registers in the Core Power Domain */
        retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -231,67 +220,35 @@ static int aarch64_init_debug_access(struct target *target)
        if (retval != ERROR_OK)
                return retval;
 
-       /* Enabling of instruction execution in debug mode is done in debug_entry code */
-
-       /* Resync breakpoint registers */
-
-       /* Since this is likely called from init or reset, update target state information*/
-       return aarch64_poll(target);
-}
-
-/* To reduce needless round-trips, pass in a pointer to the current
- * DSCR value.  Initialize it to zero if you just need to know the
- * value on return from this function; or DSCR_ITE if you
- * happen to know that no instruction is pending.
- */
-static int aarch64_exec_opcode(struct target *target,
-       uint32_t opcode, uint32_t *dscr_p)
-{
-       uint32_t dscr;
-       int retval;
-       struct armv8_common *armv8 = target_to_armv8(target);
-       dscr = dscr_p ? *dscr_p : 0;
-
-       LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
-
-       /* Wait for InstrCompl bit to be set */
-       long long then = timeval_ms();
-       while ((dscr & DSCR_ITE) == 0) {
-               retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                               armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-               if (retval != ERROR_OK) {
-                       LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
-                       return retval;
-               }
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
-                       return ERROR_FAIL;
-               }
-       }
+       /*
+        * Static CTI configuration:
+        * Channel 0 -> trigger outputs HALT request to PE
+        * Channel 1 -> trigger outputs Resume request to PE
+        * Gate all channel trigger events from entering the CTM
+        */
 
-       retval = mem_ap_write_u32(armv8->debug_ap,
-                       armv8->debug_base + CPUV8_DBG_ITR, opcode);
+       /* Enable CTI */
+       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                       armv8->cti_base + CTI_CTR, 1);
+       /* By default, gate all channel triggers to and from the CTM */
+       if (retval == ERROR_OK)
+               retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                               armv8->cti_base + CTI_GATE, 0);
+       /* output halt requests to PE on channel 0 trigger */
+       if (retval == ERROR_OK)
+               retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                               armv8->cti_base + CTI_OUTEN0, CTI_CHNL(0));
+       /* output restart requests to PE on channel 1 trigger */
+       if (retval == ERROR_OK)
+               retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                               armv8->cti_base + CTI_OUTEN1, CTI_CHNL(1));
        if (retval != ERROR_OK)
                return retval;
 
-       then = timeval_ms();
-       do {
-               retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                               armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-               if (retval != ERROR_OK) {
-                       LOG_ERROR("Could not read DSCR register");
-                       return retval;
-               }
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
-                       return ERROR_FAIL;
-               }
-       } while ((dscr & DSCR_ITE) == 0);       /* Wait for InstrCompl bit to be set */
-
-       if (dscr_p)
-               *dscr_p = dscr;
+       /* Resync breakpoint registers */
 
-       return retval;
+       /* Since this is likely called from init or reset, update target state information*/
+       return aarch64_poll(target);
 }
 
 /* Write to memory mapped registers directly with no cache or mmu handling */
@@ -307,429 +264,6 @@ static int aarch64_dap_write_memap_register_u32(struct target *target,
        return retval;
 }
 
-/*
- * AARCH64 implementation of Debug Programmer's Model
- *
- * NOTE the invariant:  these routines return with DSCR_ITE set,
- * so there's no need to poll for it before executing an instruction.
- *
- * NOTE that in several of these cases the "stall" mode might be useful.
- * It'd let us queue a few operations together... prepare/finish might
- * be the places to enable/disable that mode.
- */
-
-static inline struct aarch64_common *dpm_to_a8(struct arm_dpm *dpm)
-{
-       return container_of(dpm, struct aarch64_common, armv8_common.dpm);
-}
-
-static int aarch64_write_dcc(struct armv8_common *armv8, uint32_t data)
-{
-       LOG_DEBUG("write DCC 0x%08" PRIx32, data);
-       return mem_ap_write_u32(armv8->debug_ap,
-                               armv8->debug_base + CPUV8_DBG_DTRRX, data);
-}
-
-static int aarch64_write_dcc_64(struct armv8_common *armv8, uint64_t data)
-{
-       int ret;
-       LOG_DEBUG("write DCC Low word0x%08" PRIx32, (unsigned)data);
-       LOG_DEBUG("write DCC High word 0x%08" PRIx32, (unsigned)(data >> 32));
-       ret = mem_ap_write_u32(armv8->debug_ap,
-                              armv8->debug_base + CPUV8_DBG_DTRRX, data);
-       ret += mem_ap_write_u32(armv8->debug_ap,
-                               armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
-       return ret;
-}
-
-static int aarch64_read_dcc(struct armv8_common *armv8, uint32_t *data,
-       uint32_t *dscr_p)
-{
-       uint32_t dscr = DSCR_ITE;
-       int retval;
-
-       if (dscr_p)
-               dscr = *dscr_p;
-
-       /* Wait for DTRRXfull */
-       long long then = timeval_ms();
-       while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-               retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                               armv8->debug_base + CPUV8_DBG_DSCR,
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for read dcc");
-                       return ERROR_FAIL;
-               }
-       }
-
-       retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                                           armv8->debug_base + CPUV8_DBG_DTRTX,
-                                           data);
-       if (retval != ERROR_OK)
-               return retval;
-       LOG_DEBUG("read DCC 0x%08" PRIx32, *data);
-
-       if (dscr_p)
-               *dscr_p = dscr;
-
-       return retval;
-}
-
-static int aarch64_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
-       uint32_t *dscr_p)
-{
-       uint32_t dscr = DSCR_ITE;
-       uint32_t higher;
-       int retval;
-
-       if (dscr_p)
-               dscr = *dscr_p;
-
-       /* Wait for DTRRXfull */
-       long long then = timeval_ms();
-       while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-               retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                               armv8->debug_base + CPUV8_DBG_DSCR,
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for read dcc");
-                       return ERROR_FAIL;
-               }
-       }
-
-       retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                                           armv8->debug_base + CPUV8_DBG_DTRTX,
-                                           (uint32_t *)data);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                                           armv8->debug_base + CPUV8_DBG_DTRRX,
-                                           &higher);
-       if (retval != ERROR_OK)
-               return retval;
-
-       *data = *(uint32_t *)data | (uint64_t)higher << 32;
-       LOG_DEBUG("read DCC 0x%16.16" PRIx64, *data);
-
-       if (dscr_p)
-               *dscr_p = dscr;
-
-       return retval;
-}
-
-static int aarch64_dpm_prepare(struct arm_dpm *dpm)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       uint32_t dscr;
-       int retval;
-
-       /* set up invariant:  INSTR_COMP is set after ever DPM operation */
-       long long then = timeval_ms();
-       for (;; ) {
-               retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-                               a8->armv8_common.debug_base + CPUV8_DBG_DSCR,
-                               &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-               if ((dscr & DSCR_ITE) != 0)
-                       break;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for dpm prepare");
-                       return ERROR_FAIL;
-               }
-       }
-
-       /* this "should never happen" ... */
-       if (dscr & DSCR_DTR_RX_FULL) {
-               LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
-               /* Clear DCCRX */
-               retval = mem_ap_read_u32(a8->armv8_common.debug_ap,
-                       a8->armv8_common.debug_base + CPUV8_DBG_DTRRX, &dscr);
-               if (retval != ERROR_OK)
-                       return retval;
-
-               /* Clear sticky error */
-               retval = mem_ap_write_u32(a8->armv8_common.debug_ap,
-                       a8->armv8_common.debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
-               if (retval != ERROR_OK)
-                       return retval;
-       }
-
-       return retval;
-}
-
-static int aarch64_dpm_finish(struct arm_dpm *dpm)
-{
-       /* REVISIT what could be done here? */
-       return ERROR_OK;
-}
-
-static int aarch64_instr_execute(struct arm_dpm *dpm,
-       uint32_t opcode)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       uint32_t dscr = DSCR_ITE;
-
-       return aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-}
-
-static int aarch64_instr_write_data_dcc(struct arm_dpm *dpm,
-       uint32_t opcode, uint32_t data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       int retval;
-       uint32_t dscr = DSCR_ITE;
-
-       retval = aarch64_write_dcc(&a8->armv8_common, data);
-       if (retval != ERROR_OK)
-               return retval;
-
-       return aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-}
-
-static int aarch64_instr_write_data_dcc_64(struct arm_dpm *dpm,
-       uint32_t opcode, uint64_t data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       int retval;
-       uint32_t dscr = DSCR_ITE;
-
-       retval = aarch64_write_dcc_64(&a8->armv8_common, data);
-       if (retval != ERROR_OK)
-               return retval;
-
-       return aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-}
-
-static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
-       uint32_t opcode, uint32_t data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       uint32_t dscr = DSCR_ITE;
-       int retval;
-
-       retval = aarch64_write_dcc(&a8->armv8_common, data);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       /* then the opcode, taking data from R0 */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-
-       return retval;
-}
-
-static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
-       uint32_t opcode, uint64_t data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       uint32_t dscr = DSCR_ITE;
-       int retval;
-
-       retval = aarch64_write_dcc_64(&a8->armv8_common, data);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0),
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       /* then the opcode, taking data from R0 */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-
-       return retval;
-}
-
-static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
-{
-       struct target *target = dpm->arm->target;
-       uint32_t dscr = DSCR_ITE;
-
-       /* "Prefetch flush" after modifying execution status in CPSR */
-       return aarch64_exec_opcode(target,
-                       DSB_SY,
-                       &dscr);
-}
-
-static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
-       uint32_t opcode, uint32_t *data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       int retval;
-       uint32_t dscr = DSCR_ITE;
-
-       /* the opcode, writing data to DCC */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       return aarch64_read_dcc(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_instr_read_data_dcc_64(struct arm_dpm *dpm,
-       uint32_t opcode, uint64_t *data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       int retval;
-       uint32_t dscr = DSCR_ITE;
-
-       /* the opcode, writing data to DCC */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       return aarch64_read_dcc_64(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
-       uint32_t opcode, uint32_t *data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       uint32_t dscr = DSCR_ITE;
-       int retval;
-
-       /* the opcode, writing data to R0 */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       /* write R0 to DCC */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),  /* msr dbgdtr_el0, x0 */
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       return aarch64_read_dcc(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
-       uint32_t opcode, uint64_t *data)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       uint32_t dscr = DSCR_ITE;
-       int retval;
-
-       /* the opcode, writing data to R0 */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       opcode,
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       /* write R0 to DCC */
-       retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0),  /* msr dbgdtr_el0, x0 */
-                       &dscr);
-       if (retval != ERROR_OK)
-               return retval;
-
-       return aarch64_read_dcc_64(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
-       uint32_t addr, uint32_t control)
-{
-       struct aarch64_common *a8 = dpm_to_a8(dpm);
-       uint32_t vr = a8->armv8_common.debug_base;
-       uint32_t cr = a8->armv8_common.debug_base;
-       int retval;
-
-       switch (index_t) {
-               case 0 ... 15:  /* breakpoints */
-                       vr += CPUV8_DBG_BVR_BASE;
-                       cr += CPUV8_DBG_BCR_BASE;
-                       break;
-               case 16 ... 31: /* watchpoints */
-                       vr += CPUV8_DBG_WVR_BASE;
-                       cr += CPUV8_DBG_WCR_BASE;
-                       index_t -= 16;
-                       break;
-               default:
-                       return ERROR_FAIL;
-       }
-       vr += 16 * index_t;
-       cr += 16 * index_t;
-
-       LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
-               (unsigned) vr, (unsigned) cr);
-
-       retval = aarch64_dap_write_memap_register_u32(dpm->arm->target,
-                       vr, addr);
-       if (retval != ERROR_OK)
-               return retval;
-       retval = aarch64_dap_write_memap_register_u32(dpm->arm->target,
-                       cr, control);
-       return retval;
-}
-
-static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
-{
-       struct aarch64_common *a = dpm_to_a8(dpm);
-       uint32_t cr;
-
-       switch (index_t) {
-               case 0 ... 15:
-                       cr = a->armv8_common.debug_base + CPUV8_DBG_BCR_BASE;
-                       break;
-               case 16 ... 31:
-                       cr = a->armv8_common.debug_base + CPUV8_DBG_WCR_BASE;
-                       index_t -= 16;
-                       break;
-               default:
-                       return ERROR_FAIL;
-       }
-       cr += 16 * index_t;
-
-       LOG_DEBUG("A: bpwp disable, cr %08x", (unsigned) cr);
-
-       /* clear control register */
-       return aarch64_dap_write_memap_register_u32(dpm->arm->target, cr, 0);
-
-}
-
 static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 {
        struct arm_dpm *dpm = &a8->armv8_common.dpm;
@@ -738,32 +272,13 @@ static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
        dpm->arm = &a8->armv8_common.arm;
        dpm->didr = debug;
 
-       dpm->prepare = aarch64_dpm_prepare;
-       dpm->finish = aarch64_dpm_finish;
-
-       dpm->instr_execute = aarch64_instr_execute;
-       dpm->instr_write_data_dcc = aarch64_instr_write_data_dcc;
-       dpm->instr_write_data_dcc_64 = aarch64_instr_write_data_dcc_64;
-       dpm->instr_write_data_r0 = aarch64_instr_write_data_r0;
-       dpm->instr_write_data_r0_64 = aarch64_instr_write_data_r0_64;
-       dpm->instr_cpsr_sync = aarch64_instr_cpsr_sync;
-
-       dpm->instr_read_data_dcc = aarch64_instr_read_data_dcc;
-       dpm->instr_read_data_dcc_64 = aarch64_instr_read_data_dcc_64;
-       dpm->instr_read_data_r0 = aarch64_instr_read_data_r0;
-       dpm->instr_read_data_r0_64 = aarch64_instr_read_data_r0_64;
-
-       dpm->arm_reg_current = armv8_reg_current;
-
-       dpm->bpwp_enable = aarch64_bpwp_enable;
-       dpm->bpwp_disable = aarch64_bpwp_disable;
-
        retval = armv8_dpm_setup(dpm);
        if (retval == ERROR_OK)
                retval = armv8_dpm_initialize(dpm);
 
        return retval;
 }
+
 static struct target *get_aarch64(struct target *target, int32_t coreid)
 {
        struct target_list *head;
@@ -782,16 +297,27 @@ static int aarch64_halt(struct target *target);
 
 static int aarch64_halt_smp(struct target *target)
 {
-       int retval = 0;
-       struct target_list *head;
-       struct target *curr;
-       head = target->head;
+       int retval = ERROR_OK;
+       struct target_list *head = target->head;
+
        while (head != (struct target_list *)NULL) {
-               curr = head->target;
-               if ((curr != target) && (curr->state != TARGET_HALTED))
-                       retval += aarch64_halt(curr);
+               struct target *curr = head->target;
+               struct armv8_common *armv8 = target_to_armv8(curr);
+
+               /* open the gate for channel 0 to let HALT requests pass to the CTM */
+               if (curr->smp)
+                       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                                       armv8->cti_base + CTI_GATE, CTI_CHNL(0));
+               if (retval != ERROR_OK)
+                       break;
+
                head = head->next;
        }
+
+       /* halt the target PE */
+       if (retval == ERROR_OK)
+               retval = aarch64_halt(target);
+
        return retval;
 }
 
@@ -882,51 +408,23 @@ static int aarch64_halt(struct target *target)
        uint32_t dscr;
        struct armv8_common *armv8 = target_to_armv8(target);
 
-       /* enable CTI*/
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_CTR, 1);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_GATE, 3);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_OUTEN0, 1);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_OUTEN1, 2);
-       if (retval != ERROR_OK)
-               return retval;
-
        /*
         * add HDE in halting debug mode
         */
        retval = mem_ap_read_atomic_u32(armv8->debug_ap,
                        armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+       if (retval == ERROR_OK)
+               retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                               armv8->debug_base + CPUV8_DBG_DSCR, dscr | DSCR_HDE);
        if (retval != ERROR_OK)
                return retval;
 
+       /* trigger an event on channel 0, this outputs a halt request to the PE */
        retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->debug_base + CPUV8_DBG_DSCR, dscr | DSCR_HDE);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_APPPULSE, 1);
-       if (retval != ERROR_OK)
-               return retval;
-
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_INACK, 1);
+                       armv8->cti_base + CTI_APPPULSE, CTI_CHNL(0));
        if (retval != ERROR_OK)
                return retval;
 
-
        long long then = timeval_ms();
        for (;; ) {
                retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -1005,24 +503,10 @@ static int aarch64_internal_restore(struct target *target, int current,
        /* registers are now invalid */
        register_cache_invalidate(arm->core_cache);
 
-#if 0
-       /* the front-end may request us not to handle breakpoints */
-       if (handle_breakpoints) {
-               /* Single step past breakpoint at current address */
-               breakpoint = breakpoint_find(target, resume_pc);
-               if (breakpoint) {
-                       LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
-                       cortex_m3_unset_breakpoint(target, breakpoint);
-                       cortex_m3_single_step_core(target);
-                       cortex_m3_set_breakpoint(target, breakpoint);
-               }
-       }
-#endif
-
        return retval;
 }
 
-static int aarch64_internal_restart(struct target *target)
+static int aarch64_internal_restart(struct target *target, bool slave_pe)
 {
        struct armv8_common *armv8 = target_to_armv8(target);
        struct arm *arm = &armv8->arm;
@@ -1044,22 +528,39 @@ static int aarch64_internal_restart(struct target *target)
        if ((dscr & DSCR_ITE) == 0)
                LOG_ERROR("DSCR InstrCompl must be set before leaving debug!");
 
+       /* make sure to acknowledge the halt event before resuming */
        retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_APPPULSE, 2);
+                       armv8->cti_base + CTI_INACK, CTI_TRIG(HALT));
+
+       /*
+        * open the CTI gate for channel 1 so that the restart events
+        * get passed along to all PEs
+        */
+       if (retval == ERROR_OK)
+               retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                               armv8->cti_base + CTI_GATE, CTI_CHNL(1));
        if (retval != ERROR_OK)
                return retval;
 
-       long long then = timeval_ms();
-       for (;; ) {
-               retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                               armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+       if (!slave_pe) {
+               /* trigger an event on channel 1, generates a restart request to the PE */
+               retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                               armv8->cti_base + CTI_APPPULSE, CTI_CHNL(1));
                if (retval != ERROR_OK)
                        return retval;
-               if ((dscr & DSCR_HDE) != 0)
-                       break;
-               if (timeval_ms() > then + 1000) {
-                       LOG_ERROR("Timeout waiting for resume");
-                       return ERROR_FAIL;
+
+               long long then = timeval_ms();
+               for (;; ) {
+                       retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+                                       armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       if ((dscr & DSCR_HDE) != 0)
+                               break;
+                       if (timeval_ms() > then + 1000) {
+                               LOG_ERROR("Timeout waiting for resume");
+                               return ERROR_FAIL;
+                       }
                }
        }
 
@@ -1085,7 +586,7 @@ static int aarch64_restore_smp(struct target *target, int handle_breakpoints)
                        /*  resume current address , not in step mode */
                        retval += aarch64_internal_restore(curr, 1, &address,
                                        handle_breakpoints, 0);
-                       retval += aarch64_internal_restart(curr);
+                       retval += aarch64_internal_restart(curr, true);
                }
                head = head->next;
 
@@ -1116,7 +617,7 @@ static int aarch64_resume(struct target *target, int current,
                if (retval != ERROR_OK)
                        return retval;
        }
-       aarch64_internal_restart(target);
+       aarch64_internal_restart(target, false);
 
        if (!debug_execution) {
                target->state = TARGET_RUNNING;
@@ -1191,10 +692,14 @@ static int aarch64_post_debug_entry(struct target *target)
        struct armv8_common *armv8 = &aarch64->armv8_common;
        int retval;
 
+       /* clear sticky errors */
        mem_ap_write_atomic_u32(armv8->debug_ap,
-                                   armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
+                                   armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+
        switch (armv8->arm.core_mode) {
                case ARMV8_64_EL0T:
+                       dpmv8_modeswitch(&armv8->dpm, ARMV8_64_EL1T);
+                       /* fall through */
                case ARMV8_64_EL1T:
                case ARMV8_64_EL1H:
                        retval = armv8->arm.mrs(target, 3, /*op 0*/
@@ -1222,14 +727,27 @@ static int aarch64_post_debug_entry(struct target *target)
                        if (retval != ERROR_OK)
                                return retval;
                break;
+
+               case ARM_MODE_SVC:
+                       retval = armv8->arm.mrc(target, 15, 0, 0, 1, 0, &aarch64->system_control_reg);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       break;
+
                default:
-                       LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+                       LOG_INFO("cannot read system control register in this mode");
+                       break;
        }
+
+       dpmv8_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+
        LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
        aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
-       if (armv8->armv8_mmu.armv8_cache.ctype == -1)
-               armv8_identify_cache(target);
+       if (armv8->armv8_mmu.armv8_cache.info == -1) {
+               armv8_identify_cache(armv8);
+               armv8_read_mpidr(armv8);
+       }
 
        armv8->armv8_mmu.mmu_enabled =
                        (aarch64->system_control_reg & 0x1U) ? 1 : 0;
@@ -1241,12 +759,34 @@ static int aarch64_post_debug_entry(struct target *target)
        return ERROR_OK;
 }
 
+static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
+{
+       struct armv8_common *armv8 = target_to_armv8(target);
+       uint32_t dscr;
+
+       /* Read DSCR */
+       int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+                       armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+       if (ERROR_OK != retval)
+               return retval;
+
+       /* clear bitfield */
+       dscr &= ~bit_mask;
+       /* put new value */
+       dscr |= value & bit_mask;
+
+       /* write new DSCR */
+       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+                       armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+       return retval;
+}
+
 static int aarch64_step(struct target *target, int current, target_addr_t address,
        int handle_breakpoints)
 {
        struct armv8_common *armv8 = target_to_armv8(target);
        int retval;
-       uint32_t tmp;
+       uint32_t edecr;
 
        if (target->state != TARGET_HALTED) {
                LOG_WARNING("target not halted");
@@ -1254,25 +794,31 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
        }
 
        retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                       armv8->debug_base + CPUV8_DBG_EDECR, &tmp);
+                       armv8->debug_base + CPUV8_DBG_EDECR, &edecr);
        if (retval != ERROR_OK)
                return retval;
 
+       /* make sure EDECR.SS is not set when restoring the register */
+       edecr &= ~0x4;
+
+       /* set EDECR.SS to enter hardware step mode */
        retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->debug_base + CPUV8_DBG_EDECR, (tmp|0x4));
+                       armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
        if (retval != ERROR_OK)
                return retval;
 
-       target->debug_reason = DBG_REASON_SINGLESTEP;
+       /* disable interrupts while stepping */
+       retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
+       if (retval != ERROR_OK)
+               return ERROR_OK;
+
+       /* resume the target */
        retval = aarch64_resume(target, current, address, 0, 0);
        if (retval != ERROR_OK)
                return retval;
 
        long long then = timeval_ms();
        while (target->state != TARGET_HALTED) {
-               mem_ap_read_atomic_u32(armv8->debug_ap,
-                       armv8->debug_base + CPUV8_DBG_EDESR, &tmp);
-               LOG_DEBUG("DESR = %#x", tmp);
                retval = aarch64_poll(target);
                if (retval != ERROR_OK)
                        return retval;
@@ -1282,14 +828,16 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
                }
        }
 
+       /* restore EDECR */
        retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->debug_base + CPUV8_DBG_EDECR, (tmp&(~0x4)));
+                       armv8->debug_base + CPUV8_DBG_EDECR, edecr);
        if (retval != ERROR_OK)
                return retval;
 
-       target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-       if (target->state == TARGET_HALTED)
-               LOG_DEBUG("target stepped");
+       /* restore interrupts */
+       retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
+       if (retval != ERROR_OK)
+               return ERROR_OK;
 
        return ERROR_OK;
 }
@@ -1322,7 +870,6 @@ static int aarch64_set_breakpoint(struct target *target,
        struct aarch64_common *aarch64 = target_to_aarch64(target);
        struct armv8_common *armv8 = &aarch64->armv8_common;
        struct aarch64_brp *brp_list = aarch64->brp_list;
-       uint32_t dscr;
 
        if (breakpoint->set) {
                LOG_WARNING("breakpoint already set");
@@ -1371,27 +918,38 @@ static int aarch64_set_breakpoint(struct target *target,
 
        } else if (breakpoint->type == BKPT_SOFT) {
                uint8_t code[4];
-               buf_set_u32(code, 0, 32, ARMV8_BKPT(0x11));
+
+               buf_set_u32(code, 0, 32, ARMV8_HLT(0x11));
                retval = target_read_memory(target,
                                breakpoint->address & 0xFFFFFFFFFFFFFFFE,
                                breakpoint->length, 1,
                                breakpoint->orig_instr);
                if (retval != ERROR_OK)
                        return retval;
+
+               armv8_cache_d_inner_flush_virt(armv8,
+                               breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+                               breakpoint->length);
+
                retval = target_write_memory(target,
                                breakpoint->address & 0xFFFFFFFFFFFFFFFE,
                                breakpoint->length, 1, code);
                if (retval != ERROR_OK)
                        return retval;
+
+               armv8_cache_d_inner_flush_virt(armv8,
+                               breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+                               breakpoint->length);
+
+               armv8_cache_i_inner_inval_virt(armv8,
+                               breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+                               breakpoint->length);
+
                breakpoint->set = 0x11; /* Any nice value but 0 */
        }
 
-       retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-                                       armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
        /* Ensure that halting debug mode is enable */
-       dscr = dscr | DSCR_HDE;
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                                        armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+       retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
        if (retval != ERROR_OK) {
                LOG_DEBUG("Failed to set DSCR.HDE");
                return retval;
@@ -1638,6 +1196,11 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
                }
        } else {
                /* restore original instruction (kept in target endianness) */
+
+               armv8_cache_d_inner_flush_virt(armv8,
+                               breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+                               breakpoint->length);
+
                if (breakpoint->length == 4) {
                        retval = target_write_memory(target,
                                        breakpoint->address & 0xFFFFFFFFFFFFFFFE,
@@ -1651,6 +1214,14 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
                        if (retval != ERROR_OK)
                                return retval;
                }
+
+               armv8_cache_d_inner_flush_virt(armv8,
+                               breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+                               breakpoint->length);
+
+               armv8_cache_i_inner_inval_virt(armv8,
+                               breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+                               breakpoint->length);
        }
        breakpoint->set = 0;
 
@@ -1793,6 +1364,7 @@ static int aarch64_write_apb_ap_memory(struct target *target,
        /* write memory through APB-AP */
        int retval = ERROR_COMMAND_SYNTAX_ERROR;
        struct armv8_common *armv8 = target_to_armv8(target);
+       struct arm_dpm *dpm = &armv8->dpm;
        struct arm *arm = &armv8->arm;
        int total_bytes = count * size;
        int total_u32;
@@ -1877,17 +1449,15 @@ static int aarch64_write_apb_ap_memory(struct target *target,
        if (arm->core_state == ARM_STATE_AARCH64) {
                /* Write X0 with value 'address' using write procedure */
                /* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
-               retval += aarch64_write_dcc_64(armv8, address & ~0x3ULL);
                /* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
-               retval += aarch64_exec_opcode(target,
-                               ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+               retval = dpm->instr_write_data_dcc_64(dpm,
+                               ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
        } else {
                /* Write R0 with value 'address' using write procedure */
                /* Step 1.a+b - Write the address for read access into DBGDTRRX */
-               retval += aarch64_write_dcc(armv8, address & ~0x3ULL);
                /* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-               retval += aarch64_exec_opcode(target,
-                               T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), &dscr);
+               dpm->instr_write_data_dcc(dpm,
+                               T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL);
 
        }
        /* Step 1.d   - Change DCC to memory mode */
@@ -1948,6 +1518,7 @@ static int aarch64_read_apb_ap_memory(struct target *target,
        /* read memory through APB-AP */
        int retval = ERROR_COMMAND_SYNTAX_ERROR;
        struct armv8_common *armv8 = target_to_armv8(target);
+       struct arm_dpm *dpm = &armv8->dpm;
        struct arm *arm = &armv8->arm;
        int total_bytes = count * size;
        int total_u32;
@@ -1998,11 +1569,11 @@ static int aarch64_read_apb_ap_memory(struct target *target,
        if (arm->core_state == ARM_STATE_AARCH64) {
                /* Write X0 with value 'address' using write procedure */
                /* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
-               retval += aarch64_write_dcc_64(armv8, address & ~0x3ULL);
                /* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
-               retval += aarch64_exec_opcode(target, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+               retval += dpm->instr_write_data_dcc_64(dpm,
+                               ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
                /* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
-               retval += aarch64_exec_opcode(target, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+               retval += dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0));
                /* Step 1.e - Change DCC to memory mode */
                dscr = dscr | DSCR_MA;
                retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
@@ -2013,13 +1584,11 @@ static int aarch64_read_apb_ap_memory(struct target *target,
        } else {
                /* Write R0 with value 'address' using write procedure */
                /* Step 1.a+b - Write the address for read access into DBGDTRRXint */
-               retval += aarch64_write_dcc(armv8, address & ~0x3ULL);
                /* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-               retval += aarch64_exec_opcode(target,
-                               T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), &dscr);
+               retval += dpm->instr_write_data_dcc(dpm,
+                               T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL);
                /* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
-               retval += aarch64_exec_opcode(target,
-                               T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)), &dscr);
+               retval += dpm->instr_execute(dpm, T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)));
                /* Step 1.e - Change DCC to memory mode */
                dscr = dscr | DSCR_MA;
                retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
@@ -2113,26 +1682,16 @@ static int aarch64_read_phys_memory(struct target *target,
        target_addr_t address, uint32_t size,
        uint32_t count, uint8_t *buffer)
 {
-       struct armv8_common *armv8 = target_to_armv8(target);
        int retval = ERROR_COMMAND_SYNTAX_ERROR;
-       struct adiv5_dap *swjdp = armv8->arm.dap;
-       uint8_t apsel = swjdp->apsel;
        LOG_DEBUG("Reading memory at real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32,
                address, size, count);
 
        if (count && buffer) {
-
-               if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-
-                       /* read memory through AHB-AP */
-                       retval = mem_ap_read_buf(armv8->memory_ap, buffer, size, count, address);
-               } else {
-                       /* read memory through APB-AP */
-                       retval = aarch64_mmu_modify(target, 0);
-                       if (retval != ERROR_OK)
-                               return retval;
-                       retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
-               }
+               /* read memory through APB-AP */
+               retval = aarch64_mmu_modify(target, 0);
+               if (retval != ERROR_OK)
+                       return retval;
+               retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
        }
        return retval;
 }
@@ -2141,11 +1700,7 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
        uint32_t size, uint32_t count, uint8_t *buffer)
 {
        int mmu_enabled = 0;
-       target_addr_t virt, phys;
        int retval;
-       struct armv8_common *armv8 = target_to_armv8(target);
-       struct adiv5_dap *swjdp = armv8->arm.dap;
-       uint8_t apsel = swjdp->apsel;
 
        /* aarch64 handles unaligned memory access */
        LOG_DEBUG("Reading memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
@@ -2156,116 +1711,33 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
        if (retval != ERROR_OK)
                return retval;
 
-       if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-               if (mmu_enabled) {
-                       virt = address;
-                       retval = aarch64_virt2phys(target, virt, &phys);
-                       if (retval != ERROR_OK)
-                               return retval;
-
-                       LOG_DEBUG("Reading at virtual address. Translating v:0x%" TARGET_PRIxADDR " to r:0x%" TARGET_PRIxADDR,
-                                 virt, phys);
-                       address = phys;
-               }
-               retval = aarch64_read_phys_memory(target, address, size, count,
-                                                 buffer);
-       } else {
-               if (mmu_enabled) {
-                       retval = aarch64_check_address(target, address);
-                       if (retval != ERROR_OK)
-                               return retval;
-                       /* enable MMU as we could have disabled it for phys
-                          access */
-                       retval = aarch64_mmu_modify(target, 1);
-                       if (retval != ERROR_OK)
-                               return retval;
-               }
-               retval = aarch64_read_apb_ap_memory(target, address, size,
-                                                   count, buffer);
+       if (mmu_enabled) {
+               retval = aarch64_check_address(target, address);
+               if (retval != ERROR_OK)
+                       return retval;
+               /* enable MMU as we could have disabled it for phys access */
+               retval = aarch64_mmu_modify(target, 1);
+               if (retval != ERROR_OK)
+                       return retval;
        }
-       return retval;
+       return aarch64_read_apb_ap_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_write_phys_memory(struct target *target,
        target_addr_t address, uint32_t size,
        uint32_t count, const uint8_t *buffer)
 {
-       struct armv8_common *armv8 = target_to_armv8(target);
-       struct adiv5_dap *swjdp = armv8->arm.dap;
        int retval = ERROR_COMMAND_SYNTAX_ERROR;
-       uint8_t apsel = swjdp->apsel;
 
        LOG_DEBUG("Writing memory to real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
                size, count);
 
        if (count && buffer) {
-
-               if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-
-                       /* write memory through AHB-AP */
-                       retval = mem_ap_write_buf(armv8->memory_ap, buffer, size, count, address);
-               } else {
-
-                       /* write memory through APB-AP */
-                       retval = aarch64_mmu_modify(target, 0);
-                       if (retval != ERROR_OK)
-                               return retval;
-                       return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
-               }
-       }
-
-       /* REVISIT this op is generic ARMv7-A/R stuff */
-       if (retval == ERROR_OK && target->state == TARGET_HALTED) {
-               struct arm_dpm *dpm = armv8->arm.dpm;
-
-               retval = dpm->prepare(dpm);
+               /* write memory through APB-AP */
+               retval = aarch64_mmu_modify(target, 0);
                if (retval != ERROR_OK)
                        return retval;
-
-               /* The Cache handling will NOT work with MMU active, the
-                * wrong addresses will be invalidated!
-                *
-                * For both ICache and DCache, walk all cache lines in the
-                * address range. Cortex-A has fixed 64 byte line length.
-                *
-                * REVISIT per ARMv7, these may trigger watchpoints ...
-                */
-
-               /* invalidate I-Cache */
-               if (armv8->armv8_mmu.armv8_cache.i_cache_enabled) {
-                       /* ICIMVAU - Invalidate Cache single entry
-                        * with MVA to PoU
-                        *      MCR p15, 0, r0, c7, c5, 1
-                        */
-                       for (uint32_t cacheline = 0;
-                               cacheline < size * count;
-                               cacheline += 64) {
-                               retval = dpm->instr_write_data_r0(dpm,
-                                               ARMV8_MSR_GP(SYSTEM_ICIVAU, 0),
-                                               address + cacheline);
-                               if (retval != ERROR_OK)
-                                       return retval;
-                       }
-               }
-
-               /* invalidate D-Cache */
-               if (armv8->armv8_mmu.armv8_cache.d_u_cache_enabled) {
-                       /* DCIMVAC - Invalidate data Cache line
-                        * with MVA to PoC
-                        *      MCR p15, 0, r0, c7, c6, 1
-                        */
-                       for (uint32_t cacheline = 0;
-                               cacheline < size * count;
-                               cacheline += 64) {
-                               retval = dpm->instr_write_data_r0(dpm,
-                                               ARMV8_MSR_GP(SYSTEM_DCCVAU, 0),
-                                               address + cacheline);
-                               if (retval != ERROR_OK)
-                                       return retval;
-                       }
-               }
-
-               /* (void) */ dpm->finish(dpm);
+               return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
        }
 
        return retval;
@@ -2275,11 +1747,7 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
        uint32_t size, uint32_t count, const uint8_t *buffer)
 {
        int mmu_enabled = 0;
-       target_addr_t virt, phys;
        int retval;
-       struct armv8_common *armv8 = target_to_armv8(target);
-       struct adiv5_dap *swjdp = armv8->arm.dap;
-       uint8_t apsel = swjdp->apsel;
 
        /* aarch64 handles unaligned memory access */
        LOG_DEBUG("Writing memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32
@@ -2290,34 +1758,16 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
        if (retval != ERROR_OK)
                return retval;
 
-       if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-               LOG_DEBUG("Writing memory to address 0x%" TARGET_PRIxADDR "; size %"
-                         PRId32 "; count %" PRId32, address, size, count);
-               if (mmu_enabled) {
-                       virt = address;
-                       retval = aarch64_virt2phys(target, virt, &phys);
-                       if (retval != ERROR_OK)
-                               return retval;
-
-                       LOG_DEBUG("Writing to virtual address. Translating v:0x%"
-                                 TARGET_PRIxADDR " to r:0x%" TARGET_PRIxADDR, virt, phys);
-                       address = phys;
-               }
-               retval = aarch64_write_phys_memory(target, address, size,
-                               count, buffer);
-       } else {
-               if (mmu_enabled) {
-                       retval = aarch64_check_address(target, address);
-                       if (retval != ERROR_OK)
-                               return retval;
-                       /* enable MMU as we could have disabled it for phys access */
-                       retval = aarch64_mmu_modify(target, 1);
-                       if (retval != ERROR_OK)
-                               return retval;
-               }
-               retval = aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+       if (mmu_enabled) {
+               retval = aarch64_check_address(target, address);
+               if (retval != ERROR_OK)
+                       return retval;
+               /* enable MMU as we could have disabled it for phys access */
+               retval = aarch64_mmu_modify(target, 1);
+               if (retval != ERROR_OK)
+                       return retval;
        }
-       return retval;
+       return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_handle_target_request(void *priv)
@@ -2386,20 +1836,6 @@ static int aarch64_examine_first(struct target *target)
 
        armv8->debug_ap->memaccess_tck = 80;
 
-       /* Search for the AHB-AB */
-       armv8->memory_ap_available = false;
-       retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv8->memory_ap);
-       if (retval == ERROR_OK) {
-               retval = mem_ap_init(armv8->memory_ap);
-               if (retval == ERROR_OK)
-                       armv8->memory_ap_available = true;
-       }
-       if (retval != ERROR_OK) {
-               /* AHB-AP not found or unavailable - use the CPU */
-               LOG_DEBUG("No AHB-AP available for memory access");
-       }
-
-
        if (!target->dbgbase_set) {
                uint32_t dbgbase;
                /* Get ROM Table base */
@@ -2472,12 +1908,6 @@ static int aarch64_examine_first(struct target *target)
        } else
                armv8->cti_base = target->ctibase;
 
-       retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-                       armv8->cti_base + CTI_UNLOCK , 0xC5ACCE55);
-       if (retval != ERROR_OK)
-               return retval;
-
-
        armv8->arm.core_type = ARM_MODE_MON;
        retval = aarch64_dpm_setup(aarch64, debug);
        if (retval != ERROR_OK)
@@ -2590,22 +2020,7 @@ static int aarch64_mmu(struct target *target, int *enabled)
 static int aarch64_virt2phys(struct target *target, target_addr_t virt,
                             target_addr_t *phys)
 {
-       int retval = ERROR_FAIL;
-       struct armv8_common *armv8 = target_to_armv8(target);
-       struct adiv5_dap *swjdp = armv8->arm.dap;
-       uint8_t apsel = swjdp->apsel;
-       if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-               uint32_t ret;
-               retval = armv8_mmu_translate_va(target,
-                               virt, &ret);
-               if (retval != ERROR_OK)
-                       goto done;
-               *phys = ret;
-       } else {
-               LOG_ERROR("AAR64 processor not support translate va to pa");
-       }
-done:
-       return retval;
+       return armv8_mmu_translate_va_pa(target, virt, phys, 1);
 }
 
 COMMAND_HANDLER(aarch64_handle_cache_info_command)

Linking to existing account procedure

If you already have an account and want to add another login method you MUST first sign in with your existing account and then change URL to read https://review.openocd.org/login/?link to get to this page again but this time it'll work for linking. Thank you.

SSH host keys fingerprints

1024 SHA256:YKx8b7u5ZWdcbp7/4AeXNaqElP49m6QrwfXaqQGJAOk gerrit-code-review@openocd.zylin.com (DSA)
384 SHA256:jHIbSQa4REvwCFG4cq5LBlBLxmxSqelQPem/EXIrxjk gerrit-code-review@openocd.org (ECDSA)
521 SHA256:UAOPYkU9Fjtcao0Ul/Rrlnj/OsQvt+pgdYSZ4jOYdgs gerrit-code-review@openocd.org (ECDSA)
256 SHA256:A13M5QlnozFOvTllybRZH6vm7iSt0XLxbA48yfc2yfY gerrit-code-review@openocd.org (ECDSA)
256 SHA256:spYMBqEYoAOtK7yZBrcwE8ZpYt6b68Cfh9yEVetvbXg gerrit-code-review@openocd.org (ED25519)
+--[ED25519 256]--+
|=..              |
|+o..   .         |
|*.o   . .        |
|+B . . .         |
|Bo. = o S        |
|Oo.+ + =         |
|oB=.* = . o      |
| =+=.+   + E     |
|. .=o   . o      |
+----[SHA256]-----+
2048 SHA256:0Onrb7/PHjpo6iVZ7xQX2riKN83FJ3KGU0TvI0TaFG4 gerrit-code-review@openocd.zylin.com (RSA)