]> git.sur5r.net Git - openocd/commitdiff
aarch64: add basic Aarch32 support
authorMatthias Welwarsky <matthias.welwarsky@sysgo.com>
Thu, 15 Sep 2016 07:13:51 +0000 (09:13 +0200)
committerMatthias Welwarsky <matthias.welwarsky@sysgo.com>
Fri, 10 Feb 2017 13:04:16 +0000 (14:04 +0100)
Add database for common, equivalent opcodes for Aarch32 and
Aarch64 execution states

Revisit all functions that access Aarch64 specific registers
or use Aarch64 opcodes and rewrite them to act depending on
current state of the core.

Add core register access functions for Aarch32 state

Add function to determine the core execution state without
reading DSPSR.

Change-Id: I345e9f6d682fb4ba454e4b1d16bb5e1b27570691
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
src/target/Makefile.am
src/target/aarch64.c
src/target/armv8.c
src/target/armv8.h
src/target/armv8_dpm.c
src/target/armv8_opcodes.c [new file with mode: 0644]
src/target/armv8_opcodes.h

index 0021f44d543802231d38eb03f56d33e222c02948..e936d3f404bd447ebfe20ea9873140b98975bf1a 100644 (file)
@@ -75,6 +75,7 @@ ARMV7_SRC = \
 
 ARMV8_SRC = \
        %D%/armv8_dpm.c \
+       %D%/armv8_opcodes.c \
        %D%/aarch64.c \
        %D%/armv8.c \
        %D%/armv8_cache.c
index 8d74dbdbc37f909db923c208e135df355638f25f..824a042de917416a3bbc2ad0006c31f9ca72b7cf 100644 (file)
@@ -90,7 +90,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;
@@ -531,6 +534,7 @@ 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;
 
@@ -539,9 +543,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
                return retval;
 
        retval = aarch64_exec_opcode(
-                       a8->armv8_common.arm.target,
-                       ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
-                       &dscr);
+                       a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, READ_REG_DTRRX), &dscr);
        if (retval != ERROR_OK)
                return retval;
 
@@ -584,12 +586,11 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
 {
        struct target *target = dpm->arm->target;
+       struct armv8_common *armv8 = target_to_armv8(target);
        uint32_t dscr = DSCR_ITE;
 
        /* "Prefetch flush" after modifying execution status in CPSR */
-       return aarch64_exec_opcode(target,
-                       DSB_SY,
-                       &dscr);
+       return aarch64_exec_opcode(target, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), &dscr);
 }
 
 static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
@@ -645,9 +646,7 @@ static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
 
        /* 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);
+                       a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, WRITE_REG_DTRTX), &dscr);
        if (retval != ERROR_OK)
                return retval;
 
@@ -999,20 +998,6 @@ 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;
 }
 
@@ -1202,8 +1187,10 @@ 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:
                case ARMV8_64_EL1T:
@@ -1234,8 +1221,12 @@ static int aarch64_post_debug_entry(struct target *target)
                                return retval;
                break;
                default:
-                       LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+                       retval = armv8->arm.mrc(target, 15, 0, 0, 1, 0, &aarch64->system_control_reg);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       break;
        }
+
        LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
        aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
index c351dfdfff4e7b4ded9de384506f833d8ed7becf..306a06e7a0b14ef5cc27ee7e93e81e62a8b4a93b 100644 (file)
@@ -38,7 +38,7 @@
 #include "target_type.h"
 
 static const char * const armv8_state_strings[] = {
-       "ARM", "Thumb", "Jazelle", "ThumbEE", "ARM64",
+       "AArch32", "Thumb", "Jazelle", "ThumbEE", "AArch64",
 };
 
 static const struct {
@@ -52,6 +52,30 @@ static const struct {
 } armv8_mode_data[] = {
        /* These special modes are currently only supported
         * by ARMv6M and ARMv7M profiles */
