interesting
/
riscv-openocd
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

aarch64: register access rewrite 

All register access is now performed through common read/write
functions, which delegate the actual register access to the
armv8_common object. armv8_common contains function pointers
to direct read and write requests to the respective low-level
functions for each PE state.

The respective read/write functions are selected on debug state
entry.

At the same time, T32 opcodes are now formatted for ITR in
dpmv8_exec_opcode() and the T32_FMTITR macro is removed from global
visibility.

Change-Id: I9eaef017c7cc9e0c531e693c534901bfdbdb842c
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
This commit is contained in:
Matthias Welwarsky 2016-10-06 16:10:38 +02:00
parent 2539a32308
commit 79c4c22e15
8 changed files with 582 additions and 368 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/target/aarch64.c
View File

@ -637,13 +637,15 @@ static int aarch64_debug_entry(struct target *target)
int retval = ERROR_OK; int retval = ERROR_OK;
struct aarch64_common *aarch64 = target_to_aarch64(target); struct aarch64_common *aarch64 = target_to_aarch64(target);
struct armv8_common *armv8 = target_to_armv8(target); struct armv8_common *armv8 = target_to_armv8(target);
struct arm_dpm *dpm = &armv8->dpm;
enum arm_state core_state;
LOG_DEBUG("dscr = 0x%08" PRIx32, aarch64->cpudbg_dscr); LOG_DEBUG("%s dscr = 0x%08" PRIx32, target_name(target), aarch64->cpudbg_dscr);
/* REVISIT see A8 TRM 12.11.4 steps 2..3 -- make sure that any dpm->dscr = aarch64->cpudbg_dscr;
* imprecise data aborts get discarded by issuing a Data core_state = armv8_dpm_get_core_state(dpm);
* Synchronization Barrier: ARMV4_5_MCR(15, 0, 0, 7, 10, 4). armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
*/ armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
/* make sure to clear all sticky errors */ /* make sure to clear all sticky errors */
retval = mem_ap_write_atomic_u32(armv8->debug_ap, retval = mem_ap_write_atomic_u32(armv8->debug_ap,
@ -677,11 +679,8 @@ static int aarch64_debug_entry(struct target *target)
retval = armv8_dpm_read_current_registers(&armv8->dpm); retval = armv8_dpm_read_current_registers(&armv8->dpm);
if (armv8->post_debug_entry) { if (retval == ERROR_OK && armv8->post_debug_entry)
retval = armv8->post_debug_entry(target); retval = armv8->post_debug_entry(target);
if (retval != ERROR_OK)
return retval;
}
return retval; return retval;
} }
@ -1457,7 +1456,7 @@ static int aarch64_write_apb_ap_memory(struct target *target,
/* Step 1.a+b - Write the address for read access into DBGDTRRX */ /* Step 1.a+b - Write the address for read access into DBGDTRRX */
/* Step 1.c - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */ /* Step 1.c - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
dpm->instr_write_data_dcc(dpm, dpm->instr_write_data_dcc(dpm,
T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL); ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
} }
/* Step 1.d - Change DCC to memory mode */ /* Step 1.d - Change DCC to memory mode */
@ -1486,6 +1485,8 @@ static int aarch64_write_apb_ap_memory(struct target *target,
armv8->debug_base + CPUV8_DBG_DSCR, &dscr); armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto error_free_buff_w; goto error_free_buff_w;
dpm->dscr = dscr;
if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) { if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
/* Abort occurred - clear it and exit */ /* Abort occurred - clear it and exit */
LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr); LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
@ -1586,9 +1587,9 @@ static int aarch64_read_apb_ap_memory(struct target *target,
/* Step 1.a+b - Write the address for read access into DBGDTRRXint */ /* Step 1.a+b - Write the address for read access into DBGDTRRXint */
/* Step 1.c - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */ /* Step 1.c - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
retval += dpm->instr_write_data_dcc(dpm, retval += dpm->instr_write_data_dcc(dpm,
T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL); ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */ /* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
retval += dpm->instr_execute(dpm, T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0))); retval += dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
/* Step 1.e - Change DCC to memory mode */ /* Step 1.e - Change DCC to memory mode */
dscr = dscr | DSCR_MA; dscr = dscr | DSCR_MA;
retval += mem_ap_write_atomic_u32(armv8->debug_ap, retval += mem_ap_write_atomic_u32(armv8->debug_ap,
@ -1648,6 +1649,9 @@ static int aarch64_read_apb_ap_memory(struct target *target,
armv8->debug_base + CPUV8_DBG_DSCR, &dscr); armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto error_free_buff_r; goto error_free_buff_r;
dpm->dscr = dscr;
if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) { if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
/* Abort occurred - clear it and exit */ /* Abort occurred - clear it and exit */
LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr); LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);

5
src/target/arm_dpm.h
View File

@ -143,6 +143,9 @@ struct arm_dpm {
/** Recent value of DSCR. */ /** Recent value of DSCR. */
uint32_t dscr; uint32_t dscr;
/** Recent exception level on armv8 */
unsigned int last_el;
/* FIXME -- read/write DCSR methods and symbols */ /* FIXME -- read/write DCSR methods and symbols */
}; };
@ -150,10 +153,8 @@ int arm_dpm_setup(struct arm_dpm *dpm);
int arm_dpm_initialize(struct arm_dpm *dpm); int arm_dpm_initialize(struct arm_dpm *dpm);
int arm_dpm_read_current_registers(struct arm_dpm *); int arm_dpm_read_current_registers(struct arm_dpm *);
int arm_dpm_read_current_registers_64(struct arm_dpm *);
int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode); int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
int arm_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp); int arm_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp);
void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar); void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar);

458
src/target/armv8.c
View File

