This is an automated email from Gerrit.

Matthias Welwarsky (matth...@welwarsky.de) just uploaded a new patch set to 
Gerrit, which you can find at http://openocd.zylin.com/3770

-- gerrit

commit f65b48802e730c4f13b1f0de680cacb3d5db07f2
Author: Matthias Welwarsky <matthias.welwar...@sysgo.com>
Date:   Thu Sep 15 09:13:51 2016 +0200

    Aarch32 support WIP
    
    Change-Id: I345e9f6d682fb4ba454e4b1d16bb5e1b27570691
    Signed-off-by: Matthias Welwarsky <matthias.welwar...@sysgo.com>

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 3483bd6..0866d0e 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -88,6 +88,7 @@ ARMV7_SRC = \
 
 ARMV8_SRC = \
        armv8_dpm.c \
+       armv8_opcodes.c \
        aarch64.c \
        armv8.c
 
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 60e17cb..436ee40 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -89,7 +89,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;
@@ -520,6 +523,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;
 
@@ -528,9 +532,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;
 
@@ -573,12 +575,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,
@@ -634,9 +635,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;
 
@@ -1151,6 +1150,7 @@ static int aarch64_debug_entry(struct target *target)
                return retval;
 
        /* Examine debug reason */
+
        armv8_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
 
        /* save address of instruction that triggered the watchpoint? */
@@ -1191,8 +1191,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:
@@ -1223,8 +1225,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;
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 1348843..5ec4b74 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -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 {
@@ -53,6 +53,30 @@ static const struct {
        /* 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,
        },
@@ -266,9 +290,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)
@@ -300,6 +374,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);
@@ -534,14 +647,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) {
@@ -572,18 +684,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) {
@@ -592,18 +707,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;
 
@@ -611,30 +721,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);
@@ -776,6 +877,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);
diff --git a/src/target/armv8.h b/src/target/armv8.h
index f077045..df3faf5 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -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 */
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 86a9d35..c0f4565 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -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
index 0000000..78b60e0
--- /dev/null
+++ b/src/target/armv8_opcodes.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2015 by Matthias Welwarsky <matthias.welwar...@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);
+}
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index a1fb5d4..7de882a 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -94,8 +94,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))
@@ -104,11 +102,23 @@
 /* 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_MRC_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) \
@@ -125,5 +135,21 @@
 #define ARMV8_MOVFSP_32(Rt) (0x11000000 | (0x1f << 5) | (Rt))
 #define ARMV8_MOVTSP_32(Rt) (0x11000000 | (Rt << 5) | (0x1F))
 
-
-#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 */

-- 

------------------------------------------------------------------------------
_______________________________________________
OpenOCD-devel mailing list
OpenOCD-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/openocd-devel

Reply via email to