+       {
+               .name = "USR",
+               .psr = ARM_MODE_USR,
+       },
+       {
+               .name = "FIQ",
+               .psr = ARM_MODE_FIQ,
+       },
+       {
+               .name = "IRQ",
+               .psr = ARM_MODE_IRQ,
+       },
+       {
+               .name = "SVC",
+               .psr = ARM_MODE_SVC,
+       },
+       {
+               .name = "MON",
+               .psr = ARM_MODE_MON,
+       },
+       {
+               .name = "ABT",
+               .psr = ARM_MODE_ABT,
+       },
        {
                .name = "EL0T",
                .psr = ARMV8_64_EL0T,
@@ -260,9 +284,59 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
                armv8_state_strings[arm->core_state]);
 }
 
+static void armv8_show_fault_registers32(struct armv8_common *armv8)
+{
+       uint32_t dfsr, ifsr, dfar, ifar;
+       struct arm_dpm *dpm = armv8->arm.dpm;
+       int retval;
+
+       retval = dpm->prepare(dpm);
+       if (retval != ERROR_OK)
+               return;
+
+       /* ARMV4_5_MRC(cpnum, op1, r0, CRn, CRm, op2) */
+
+       /* c5/c0 - {data, instruction} fault status registers */
+       retval = dpm->instr_read_data_r0(dpm,
+                       T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 0)),
+                       &dfsr);
+       if (retval != ERROR_OK)
+               goto done;
+
+       retval = dpm->instr_read_data_r0(dpm,
+                       T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 1)),
+                       &ifsr);
+       if (retval != ERROR_OK)
+               goto done;
+
+       /* c6/c0 - {data, instruction} fault address registers */
+       retval = dpm->instr_read_data_r0(dpm,
+                       T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 0)),
+                       &dfar);
+       if (retval != ERROR_OK)
+               goto done;
+
+       retval = dpm->instr_read_data_r0(dpm,
+                       T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 2)),
+                       &ifar);
+       if (retval != ERROR_OK)
+               goto done;
+
+       LOG_USER("Data fault registers        DFSR: %8.8" PRIx32
+               ", DFAR: %8.8" PRIx32, dfsr, dfar);
+       LOG_USER("Instruction fault registers IFSR: %8.8" PRIx32
+               ", IFAR: %8.8" PRIx32, ifsr, ifar);
+
+done:
+       /* (void) */ dpm->finish(dpm);
+}
+
 static void armv8_show_fault_registers(struct target *target)
 {
-       /* TODO */
+       struct armv8_common *armv8 = target_to_armv8(target);
+
+       if (armv8->arm.core_state != ARM_STATE_AARCH64)
+               armv8_show_fault_registers32(armv8);
 }
 
 static uint8_t armv8_pa_size(uint32_t ps)
@@ -294,6 +368,45 @@ static uint8_t armv8_pa_size(uint32_t ps)
        return ret;
 }
 