@ -161,55 +161,333 @@ int armv8_mode_to_number(enum arm_mode mode)
} }
} }
static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regval)
static int armv8_read_core_reg(struct target *target, struct reg *r,
int num, enum arm_mode mode)
{ {
uint64_t reg_value; struct arm_dpm *dpm = &armv8->dpm;
int retval; int retval;
struct arm_reg *armv8_core_reg; uint32_t value;
struct armv8_common *armv8 = target_to_armv8(target); uint64_t value_64;
assert(num < (int)armv8->arm.core_cache->num_regs); switch (regnum) {
case 0 ... 30:
retval = dpm->instr_read_data_dcc_64(dpm,
ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum), &value_64);
break;
case ARMV8_SP:
retval = dpm->instr_read_data_r0_64(dpm,
ARMV8_MOVFSP_64(0), &value_64);
break;
case ARMV8_PC:
retval = dpm->instr_read_data_r0_64(dpm,
ARMV8_MRS_DLR(0), &value_64);
break;
case ARMV8_xPSR:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS_DSPSR(0), &value);
value_64 = value;
break;
case ARMV8_ELR_EL1:
retval = dpm->instr_read_data_r0_64(dpm,
ARMV8_MRS(SYSTEM_ELR_EL1, 0), &value_64);
break;
case ARMV8_ELR_EL2:
retval = dpm->instr_read_data_r0_64(dpm,
ARMV8_MRS(SYSTEM_ELR_EL2, 0), &value_64);
break;
case ARMV8_ELR_EL3:
retval = dpm->instr_read_data_r0_64(dpm,
ARMV8_MRS(SYSTEM_ELR_EL3, 0), &value_64);
break;
case ARMV8_ESR_EL1:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS(SYSTEM_ESR_EL1, 0), &value);
value_64 = value;
break;
case ARMV8_ESR_EL2:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS(SYSTEM_ESR_EL2, 0), &value);
value_64 = value;
break;
case ARMV8_ESR_EL3:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS(SYSTEM_ESR_EL3, 0), &value);
value_64 = value;
break;
case ARMV8_SPSR_EL1:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS(SYSTEM_SPSR_EL1, 0), &value);
value_64 = value;
break;
case ARMV8_SPSR_EL2:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS(SYSTEM_SPSR_EL2, 0), &value);
value_64 = value;
break;
case ARMV8_SPSR_EL3:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS(SYSTEM_SPSR_EL3, 0), &value);
value_64 = value;
break;
default:
retval = ERROR_FAIL;
break;
}
armv8_core_reg = armv8->arm.core_cache->reg_list[num].arch_info; if (retval == ERROR_OK && regval != NULL)
retval = armv8->load_core_reg_u64(target, *regval = value_64;
armv8_core_reg->num, &reg_value);
buf_set_u64(armv8->arm.core_cache->reg_list[num].value, 0, 64, reg_value);
armv8->arm.core_cache->reg_list[num].valid = 1;
armv8->arm.core_cache->reg_list[num].dirty = 0;
return retval; return retval;
} }
#if 0 static int armv8_write_reg(struct armv8_common *armv8, int regnum, uint64_t value_64)
static int armv8_write_core_reg(struct target *target, struct reg *r,
int num, enum arm_mode mode, target_addr_t value)
{ {
struct arm_dpm *dpm = &armv8->dpm;
int retval; int retval;
struct arm_reg *armv8_core_reg; uint32_t value;
struct armv8_common *armv8 = target_to_armv8(target);
assert(num < (int)armv8->arm.core_cache->num_regs); switch (regnum) {
case 0 ... 30:
armv8_core_reg = armv8->arm.core_cache->reg_list[num].arch_info; retval = dpm->instr_write_data_dcc_64(dpm,
retval = armv8->store_core_reg_u64(target, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
armv8_core_reg->num, value_64);
value); break;
if (retval != ERROR_OK) { case ARMV8_SP:
LOG_ERROR("JTAG failure"); retval = dpm->instr_write_data_r0_64(dpm,
armv8->arm.core_cache->reg_list[num].dirty = armv8->arm.core_cache->reg_list[num].valid; ARMV8_MOVTSP_64(0),
return ERROR_JTAG_DEVICE_ERROR; value_64);
break;
case ARMV8_PC:
retval = dpm->instr_write_data_r0_64(dpm,
ARMV8_MSR_DLR(0),
value_64);
break;
case ARMV8_xPSR:
value = value_64;
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_DSPSR(0),
value);
break;
/* registers clobbered by taking exception in debug state */
case ARMV8_ELR_EL1:
retval = dpm->instr_write_data_r0_64(dpm,
ARMV8_MSR_GP(SYSTEM_ELR_EL1, 0), value_64);
break;
case ARMV8_ELR_EL2:
retval = dpm->instr_write_data_r0_64(dpm,
ARMV8_MSR_GP(SYSTEM_ELR_EL2, 0), value_64);
break;
case ARMV8_ELR_EL3:
retval = dpm->instr_write_data_r0_64(dpm,
ARMV8_MSR_GP(SYSTEM_ELR_EL3, 0), value_64);
break;
case ARMV8_ESR_EL1:
value = value_64;
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP(SYSTEM_ESR_EL1, 0), value);
break;
case ARMV8_ESR_EL2:
value = value_64;
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP(SYSTEM_ESR_EL2, 0), value);
break;
case ARMV8_ESR_EL3:
value = value_64;
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP(SYSTEM_ESR_EL3, 0), value);
break;
case ARMV8_SPSR_EL1:
value = value_64;
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP(SYSTEM_SPSR_EL1, 0), value);
break;
case ARMV8_SPSR_EL2:
value = value_64;
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP(SYSTEM_SPSR_EL2, 0), value);
break;
case ARMV8_SPSR_EL3:
value = value_64;
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP(SYSTEM_SPSR_EL3, 0), value);
break;
default:
retval = ERROR_FAIL;
break;
} }
LOG_DEBUG("write core reg %i value 0x%" PRIx64 "", num, value); return retval;
armv8->arm.core_cache->reg_list[num].valid = 1; }
armv8->arm.core_cache->reg_list[num].dirty = 0;
static int armv8_read_reg32(struct armv8_common *armv8, int regnum, uint64_t *regval)
return ERROR_OK; {
struct arm_dpm *dpm = &armv8->dpm;
uint32_t value = 0;
int retval;
switch (regnum) {
case ARMV8_R0 ... ARMV8_R14:
/* return via DCC: "MCR p14, 0, Rnum, c0, c5, 0" */
retval = dpm->instr_read_data_dcc(dpm,
ARMV4_5_MCR(14, 0, regnum, 0, 5, 0),
&value);
break;
case ARMV8_SP:
retval = dpm->instr_read_data_dcc(dpm,
ARMV4_5_MCR(14, 0, 13, 0, 5, 0),
&value);
break;
case ARMV8_PC:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRC_DLR(0),
&value);
break;
case ARMV8_xPSR:
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRC_DSPSR(0),
&value);
break;
case ARMV8_ELR_EL1: /* mapped to LR_svc */
retval = dpm->instr_read_data_dcc(dpm,
ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
&value);
break;
case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS_T1(0, 14, 0, 1),
&value);
break;
case ARMV8_ELR_EL3: /* mapped to LR_mon */
retval = dpm->instr_read_data_dcc(dpm,
ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
&value);
break;
case ARMV8_ESR_EL1: /* mapped to DFSR */
retval = dpm->instr_read_data_r0(dpm,
ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
&value);
break;
case ARMV8_ESR_EL2: /* mapped to HSR */
retval = dpm->instr_read_data_r0(dpm,
ARMV4_5_MRC(15, 4, 0, 5, 2, 0),
&value);
break;
case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
retval = ERROR_FAIL;
break;
case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS_xPSR_T1(1, 0),
&value);
break;
case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS_xPSR_T1(1, 0),
&value);
break;
case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS_xPSR_T1(1, 0),
&value);
break;
default:
retval = ERROR_FAIL;
break;
}
if (retval == ERROR_OK && regval != NULL)
*regval = value;
return retval;
}
static int armv8_write_reg32(struct armv8_common *armv8, int regnum, uint64_t value)
{
struct arm_dpm *dpm = &armv8->dpm;
int retval;
switch (regnum) {
case ARMV8_R0 ... ARMV8_R14:
/* load register from DCC: "MRC p14, 0, Rnum, c0, c5, 0" */
retval = dpm->instr_write_data_dcc(dpm,
ARMV4_5_MRC(14, 0, regnum, 0, 5, 0), value);
break;
case ARMV8_SP:
retval = dpm->instr_write_data_dcc(dpm,
ARMV4_5_MRC(14, 0, 13, 0, 5, 0),
value);
break;
case ARMV8_PC:/* PC
* read r0 from DCC; then "MOV pc, r0" */
retval = dpm->instr_write_data_r0(dpm,
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,
ARMV8_MCR_DSPSR(0), value);
break;
case ARMV8_ELR_EL1: /* mapped to LR_svc */
retval = dpm->instr_write_data_dcc(dpm,
ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
value);
break;
case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP_T1(0, 14, 0, 1),
value);
break;
case ARMV8_ELR_EL3: /* mapped to LR_mon */
retval = dpm->instr_write_data_dcc(dpm,
ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
value);
break;
case ARMV8_ESR_EL1: /* mapped to DFSR */
retval = dpm->instr_write_data_r0(dpm,
ARMV4_5_MCR(15, 0, 0, 5, 0, 0),
value);
break;
case ARMV8_ESR_EL2: /* mapped to HSR */
retval = dpm->instr_write_data_r0(dpm,
ARMV4_5_MCR(15, 4, 0, 5, 2, 0),
value);
break;
case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
retval = ERROR_FAIL;
break;
case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
value);
break;
case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
value);
break;
case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
retval = dpm->instr_write_data_r0(dpm,
ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
value);
break;
default:
retval = ERROR_FAIL;
break;
}
return retval;
}
void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64)
{
if (is_aarch64) {
armv8->read_reg_u64 = armv8_read_reg;
armv8->write_reg_u64 = armv8_write_reg;
} else {
armv8->read_reg_u64 = armv8_read_reg32;
armv8->write_reg_u64 = armv8_write_reg32;
}
} }
#endif
/* retrieve core id cluster id */ /* retrieve core id cluster id */
int armv8_read_mpidr(struct armv8_common *armv8) int armv8_read_mpidr(struct armv8_common *armv8)
@ -306,26 +584,26 @@ static void armv8_show_fault_registers32(struct armv8_common *armv8)
/* c5/c0 - {data, instruction} fault status registers */ /* c5/c0 - {data, instruction} fault status registers */
retval = dpm->instr_read_data_r0(dpm, retval = dpm->instr_read_data_r0(dpm,
T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 0)), ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
&dfsr); &dfsr);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto done; goto done;
retval = dpm->instr_read_data_r0(dpm, retval = dpm->instr_read_data_r0(dpm,
T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 1)), ARMV4_5_MRC(15, 0, 0, 5, 0, 1),
&ifsr); &ifsr);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto done; goto done;
/* c6/c0 - {data, instruction} fault address registers */ /* c6/c0 - {data, instruction} fault address registers */
retval = dpm->instr_read_data_r0(dpm, retval = dpm->instr_read_data_r0(dpm,
T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 0)), ARMV4_5_MRC(15, 0, 0, 6, 0, 0),
&dfar); &dfar);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto done; goto done;
retval = dpm->instr_read_data_r0(dpm, retval = dpm->instr_read_data_r0(dpm,
T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 2)), ARMV4_5_MRC(15, 0, 0, 6, 0, 2),
&ifar); &ifar);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto done; goto done;
@ -386,7 +664,7 @@ static __unused int armv8_read_ttbcr32(struct target *target)
goto done; goto done;
/* MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/ /* MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
retval = dpm->instr_read_data_r0(dpm, retval = dpm->instr_read_data_r0(dpm,
T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 2, 0, 2)), ARMV4_5_MRC(15, 0, 0, 2, 0, 2),
&ttbcr); &ttbcr);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto done; goto done;
@ -431,7 +709,7 @@ static __unused int armv8_read_ttbcr(struct target *target)
memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used)); memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used));
memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask)); memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask));
switch (armv8_curel_from_core_mode(arm)) { switch (armv8_curel_from_core_mode(arm->core_mode)) {
case SYSTEM_CUREL_EL3: case SYSTEM_CUREL_EL3:
retval = dpm->instr_read_data_r0(dpm, retval = dpm->instr_read_data_r0(dpm,
ARMV8_MRS(SYSTEM_TCR_EL3, 0), ARMV8_MRS(SYSTEM_TCR_EL3, 0),
@ -519,7 +797,7 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
if (retval != ERROR_OK) if (retval != ERROR_OK)
return retval; return retval;
switch (armv8_curel_from_core_mode(arm)) { switch (armv8_curel_from_core_mode(arm->core_mode)) {
case SYSTEM_CUREL_EL0: case SYSTEM_CUREL_EL0:
instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0); instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0);
/* can only execute instruction at EL2 */ /* can only execute instruction at EL2 */
@ -602,11 +880,6 @@ int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
armv8->arm.common_magic = ARM_COMMON_MAGIC; armv8->arm.common_magic = ARM_COMMON_MAGIC;
armv8->common_magic = ARMV8_COMMON_MAGIC; armv8->common_magic = ARMV8_COMMON_MAGIC;
arm->read_core_reg = armv8_read_core_reg;
#if 0
arm->write_core_reg = armv8_write_core_reg;
#endif
armv8->armv8_mmu.armv8_cache.l2_cache = NULL; armv8->armv8_mmu.armv8_cache.l2_cache = NULL;
armv8->armv8_mmu.armv8_cache.info = -1; armv8->armv8_mmu.armv8_cache.info = -1;
armv8->armv8_mmu.armv8_cache.flush_all_data_cache = NULL; armv8->armv8_mmu.armv8_cache.flush_all_data_cache = NULL;
@ -673,46 +946,59 @@ static const struct {
unsigned id; unsigned id;
const char *name; const char *name;
unsigned bits; unsigned bits;
enum arm_mode mode;
enum reg_type type; enum reg_type type;
const char *group; const char *group;
const char *feature; const char *feature;
} armv8_regs[] = { } armv8_regs[] = {
{ ARMV8_R0, "x0", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R0, "x0", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R1, "x1", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R1, "x1", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R2, "x2", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R2, "x2", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R3, "x3", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R3, "x3", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R4, "x4", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R4, "x4", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R5, "x5", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R5, "x5", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R6, "x6", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R6, "x6", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R7, "x7", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R7, "x7", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R8, "x8", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R8, "x8", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R9, "x9", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R9, "x9", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R10, "x10", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R10, "x10", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R11, "x11", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R11, "x11", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R12, "x12", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R12, "x12", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R13, "x13", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R13, "x13", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R14, "x14", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R14, "x14", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R15, "x15", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R15, "x15", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R16, "x16", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R16, "x16", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R17, "x17", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R17, "x17", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R18, "x18", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R18, "x18", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R19, "x19", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R19, "x19", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R20, "x20", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R20, "x20", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R21, "x21", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R21, "x21", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R22, "x22", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R22, "x22", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R23, "x23", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R23, "x23", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R24, "x24", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R24, "x24", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R25, "x25", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R25, "x25", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R26, "x26", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R26, "x26", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R27, "x27", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R27, "x27", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R28, "x28", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R28, "x28", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R29, "x29", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R29, "x29", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R30, "x30", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_R30, "x30", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_R31, "sp", 64, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_SP, "sp", 64, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_PC, "pc", 64, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_PC, "pc", 64, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_xPSR, "CPSR", 32, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core" }, { ARMV8_xPSR, "CPSR", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core" },
{ ARMV8_ELR_EL1, "ELR_EL1", 64, ARMV8_64_EL1H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_ESR_EL1, "ESR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_SPSR_EL1, "SPSR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_ELR_EL2, "ELR_EL2", 64, ARMV8_64_EL2H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_ESR_EL2, "ESR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_SPSR_EL2, "SPSR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_ELR_EL3, "ELR_EL3", 64, ARMV8_64_EL3H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_ESR_EL3, "ESR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
{ ARMV8_SPSR_EL3, "SPSR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
}; };
#define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs) #define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
@ -782,6 +1068,7 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
for (i = 0; i < num_regs; i++) { for (i = 0; i < num_regs; i++) {
arch_info[i].num = armv8_regs[i].id; arch_info[i].num = armv8_regs[i].id;
arch_info[i].mode = armv8_regs[i].mode;
arch_info[i].target = target; arch_info[i].target = target;
arch_info[i].arm = arm; arch_info[i].arm = arm;
@ -847,11 +1134,18 @@ int armv8_get_gdb_reg_list(struct target *target,
switch (reg_class) { switch (reg_class) {
case REG_CLASS_GENERAL: case REG_CLASS_GENERAL:
*reg_list_size = ARMV8_ELR_EL1;
*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
for (i = 0; i < *reg_list_size; i++)
(*reg_list)[i] = armv8_reg_current(arm, i);
return ERROR_OK;
case REG_CLASS_ALL: case REG_CLASS_ALL:
*reg_list_size = ARMV8_LAST_REG; *reg_list_size = ARMV8_LAST_REG;
*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size)); *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
for (i = 0; i < ARMV8_LAST_REG; i++) for (i = 0; i < *reg_list_size; i++)
(*reg_list)[i] = armv8_reg_current(arm, i); (*reg_list)[i] = armv8_reg_current(arm, i);
return ERROR_OK; return ERROR_OK;

46
src/target/armv8.h
View File

@ -26,7 +26,7 @@
#include "armv8_dpm.h" #include "armv8_dpm.h"
enum { enum {
ARMV8_R0, ARMV8_R0 = 0,
ARMV8_R1, ARMV8_R1,
ARMV8_R2, ARMV8_R2,
ARMV8_R3, ARMV8_R3,
@ -57,11 +57,23 @@ enum {
ARMV8_R28, ARMV8_R28,
ARMV8_R29, ARMV8_R29,
ARMV8_R30, ARMV8_R30,
ARMV8_R31,
ARMV8_SP = 31,
ARMV8_PC = 32, ARMV8_PC = 32,
ARMV8_xPSR = 33, ARMV8_xPSR = 33,
ARMV8_ELR_EL1 = 34,
ARMV8_ESR_EL1 = 35,
ARMV8_SPSR_EL1 = 36,
ARMV8_ELR_EL2 = 37,
ARMV8_ESR_EL2 = 38,
ARMV8_SPSR_EL2 = 39,
ARMV8_ELR_EL3 = 40,
ARMV8_ESR_EL3 = 41,
ARMV8_SPSR_EL3 = 42,
ARMV8_LAST_REG, ARMV8_LAST_REG,
}; };
@ -162,8 +174,8 @@ struct armv8_common {
struct armv8_mmu_common armv8_mmu; struct armv8_mmu_common armv8_mmu;
/* Direct processor core register read and writes */ /* Direct processor core register read and writes */
int (*load_core_reg_u64)(struct target *target, uint32_t num, uint64_t *value); int (*read_reg_u64)(struct armv8_common *armv8, int num, uint64_t *value);
int (*store_core_reg_u64)(struct target *target, uint32_t num, uint64_t value); int (*write_reg_u64)(struct armv8_common *armv8, int num, uint64_t value);
int (*examine_debug_reason)(struct target *target); int (*examine_debug_reason)(struct target *target);
int (*post_debug_entry)(struct target *target); int (*post_debug_entry)(struct target *target);
@ -270,10 +282,32 @@ int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
void armv8_set_cpsr(struct arm *arm, uint32_t cpsr); void armv8_set_cpsr(struct arm *arm, uint32_t cpsr);
static inline int armv8_curel_from_core_mode(struct arm *arm) static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
{ {
return (arm->core_mode >> 6) & 3; switch (core_mode) {
/* Aarch32 modes */
case ARM_MODE_USR:
return 0;
case ARM_MODE_SVC:
case ARM_MODE_ABT: /* FIXME: EL3? */
case ARM_MODE_IRQ: /* FIXME: EL3? */
case ARM_MODE_FIQ: /* FIXME: EL3? */
case ARM_MODE_UND: /* FIXME: EL3? */
case ARM_MODE_SYS: /* FIXME: EL3? */
return 1;
/* case ARM_MODE_HYP:
* return 2;
*/
case ARM_MODE_MON:
return 3;
/* all Aarch64 modes */
default:
return (core_mode >> 6) & 3;
}
} }
void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64);
extern const struct command_registration armv8_command_handlers[]; extern const struct command_registration armv8_command_handlers[];
#endif #endif

375
src/target/armv8_dpm.c
View File

@ -28,6 +28,8 @@
#include "helper/time_support.h" #include "helper/time_support.h"
/* T32 ITR format */
#define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
/** /**
* @file * @file
@ -42,6 +44,29 @@
* to minimize needless differences in debug behavior between cores. * to minimize needless differences in debug behavior between cores.
*/ */
/**
* Get core state from EDSCR, without necessity to retrieve CPSR
*/
enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
{
int el = (dpm->dscr >> 8) & 0x3;
int rw = (dpm->dscr >> 10) & 0xF;
int pos;
dpm->last_el = el;
/* find the first '0' in DSCR.RW */
for (pos = 3; pos >= 0; pos--) {
if ((rw & (1 << pos)) == 0)
break;
}
if (el > pos)
return ARM_STATE_AARCH64;
return ARM_STATE_ARM;
}
/*----------------------------------------------------------------------*/ /*----------------------------------------------------------------------*/
static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data) static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
@ -166,6 +191,9 @@ static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
} }
} }
/* update the stored copy of dscr */
dpm->dscr = dscr;
/* this "should never happen" ... */ /* this "should never happen" ... */
if (dscr & DSCR_DTR_RX_FULL) { if (dscr & DSCR_DTR_RX_FULL) {
LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr); LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
@ -218,6 +246,9 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
} }
} }
if (armv8_dpm_get_core_state(dpm) != ARM_STATE_AARCH64)
opcode = T32_FMTITR(opcode);
retval = mem_ap_write_u32(armv8->debug_ap, retval = mem_ap_write_u32(armv8->debug_ap,
armv8->debug_base + CPUV8_DBG_ITR, opcode); armv8->debug_base + CPUV8_DBG_ITR, opcode);
if (retval != ERROR_OK) if (retval != ERROR_OK)
@ -237,6 +268,20 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
} }
} while ((dscr & DSCR_ITE) == 0); /* Wait for InstrCompl bit to be set */ } while ((dscr & DSCR_ITE) == 0); /* Wait for InstrCompl bit to be set */
/* update dscr and el after each command execution */
dpm->dscr = dscr;
if (dpm->last_el != ((dscr >> 8) & 3))
LOG_DEBUG("EL %i -> %i", dpm->last_el, (dscr >> 8) & 3);
dpm->last_el = (dscr >> 8) & 3;
if (dscr & DSCR_ERR) {
LOG_ERROR("Opcode 0x%08"PRIx32", DSCR.ERR=1, DSCR.EL=%i", opcode, dpm->last_el);
/* clear the sticky error condition */
mem_ap_write_atomic_u32(armv8->debug_ap,
armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
retval = ERROR_FAIL;
}
if (p_dscr) if (p_dscr)
*p_dscr = dscr; *p_dscr = dscr;
@ -471,7 +516,7 @@ static int dpmv8_mrc(struct target *target, int cpnum,
/* read coprocessor register into R0; return via DCC */ /* read coprocessor register into R0; return via DCC */
retval = dpm->instr_read_data_r0(dpm, retval = dpm->instr_read_data_r0(dpm,
T32_FMTITR(ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2)), ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2),
value); value);
/* (void) */ dpm->finish(dpm); /* (void) */ dpm->finish(dpm);
@ -496,7 +541,7 @@ static int dpmv8_mcr(struct target *target, int cpnum,
/* read DCC into r0; then write coprocessor register from R0 */ /* read DCC into r0; then write coprocessor register from R0 */
retval = dpm->instr_write_data_r0(dpm, retval = dpm->instr_write_data_r0(dpm,
T32_FMTITR(ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2)), ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2),
value); value);
/* (void) */ dpm->finish(dpm); /* (void) */ dpm->finish(dpm);
@ -616,210 +661,50 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
return retval; return retval;
} }
static int dpmv8_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum) /*
{ * Common register read, relies on armv8_select_reg_access() having been called.
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) static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
{ {
uint32_t value; struct armv8_common *armv8 = dpm->arm->arch_info;
uint64_t value_64; uint64_t value_64;
int retval = ERROR_FAIL; int retval;
switch (regnum) { retval = armv8->read_reg_u64(armv8, regnum, &value_64);
case 0 ... 30:
retval = dpm->instr_read_data_dcc_64(dpm,
ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum),
&value_64);
break;
case ARMV8_R31:
retval = dpm->instr_read_data_r0_64(dpm,
ARMV8_MOVFSP_64(0),
&value_64);
break;
case ARMV8_PC:
retval = dpm->instr_read_data_r0_64(dpm,
ARMV8_MRS_DLR(0),
&value_64);
break;
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;
}
if (retval == ERROR_OK) { if (retval == ERROR_OK) {
r->valid = true; r->valid = true;
r->dirty = false; r->dirty = false;
if (r->size == 64) { buf_set_u64(r->value, 0, r->size, value_64);
buf_set_u64(r->value, 0, 64, value_64); if (r->size == 64)
LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) 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 int) value_64);
LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
}
} }
return retval; return ERROR_OK;
} }
/* just write the register -- rely on the core mode being right */ /*
* Common register write, relies on armv8_select_reg_access() having been called.
*/
static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum) static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
{ {
struct armv8_common *armv8 = dpm->arm->arch_info;
int retval = ERROR_FAIL; int retval = ERROR_FAIL;
uint32_t value = 0xFFFFFFFF; uint64_t value_64;
uint64_t value_64 = 0xFFFFFFFFFFFFFFFF;
switch (regnum) {
case 0 ... 30:
value_64 = buf_get_u64(r->value, 0, 64);
retval = dpm->instr_write_data_dcc_64(dpm,
ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
value_64);
break;
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 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 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;
}
value_64 = buf_get_u64(r->value, 0, r->size);
retval = armv8->write_reg_u64(armv8, regnum, value_64);
if (retval == ERROR_OK) { if (retval == ERROR_OK) {
r->dirty = false; r->dirty = false;
if (r->size == 64) if (r->size == 64)
LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long) value_64); LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
else else
LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned) value); LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
} }
return retval; return ERROR_OK;
}
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;
} }
/** /**
@ -833,60 +718,59 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
{ {
struct arm *arm = dpm->arm; struct arm *arm = dpm->arm;
struct armv8_common *armv8 = (struct armv8_common *)arm->arch_info; struct armv8_common *armv8 = (struct armv8_common *)arm->arch_info;
enum arm_state core_state; struct reg_cache *cache;
uint32_t cpsr;
int retval;
struct reg *r; struct reg *r;
uint32_t cpsr;
int retval;
retval = dpm->prepare(dpm); retval = dpm->prepare(dpm);
if (retval != ERROR_OK) if (retval != ERROR_OK)
return retval; return retval;
core_state = dpm_get_core_state(dpm->dscr); cache = arm->core_cache;
armv8_select_opcodes(armv8, core_state);
/* read R0 first (it's used for scratch), then CPSR */ /* read R0 first (it's used for scratch), then CPSR */
r = arm->core_cache->reg_list + 0; r = cache->reg_list + 0;
if (!r->valid) { if (!r->valid) {
retval = core_state == ARM_STATE_AARCH64 ? retval = dpmv8_read_reg(dpm, r, 0);
dpmv8_read_reg(dpm, r, 0) : dpmv8_read_reg32(dpm, r, 0);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto fail; goto fail;
} }
r->dirty = true; r->dirty = true;
/* read cpsr to r0 and get it back */ /* read cpsr to r0 and get it back */
retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_DSPSR), &cpsr); retval = dpm->instr_read_data_r0(dpm,
armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto fail; goto fail;
/* update core mode and state, plus shadow mapping for R8..R14 */ /* update core mode and state */
armv8_set_cpsr(arm, cpsr); armv8_set_cpsr(arm, cpsr);
/* REVISIT we can probably avoid reading R1..R14, saving time... */ for (unsigned int i = 1; i < cache->num_regs ; i++) {
for (unsigned i = 1; i < arm->core_cache->num_regs ; i++) { struct arm_reg *arm_reg;
r = armv8_reg_current(arm, i); r = armv8_reg_current(arm, i);
if (r->valid) if (r->valid)
continue; continue;
retval = core_state == ARM_STATE_AARCH64 ? /*
dpmv8_read_reg(dpm, r, i) : dpmv8_read_reg32(dpm, r, i); * Only read registers that are available from the
* current EL (or core mode).
*/
arm_reg = r->arch_info;
if (arm_reg->mode != ARM_MODE_ANY &&
dpm->last_el != armv8_curel_from_core_mode(arm_reg->mode))
continue;
retval = dpmv8_read_reg(dpm, r, i);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto fail; goto fail;
} }
/* NOTE: SPSR ignored (if it's even relevant). */
/* REVISIT the debugger can trigger various exceptions. See the
* ARMv7A architecture spec, section C5.7, for more info about
* what defenses are needed; v6 debug has the most issues.
*/
fail: fail:
/* (void) */ dpm->finish(dpm); dpm->finish(dpm);
return retval; return retval;
} }
@ -951,7 +835,6 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
struct arm *arm = dpm->arm; struct arm *arm = dpm->arm;
struct reg_cache *cache = arm->core_cache; struct reg_cache *cache = arm->core_cache;
int retval; int retval;
bool is_aarch64 = arm->core_state == ARM_STATE_AARCH64;
retval = dpm->prepare(dpm); retval = dpm->prepare(dpm);
if (retval != ERROR_OK) if (retval != ERROR_OK)
@ -991,59 +874,48 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
* be queued, and need (efficient/batched) flushing later. * be queued, and need (efficient/batched) flushing later.
*/ */
/* Scan the registers until we find one that's both dirty and /* Restore original core mode and state */
* eligible for flushing. Flush that and everything else that retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
* shares the same core mode setting. Typically this won't if (retval != ERROR_OK)
* actually find anything to do... goto done;
*/
/* check everything except our scratch register R0 */ /* check everything except our scratch register R0 */
for (unsigned i = 1; i < cache->num_regs; i++) { for (unsigned i = 1; i < cache->num_regs; i++) {
struct arm_reg *r; struct arm_reg *r;
unsigned regnum;
/* also skip PC, CPSR, and non-dirty */ /* skip PC and CPSR */
if (i == (arm->core_cache->num_regs - 2)) if (i == ARMV8_PC || i == ARMV8_xPSR)
continue; continue;
if (arm->cpsr == cache->reg_list + i) /* skip invalid */
if (!cache->reg_list[i].valid)
continue; continue;
/* skip non-dirty */
if (!cache->reg_list[i].dirty) if (!cache->reg_list[i].dirty)
continue; continue;
/* skip all registers not on the current EL */
r = cache->reg_list[i].arch_info; r = cache->reg_list[i].arch_info;
regnum = r->num; if (r->mode != ARM_MODE_ANY &&
dpm->last_el != armv8_curel_from_core_mode(r->mode))
continue;
retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[i], regnum) retval = dpmv8_write_reg(dpm, &cache->reg_list[i], i);
: dpmv8_write_reg32(dpm, &cache->reg_list[i], regnum);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto done; break;
} }
/* flush CPSR and PC */
/* Restore original CPSR ... assuming either that we changed it, if (retval == ERROR_OK)
* or it's dirty. Must write PC to ensure the return address is retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_xPSR], ARMV8_xPSR);
* defined, and must not write it before CPSR. if (retval == ERROR_OK)
*/ retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_PC], ARMV8_PC);
retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
if (retval != ERROR_OK)
goto done;
arm->cpsr->dirty = false;
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 */ /* flush R0 -- it's *very* dirty by now */
retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[0], 0) if (retval == ERROR_OK)
: dpmv8_write_reg32(dpm, &cache->reg_list[0], 0); retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
if (retval != ERROR_OK) if (retval == ERROR_OK)
goto done; dpm->instr_cpsr_sync(dpm);
cache->reg_list[0].dirty = false;
/* (void) */ dpm->finish(dpm);
done: done:
dpm->finish(dpm);
return retval; return retval;
} }
@ -1061,19 +933,18 @@ static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
int retval; int retval;
int max = arm->core_cache->num_regs; int max = arm->core_cache->num_regs;
if (regnum < 0 || regnum > max) if (regnum < 0 || regnum >= max)
return ERROR_COMMAND_SYNTAX_ERROR; return ERROR_COMMAND_SYNTAX_ERROR;
/* REVISIT what happens if we try to read SPSR in a core mode /*
* REVISIT what happens if we try to read SPSR in a core mode
* which has no such register? * which has no such register?
*/ */
retval = dpm->prepare(dpm); retval = dpm->prepare(dpm);
if (retval != ERROR_OK) if (retval != ERROR_OK)
return retval; return retval;
retval = arm->core_state == ARM_STATE_AARCH64 ? retval = dpmv8_read_reg(dpm, r, regnum);
dpmv8_read_reg(dpm, r, regnum) : dpmv8_read_reg32(dpm, r, regnum);
if (retval != ERROR_OK) if (retval != ERROR_OK)
goto fail; goto fail;
@ -1101,12 +972,11 @@ static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
if (retval != ERROR_OK) if (retval != ERROR_OK)
return retval; return retval;
retval = arm->core_state == ARM_STATE_AARCH64 ? retval = dpmv8_write_reg(dpm, r, regnum);
dpmv8_write_reg(dpm, r, regnum) : dpmv8_write_reg32(dpm, r, regnum);
/* always clean up, regardless of error */ /* always clean up, regardless of error */
dpm->finish(dpm);
/* (void) */ dpm->finish(dpm);
return retval; return retval;
} }
@ -1402,6 +1272,7 @@ void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
struct target *target = dpm->arm->target; struct target *target = dpm->arm->target;
dpm->dscr = dscr; dpm->dscr = dscr;
dpm->last_el = (dscr >> 8) & 3;
/* Examine debug reason */ /* Examine debug reason */
switch (DSCR_ENTRY(dscr)) { switch (DSCR_ENTRY(dscr)) {

1
src/target/armv8_dpm.h
View File

@ -111,5 +111,6 @@ void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
#define PRSR_SDR (1 << 11) #define PRSR_SDR (1 << 11)
void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr); void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm);
#endif /* OPENOCD_TARGET_ARM_DPM_H */ #endif /* OPENOCD_TARGET_ARM_DPM_H */

20
src/target/armv8_opcodes.c
View File

@ -37,16 +37,16 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
}; };
static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = { 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_CLIDR] = 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_CSSELR] = ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
[READ_REG_CCSIDR] = T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 0)), [READ_REG_CCSIDR] = ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
[WRITE_REG_CSSELR] = T32_FMTITR(ARMV4_5_MCR(15, 2, 0, 0, 0, 0)), [WRITE_REG_CSSELR] = 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_MPIDR] = ARMV4_5_MRC(15, 0, 0, 0, 0, 5),
[READ_REG_DTRRX] = T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), [READ_REG_DTRRX] = 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_DTRTX] = ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
[WRITE_REG_DSPSR] = T32_FMTITR(ARMV8_MCR_DSPSR(0)), [WRITE_REG_DSPSR] = ARMV8_MCR_DSPSR(0),
[READ_REG_DSPSR] = T32_FMTITR(ARMV8_MRC_DSPSR(0)), [READ_REG_DSPSR] = ARMV8_MRC_DSPSR(0),
[ARMV8_OPC_DSB_SY] = T32_FMTITR(ARMV8_DSB_SY_T1), [ARMV8_OPC_DSB_SY] = ARMV8_DSB_SY_T1,
}; };
void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64) void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)

17
src/target/armv8_opcodes.h
View File