+static int armv8_read_ttbcr32(struct target *target)
+{
+       struct armv8_common *armv8 = target_to_armv8(target);
+       struct arm_dpm *dpm = armv8->arm.dpm;
+       uint32_t ttbcr, ttbcr_n;
+       int retval = dpm->prepare(dpm);
+       if (retval != ERROR_OK)
+               goto done;
+       /*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
+       retval = dpm->instr_read_data_r0(dpm,
+                       T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 2, 0, 2)),
+                       &ttbcr);
+       if (retval != ERROR_OK)
+               goto done;
+
+       LOG_DEBUG("ttbcr %" PRIx32, ttbcr);
+
+       ttbcr_n = ttbcr & 0x7;
+       armv8->armv8_mmu.ttbcr = ttbcr;
+
+       /*
+        * ARM Architecture Reference Manual (ARMv7-A and ARMv7-Redition),
+        * document # ARM DDI 0406C
+        */
+       armv8->armv8_mmu.ttbr_range[0]  = 0xffffffff >> ttbcr_n;
+       armv8->armv8_mmu.ttbr_range[1] = 0xffffffff;
+       armv8->armv8_mmu.ttbr_mask[0] = 0xffffffff << (14 - ttbcr_n);
+       armv8->armv8_mmu.ttbr_mask[1] = 0xffffffff << 14;
+
+       LOG_DEBUG("ttbr1 %s, ttbr0_mask %" PRIx32 " ttbr1_mask %" PRIx32,
+                 (ttbcr_n != 0) ? "used" : "not used",
+                 armv8->armv8_mmu.ttbr_mask[0],
+                 armv8->armv8_mmu.ttbr_mask[1]);
+
+done:
+       dpm->finish(dpm);
+       return retval;
+}
+
 static int armv8_read_ttbcr(struct target *target)
 {
        struct armv8_common *armv8 = target_to_armv8(target);
@@ -528,14 +641,13 @@ static int armv8_read_mpidr(struct target *target)
        struct armv8_common *armv8 = target_to_armv8(target);
        struct arm_dpm *dpm = armv8->arm.dpm;
        uint32_t mpidr;
+
        retval = dpm->prepare(dpm);
        if (retval != ERROR_OK)
                goto done;
        /* MRC p15,0,<Rd>,c0,c0,5; read Multiprocessor ID register*/
 
-       retval = dpm->instr_read_data_r0(dpm,
-                       ARMV8_MRS(SYSTEM_MPIDR, 0),
-                       &mpidr);
+       retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
        if (retval != ERROR_OK)
                goto done;
        if (mpidr & 1<<31) {
@@ -566,18 +678,21 @@ int armv8_identify_cache(struct target *target)
        uint32_t cache_selected, clidr;
        uint32_t cache_i_reg, cache_d_reg;
        struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
-       armv8_read_ttbcr(target);
-       retval = dpm->prepare(dpm);
+       int is_aarch64 = armv8->arm.core_state == ARM_STATE_AARCH64;
 
+       retval = is_aarch64 ? armv8_read_ttbcr(target) : armv8_read_ttbcr32(target);
+       if (retval != ERROR_OK)
+               return retval;
+
+       retval = dpm->prepare(dpm);
        if (retval != ERROR_OK)
                goto done;
-       /*      retrieve CLIDR
-        *      mrc p15, 1, r0, c0, c0, 1               @ read clidr */
-       retval = dpm->instr_read_data_r0(dpm,
-                       ARMV8_MRS(SYSTEM_CLIDR, 0),
-                       &clidr);
+
+       /*      retrieve CLIDR */
+       retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CLIDR), &clidr);
        if (retval != ERROR_OK)
                goto done;
+
        clidr = (clidr & 0x7000000) >> 23;
        LOG_INFO("number of cache level %" PRIx32, (uint32_t)(clidr / 2));
        if ((clidr / 2) > 1) {
@@ -586,18 +701,13 @@ int armv8_identify_cache(struct target *target)
                LOG_ERROR("cache l2 present :not supported");
        }
        /*      retrieve selected cache*/
-       retval = dpm->instr_read_data_r0(dpm,
-                       ARMV8_MRS(SYSTEM_CSSELR, 0),
-                       &cache_selected);
+       retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CSSELR), &cache_selected);
        if (retval != ERROR_OK)
                goto done;
 
-
        /* select instruction cache
         *      [0]  : 1 instruction cache selection , 0 data cache selection */
-       retval = dpm->instr_write_data_r0(dpm,
-                       ARMV8_MRS(SYSTEM_CSSELR, 0),
-                       1);
+       retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 1);
        if (retval != ERROR_OK)
                goto done;
 
@@ -605,30 +715,21 @@ int armv8_identify_cache(struct target *target)
         * MRC P15,1,<RT>,C0, C0,0 ;on cortex A9 read CCSIDR
         * [2:0] line size      001 eight word per line
         * [27:13] NumSet 0x7f 16KB, 0xff 32Kbytes, 0x1ff 64Kbytes */
-       retval = dpm->instr_read_data_r0(dpm,
-                       ARMV8_MRS(SYSTEM_CCSIDR, 0),
-                       &cache_i_reg);
+       retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_i_reg);
        if (retval != ERROR_OK)
                goto done;
 
        /*      select data cache*/
-       retval = dpm->instr_write_data_r0(dpm,
-                       ARMV8_MRS(SYSTEM_CSSELR, 0),
-                       0);
+       retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 0);
        if (retval != ERROR_OK)
                goto done;
 
-       retval = dpm->instr_read_data_r0(dpm,
-                       ARMV8_MRS(SYSTEM_CCSIDR, 0),
-                       &cache_d_reg);
+       retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_d_reg);
        if (retval != ERROR_OK)
                goto done;
 
        /*      restore selected cache  */
-       dpm->instr_write_data_r0(dpm,
-               ARMV8_MRS(SYSTEM_CSSELR, 0),
-               cache_selected);
-
+       dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), cache_selected);
        if (retval != ERROR_OK)
                goto done;
        dpm->finish(dpm);
@@ -770,6 +871,7 @@ int armv8_arch_state(struct target *target)
 
        if (arm->core_mode == ARM_MODE_ABT)
                armv8_show_fault_registers(target);
+
        if (target->debug_reason == DBG_REASON_WATCHPOINT)
                LOG_USER("Watchpoint triggered at PC %#08x",
                        (unsigned) armv8->dpm.wp_pc);