@ -66,7 +66,6 @@
#define SYSTEM_ISR_EL1 0b1100011000001000 #define SYSTEM_ISR_EL1 0b1100011000001000
#define SYSTEM_DBG_DSPSR_EL0 0b1101101000101000 #define SYSTEM_DBG_DSPSR_EL0 0b1101101000101000
#define SYSTEM_DBG_DLR_EL0 0b1101101000101001 #define SYSTEM_DBG_DLR_EL0 0b1101101000101001
#define SYSTEM_DBG_DTRRX_EL0 0b1001100000101000 #define SYSTEM_DBG_DTRRX_EL0 0b1001100000101000
@ -103,14 +102,19 @@
#define SYSTEM_ATS1E2R 0b0110001111000000 #define SYSTEM_ATS1E2R 0b0110001111000000
#define SYSTEM_ATS1E3R 0b0111001111000000 #define SYSTEM_ATS1E3R 0b0111001111000000
/* fault status and fault address */
#define SYSTEM_FAR_EL1 0b1100001100000000
#define SYSTEM_FAR_EL2 0b1110001100000000
#define SYSTEM_FAR_EL3 0b1111001100000000
#define SYSTEM_ESR_EL1 0b1100001010010000
#define SYSTEM_ESR_EL2 0b1110001010010000
#define SYSTEM_ESR_EL3 0b1111001010010000
#define ARMV8_MRS_DSPSR(Rt) (0xd53b4500 | (Rt)) #define ARMV8_MRS_DSPSR(Rt) (0xd53b4500 | (Rt))
#define ARMV8_MSR_DSPSR(Rt) (0xd51b4500 | (Rt)) #define ARMV8_MSR_DSPSR(Rt) (0xd51b4500 | (Rt))
#define ARMV8_MRS_DLR(Rt) (0xd53b4520 | (Rt)) #define ARMV8_MRS_DLR(Rt) (0xd53b4520 | (Rt))
#define ARMV8_MSR_DLR(Rt) (0xd51b4520 | (Rt)) #define ARMV8_MSR_DLR(Rt) (0xd51b4520 | (Rt))
/* T32 ITR format */
#define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
/* T32 instruction to access coprocessor registers */ /* 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_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) #define ARMV8_MRC_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MRC(cp, opc1, Rt, CRn, CRm, opc2)
@ -136,6 +140,11 @@
#define ARMV8_MSR_IM(Op1, CRm, Op2) \ #define ARMV8_MSR_IM(Op1, CRm, Op2) \
(0xd500401f | ((Op1) << 16) | ((CRm) << 8) | ((Op2) << 5)) (0xd500401f | ((Op1) << 16) | ((CRm) << 8) | ((Op2) << 5))
#define ARMV8_MRS_T1(R, M1, Rd, M) (0xF3E08020 | (R << 20) | (M1 << 16) | (Rd << 8) | (M << 4))
#define ARMV8_MRS_xPSR_T1(R, Rd) (0xF3EF8000 | (R << 20) | (Rd << 8))
#define ARMV8_MSR_GP_T1(R, M1, Rd, M) (0xF3808020 | (R << 20) | (M1 << 8) | (Rd << 16) | (M << 4))
#define ARMV8_MSR_GP_xPSR_T1(R, Rn, mask) (0xF3808000 | (R << 20) | (Rn << 16) | (mask << 8))
#define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5)) #define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5))
#define ARMV8_HLT(Im) (0x0D4400000 | ((Im & 0xffff) << 5)) #define ARMV8_HLT(Im) (0x0D4400000 | ((Im & 0xffff) << 5))