index f6859b2993974dc7a2a4c2dee99f6e8211038bf8..fa3674b1447e752030e5e130447d2da91328706d 100644 (file)
@@ -116,6 +116,10 @@ struct armv8_mmu_common {
        uint64_t ttbr0_mask;/*  masked to be used  */
        uint32_t os_border;
 
+       uint32_t ttbcr;     /* cache for ttbcr register */
+       uint32_t ttbr_mask[2];
+       uint32_t ttbr_range[2];
+
        int (*read_physical_memory)(struct target *target, target_addr_t address,
                        uint32_t size, uint32_t count, uint8_t *buffer);
        struct armv8_cache_common armv8_cache;
@@ -133,6 +137,8 @@ struct armv8_common {
        uint32_t cti_base;
        struct adiv5_ap *debug_ap;
 
+       const uint32_t *opcodes;
+
        /* mdir */
        uint8_t multi_processor_system;
        uint8_t cluster_id;
@@ -144,7 +150,6 @@ struct armv8_common {
        uint32_t page_size;
        uint64_t ttbr_base;
 
-       /* cache specific to V7 Memory Management Unit compatible with v4_5*/
        struct armv8_mmu_common armv8_mmu;
 
        /* Direct processor core register read and writes */
index 8c0d45a3fc1733d595fe402d0c8cc0f4c3170c37..c3d5ec4def8b716966cf2f7e4d2b0ceac5927085 100644 (file)
@@ -164,6 +164,7 @@ static int dpmv8_msr(struct target *target, uint32_t op0,
  */
 int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 {
+       struct armv8_common *armv8 = (struct armv8_common *)dpm->arm->arch_info;
        int retval;
        uint32_t cpsr;
 
@@ -199,7 +200,7 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
        }
 
 
-       retval = dpm->instr_write_data_r0(dpm, ARMV8_MSR_DSPSR(0), cpsr);
+       retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_DSPSR), cpsr);
        if (retval != ERROR_OK)
                return retval;
 
@@ -209,6 +210,86 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
        return retval;
 }
 
+static int dpmv8_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+       uint32_t value;
+       int retval = ERROR_FAIL;
+       bool valid = true;
+
+       switch (regnum) {
+       case 0 ... 14:
+               /* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
+               retval = dpm->instr_read_data_dcc(dpm,
+                       T32_FMTITR(ARMV4_5_MCR(14, 0, regnum, 0, 5, 0)),
+                       &value);
+               break;
+               case ARMV8_R31:
+                       retval = dpm->instr_read_data_dcc(dpm,
+                               T32_FMTITR(ARMV4_5_MCR(14, 0, 13, 0, 5, 0)),
+                               &value);
+                               break;
+               case ARMV8_PC:
+                       retval = dpm->instr_read_data_r0(dpm,
+                               T32_FMTITR(ARMV8_MRC_DLR(0)),
+                               &value);
+                       break;
+               case ARMV8_xPSR:
+                       retval = dpm->instr_read_data_r0(dpm,
+                               T32_FMTITR(ARMV8_MRC_DSPSR(0)),
+                               &value);
+                       break;
+               default:
+                       LOG_DEBUG("READ: %s ignored", r->name);
+                       retval = ERROR_OK;
+                       value = 0xFFFFFFFF;
+                       valid = false;
+                       break;
+       }
+
+       if (retval == ERROR_OK) {
+               r->valid = valid;
+               r->dirty = false;
+               buf_set_u64(r->value, 0, 32, value);
+               LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
+       }
+       return retval;
+}
+
+static int dpmv8_write_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+       int retval;
+       uint64_t value = buf_get_u64(r->value, 0, 32);
+
+       switch (regnum) {
+               case 0 ... 14:
+                       /* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
+                       retval = dpm->instr_write_data_dcc(dpm,
+                                       T32_FMTITR(ARMV4_5_MRC(14, 0, regnum, 0, 5, 0)), value);
+                       break;
+               case ARMV8_PC:/* PC
+                        * read r0 from DCC; then "MOV pc, r0" */
+                       retval = dpm->instr_write_data_r0(dpm,
+                                       T32_FMTITR(ARMV8_MCR_DLR(0)), value);
+                       break;
+               case ARMV8_xPSR: /* CPSR */
+                       /* read r0 from DCC, then "MCR r0, DSPSR" */
+                       retval = dpm->instr_write_data_r0(dpm,
+                                       T32_FMTITR(ARMV8_MCR_DSPSR(0)), value);
+                       break;
+               default:
+                       retval = ERROR_OK;
+                       LOG_DEBUG("WRITE: %s ignored", r->name);
+                       break;
+       }
+
+       if (retval == ERROR_OK) {
+               r->dirty = false;
+               LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned) value);
+       }
+
+       return retval;
+}
+
 /* just read the register -- rely on the core mode being right */
 static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
@@ -222,20 +303,21 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
                                ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum),
                                &value_64);
                        break;
-               case 31:
+               case ARMV8_R31:
                        retval = dpm->instr_read_data_r0_64(dpm,
                                ARMV8_MOVFSP_64(0),
                                &value_64);
                        break;
-               case 32:
+               case ARMV8_PC:
                        retval = dpm->instr_read_data_r0_64(dpm,
                                ARMV8_MRS_DLR(0),
                                &value_64);
                        break;
-               case 33:
+               case ARMV8_xPSR:
                        retval = dpm->instr_read_data_r0(dpm,
                                ARMV8_MRS_DSPSR(0),
                                &value);
+                       break;
                default:
                        LOG_DEBUG("READ: %s fail", r->name);
                        break;
@@ -244,11 +326,13 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
        if (retval == ERROR_OK) {
                r->valid = true;
                r->dirty = false;
-               buf_set_u64(r->value, 0, 32, value_64);
-               if (r->size == 64)
+               if (r->size == 64) {
+                       buf_set_u64(r->value, 0, 64, value_64);
                        LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
-               else
+               } else {
+                       buf_set_u32(r->value, 0, 32, value);
                        LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
+               }
        }
        return retval;
 }
@@ -267,23 +351,24 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
                                ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
                                value_64);
                        break;
-               case 31:
+               case ARMV8_R31:
                        value_64 = buf_get_u64(r->value, 0, 64);
                        retval = dpm->instr_write_data_r0_64(dpm,
                                ARMV8_MOVTSP_64(0),
                                value_64);
                        break;
-               case 32:
+               case ARMV8_PC:
                        value_64 = buf_get_u64(r->value, 0, 64);
                        retval = dpm->instr_write_data_r0_64(dpm,
                                ARMV8_MSR_DLR(0),
                                value_64);
                        break;
-               case 33:
+               case ARMV8_xPSR:
                        value = buf_get_u32(r->value, 0, 32);
                        retval = dpm->instr_write_data_r0(dpm,
                                ARMV8_MSR_DSPSR(0),
                                value);
+                       break;
                default:
                        LOG_DEBUG("write: %s fail", r->name);
                        break;
@@ -301,6 +386,36 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
        return retval;
 }
 
+static inline enum arm_state dpm_get_core_state(uint32_t dscr)
+{
+       int el = (dscr >> 8) & 0x3;
+       int rw = (dscr >> 10) & 0xF;
+
+       LOG_DEBUG("EL:%i, RW:0x%x", el, rw);
+
+       /* DSCR.RW = 0b1111 - all EL are using AArch64 state */
+       if (rw == 0xF)
+               return ARM_STATE_AARCH64;
+
+       /* DSCR.RW = 0b1110 - all EL > 0 are using AArch64 state */
+       if (rw == 0xE && el > 0)
+               return ARM_STATE_AARCH64;
+
+       /* DSCR.RW = 0b110x - all EL > 1 are using Aarch64 state */
+       if ((rw & 0xE) == 0xC && el > 1)
+               return ARM_STATE_AARCH64;
+
+       /* DSCR.RW = 0b10xx - all EL > 2 are using Aarch64 state */
+       if ((rw & 0xC) == 0x8 && el > 2)
+               return ARM_STATE_AARCH64;
+
+       /* DSCR.RW = 0b0xxx - all EL are using AArch32 state */
+       if ((rw & 0x8) == 0)
+               return ARM_STATE_ARM;
+
+       return ARM_STATE_ARM;
+}
+
 /**
  * Read basic registers of the the current context:  R0 to R15, and CPSR;
  * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
@@ -311,7 +426,10 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 {
        struct arm *arm = dpm->arm;
+       struct armv8_common *armv8 = (struct armv8_common *)arm->arch_info;
+       enum arm_state core_state;
        uint32_t cpsr;
+
        int retval;
        struct reg *r;
 
@@ -319,16 +437,22 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
        if (retval != ERROR_OK)
                return retval;
 
+       core_state = dpm_get_core_state(dpm->dscr);
+
+       armv8_select_opcodes(armv8, core_state);
+
        /* read R0 first (it's used for scratch), then CPSR */
        r = arm->core_cache->reg_list + 0;
        if (!r->valid) {
-               retval = dpmv8_read_reg(dpm, r, 0);
+               retval = core_state == ARM_STATE_AARCH64 ?
+                                       dpmv8_read_reg(dpm, r, 0) : dpmv8_read_reg32(dpm, r, 0);
                if (retval != ERROR_OK)
                        goto fail;
        }
        r->dirty = true;
+
        /* read cpsr to r0 and get it back */
-       retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS_DSPSR(0), &cpsr);
+       retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
        if (retval != ERROR_OK)
                goto fail;
 
@@ -341,7 +465,9 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
                if (r->valid)
                        continue;
 
-               retval = dpmv8_read_reg(dpm, r, i);
+               retval = core_state == ARM_STATE_AARCH64 ?
+                                       dpmv8_read_reg(dpm, r, i) : dpmv8_read_reg32(dpm, r, i);
+
                if (retval != ERROR_OK)
                        goto fail;
        }
@@ -419,6 +545,7 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
        struct arm *arm = dpm->arm;
        struct reg_cache *cache = arm->core_cache;
        int retval;
+       bool is_aarch64 = arm->core_state == ARM_STATE_AARCH64;
 
        retval = dpm->prepare(dpm);
        if (retval != ERROR_OK)
@@ -480,9 +607,8 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
                r = cache->reg_list[i].arch_info;
                regnum = r->num;
 
-               retval = dpmv8_write_reg(dpm,
-                               &cache->reg_list[i],
-                               regnum);
+               retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[i], regnum)
+                               : dpmv8_write_reg32(dpm, &cache->reg_list[i], regnum);
                if (retval != ERROR_OK)
                        goto done;
        }
@@ -497,13 +623,15 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
                goto done;
        arm->cpsr->dirty = false;
 
-       retval = dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2));
+       retval = is_aarch64 ? dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2))
+                       : dpmv8_write_reg32(dpm, arm->pc, (arm->core_cache->num_regs - 2));
        if (retval != ERROR_OK)
                goto done;
        arm->pc->dirty = false;
 
        /* flush R0 -- it's *very* dirty by now */
-       retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
+       retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[0], 0)
+                       : dpmv8_write_reg32(dpm, &cache->reg_list[0], 0);
        if (retval != ERROR_OK)
                goto done;
        cache->reg_list[0].dirty = false;
@@ -538,7 +666,8 @@ static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
        if (retval != ERROR_OK)
                return retval;
 
-       retval = dpmv8_read_reg(dpm, r, regnum);
+       retval = arm->core_state == ARM_STATE_AARCH64 ?
+                       dpmv8_read_reg(dpm, r, regnum) : dpmv8_read_reg32(dpm, r, regnum);
        if (retval != ERROR_OK)
                goto fail;
 
@@ -566,7 +695,9 @@ static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
        if (retval != ERROR_OK)
                return retval;
 
-       retval = dpmv8_write_reg(dpm, r, regnum);
+       retval = arm->core_state == ARM_STATE_AARCH64 ?
+                       dpmv8_write_reg(dpm, r, regnum) : dpmv8_write_reg32(dpm, r, regnum);
+
        /* always clean up, regardless of error */
 
        /* (void) */ dpm->finish(dpm);
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
new file mode 100644 (file)
index 0000000..78b60e0
--- /dev/null
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2015 by Matthias Welwarsky <matthias.welwarsky@sysgo.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "armv8.h"
+#include "armv8_opcodes.h"
+
+static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
+               [READ_REG_CLIDR]        = ARMV8_MRS(SYSTEM_CLIDR, 0),
+               [READ_REG_CSSELR]       = ARMV8_MRS(SYSTEM_CSSELR, 0),
+               [READ_REG_CCSIDR]       = ARMV8_MRS(SYSTEM_CCSIDR, 0),
+               [WRITE_REG_CSSELR]      = ARMV8_MSR_GP(SYSTEM_CSSELR, 0),
+               [READ_REG_MPIDR]        = ARMV8_MRS(SYSTEM_MPIDR, 0),
+               [READ_REG_DTRRX]        = ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
+               [WRITE_REG_DTRTX]       = ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),
+               [WRITE_REG_DSPSR]       = ARMV8_MSR_DSPSR(0),
+               [READ_REG_DSPSR]        = ARMV8_MRS_DSPSR(0),
+               [ARMV8_OPC_DSB_SY]  = ARMV8_DSB_SY,
+};
+
+static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
+               [READ_REG_CLIDR]        = T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 1)),
+               [READ_REG_CSSELR]       = T32_FMTITR(ARMV4_5_MRC(15, 2, 0, 0, 0, 0)),
+               [READ_REG_CCSIDR]       = T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 0)),
+               [WRITE_REG_CSSELR]      = T32_FMTITR(ARMV4_5_MCR(15, 2, 0, 0, 0, 0)),
+               [READ_REG_MPIDR]        = T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 0, 0, 5)),
+               [READ_REG_DTRRX]        = T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)),
+               [WRITE_REG_DTRTX]       = T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)),
+               [WRITE_REG_DSPSR]       = T32_FMTITR(ARMV8_MCR_DSPSR(0)),
+               [READ_REG_DSPSR]        = T32_FMTITR(ARMV8_MRC_DSPSR(0)),
+               [ARMV8_OPC_DSB_SY]      = T32_FMTITR(ARMV8_DSB_SY_T1),
+};
+
+void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)
+{
+       if (state_is_aarch64)
+               armv8->opcodes = &a64_opcodes[0];
+       else
+               armv8->opcodes = &t32_opcodes[0];
+}
+
+uint32_t armv8_opcode(struct armv8_common *armv8, enum armv8_opcode code)
+{
+       if ((int)code >= ARMV8_OPC_NUM)
+               return -1;
+
+       return *(armv8->opcodes + code);
+}
index 89d7440a8769734d3d183014ccd468f6fcd72810..9f5ebf8dcf9d6ca4ed151f485392a006109d12ae 100644 (file)
@@ -95,8 +95,6 @@
 #define SYSTEM_TTBR0_EL3               0b1111000100000000
 #define SYSTEM_TTBR1_EL1               0b1100000100000001
 
-
-
 #define ARMV8_MRS_DSPSR(Rt)    (0xd53b4500 | (Rt))
 #define ARMV8_MSR_DSPSR(Rt)    (0xd51b4500 | (Rt))
 #define ARMV8_MRS_DLR(Rt)      (0xd53b4520 | (Rt))
 /* T32 ITR format */
 #define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
 
+/* T32 instruction to access coprocessor registers */
+#define ARMV8_MCR_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MCR(cp, opc1, Rt, CRn, CRm, opc2)
+#define ARMV8_MRC_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MRC(cp, opc1, Rt, CRn, CRm, opc2)
+
+/* T32 instructions to access DSPSR and DLR */
+#define ARMV8_MRC_DSPSR(Rt) ARMV8_MRC_T1(15, 4, 3, 5, 0, Rt)
+#define ARMV8_MCR_DSPSR(Rt) ARMV8_MCR_T1(15, 4, 3, 5, 0, Rt)
+#define ARMV8_MRC_DLR(Rt)      ARMV8_MRC_T1(15, 4, 3, 5, 1, Rt)
+#define ARMV8_MCR_DLR(Rt)      ARMV8_MCR_T1(15, 4, 3, 5, 1, Rt)
+
 #define ARMV8_DCPS1(IM)        (0xd4a00001 | (((IM) & 0xFFFF) << 5))
 #define ARMV8_DCPS2(IM)        (0xd4a00002 | (((IM) & 0xFFFF) << 5))
 #define ARMV8_DCPS3(IM)        (0xd4a00003 | (((IM) & 0xFFFF) << 5))
 
-#define DSB_SY                         0xd5033F9F
+#define ARMV8_DSB_SY                           0xd5033F9F
+#define ARMV8_DSB_SY_T1                                0xf3bf8f4f
+
 #define ARMV8_MRS(System, Rt)  (0xd5300000 | ((System) << 5) | (Rt))
 /* ARM V8 Move to system register. */
 #define ARMV8_MSR_GP(System, Rt) \
 
 #define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
-#endif /* __ARM_OPCODES_H */
+enum armv8_opcode {
+       READ_REG_CLIDR,
+       READ_REG_CSSELR,
+       READ_REG_CCSIDR,
+       WRITE_REG_CSSELR,
+       READ_REG_MPIDR,
+       READ_REG_DTRRX,
+       WRITE_REG_DTRTX,
+       WRITE_REG_DSPSR,
+       READ_REG_DSPSR,
+       ARMV8_OPC_DSB_SY,
+       ARMV8_OPC_NUM,
+};
+
+extern uint32_t armv8_opcode(struct armv8_common *armv8, enum armv8_opcode);
+extern void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64);
+
+#endif /* OPENOCD_TARGET_ARMV8_OPCODES_H */