X-Git-Url: https://review.openocd.org/gitweb?a=blobdiff_plain;f=src%2Ftarget%2Farmv8_dpm.c;h=f4e7a07996c651546c92812b04f4f085a76305cd;hb=f7836bbc75586666d639edd1cf39a871c434d980;hp=ef53452f142cfc759c8662f8b545698232df2bfd;hpb=f7dbcc3fab3d5b52710f235ae3ead3135cf9c2e6;p=openocd.git

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index ef53452f14..f4e7a07996 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -51,17 +51,11 @@ 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)
+	/* In Debug state, each bit gives the current Execution state of each EL */
+	if ((rw >> el) & 0b1)
 		return ARM_STATE_AARCH64;
 
 	return ARM_STATE_ARM;
@@ -71,7 +65,6 @@ enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
 
 static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
 {
-	LOG_DEBUG("write DCC 0x%08" PRIx32, data);
 	return mem_ap_write_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DTRRX, data);
 }
@@ -79,11 +72,10 @@ static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
 static int dpmv8_write_dcc_64(struct armv8_common *armv8, uint64_t data)
 {
 	int ret;
-	LOG_DEBUG("write DCC Low word 0x%08" PRIx32, (unsigned)data);
-	LOG_DEBUG("write DCC High word 0x%08" PRIx32, (unsigned)(data >> 32));
 	ret = mem_ap_write_u32(armv8->debug_ap,
 			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
-	ret += mem_ap_write_u32(armv8->debug_ap,
+	if (ret == ERROR_OK)
+		ret = mem_ap_write_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
 	return ret;
 }
@@ -116,7 +108,6 @@ static int dpmv8_read_dcc(struct armv8_common *armv8, uint32_t *data,
 					    data);
 	if (retval != ERROR_OK)
 		return retval;
-	LOG_DEBUG("read DCC 0x%08" PRIx32, *data);
 
 	if (dscr_p)
 		*dscr_p = dscr;
@@ -143,7 +134,7 @@ static int dpmv8_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 		if (retval != ERROR_OK)
 			return retval;
 		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for read dcc");
+			LOG_ERROR("Timeout waiting for DTR_TX_FULL, dscr = 0x%08" PRIx32, dscr);
 			return ERROR_FAIL;
 		}
 	}
@@ -161,7 +152,6 @@ static int dpmv8_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 		return retval;
 
 	*data = *(uint32_t *)data | (uint64_t)higher << 32;
-	LOG_DEBUG("read DCC 0x%16.16" PRIx64, *data);
 
 	if (dscr_p)
 		*dscr_p = dscr;
@@ -175,7 +165,7 @@ static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
 	uint32_t dscr;
 	int retval;
 
-	/* set up invariant:  INSTR_COMP is set after ever DPM operation */
+	/* set up invariant:  ITE is set after ever DPM operation */
 	long long then = timeval_ms();
 	for (;; ) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -217,11 +207,9 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t *p_dscr)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
+	uint32_t dscr = dpm->dscr;
 	int retval;
 
-	LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
-
 	if (p_dscr)
 		dscr = *p_dscr;
 
@@ -334,19 +322,21 @@ static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
-	retval = dpmv8_write_dcc_64(armv8, data);
-	if (retval != ERROR_OK)
-		return retval;
+	if (dpm->arm->core_state != ARM_STATE_AARCH64)
+		return dpmv8_instr_write_data_r0(dpm, opcode, data);
 
-	retval = dpmv8_exec_opcode(dpm, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
-	if (retval != ERROR_OK)
-		return retval;
+	/* transfer data from DCC to R0 */
+	retval = dpmv8_write_dcc_64(armv8, data);
+	if (retval == ERROR_OK)
+		retval = dpmv8_exec_opcode(dpm, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
 
 	/* then the opcode, taking data from R0 */
-	return dpmv8_exec_opcode(dpm, opcode, &dscr);
+	if (retval == ERROR_OK)
+		retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
+
+	return retval;
 }
 
 static int dpmv8_instr_cpsr_sync(struct arm_dpm *dpm)
@@ -365,70 +355,74 @@ static int dpmv8_instr_read_data_dcc(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to DCC */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc(armv8, data, &dscr);
+	return dpmv8_read_dcc(armv8, data, &dpm->dscr);
 }
 
 static int dpmv8_instr_read_data_dcc_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to DCC */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc_64(armv8, data, &dscr);
+	return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
 }
 
 static int dpmv8_instr_read_data_r0(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to R0 */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* write R0 to DCC */
-	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, WRITE_REG_DTRTX), &dscr);
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, WRITE_REG_DTRTX), &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc(armv8, data, &dscr);
+	return dpmv8_read_dcc(armv8, data, &dpm->dscr);
 }
 
 static int dpmv8_instr_read_data_r0_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
+	if (dpm->arm->core_state != ARM_STATE_AARCH64) {
+		uint32_t tmp;
+		retval = dpmv8_instr_read_data_r0(dpm, opcode, &tmp);
+		if (retval == ERROR_OK)
+			*data = tmp;
+		return retval;
+	}
+
 	/* the opcode, writing data to R0 */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* write R0 to DCC */
-	retval = dpmv8_exec_opcode(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+	retval = dpmv8_exec_opcode(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc_64(armv8, data, &dscr);
+	return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
 }
 
 #if 0
@@ -545,62 +539,6 @@ static int dpmv8_mcr(struct target *target, int cpnum,
 	return retval;
 }
 
-static int dpmv8_mrs(struct target *target, uint32_t op0,
-	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-	uint32_t *value)
-{
-	struct arm *arm = target_to_arm(target);
-	struct arm_dpm *dpm = arm->dpm;
-	int retval;
-	uint32_t op_code;
-
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		return retval;
-	op_code = ((op0 & 0x3) << 19 | (op1 & 0x7) << 16 | (CRn & 0xF) << 12 |\
-				(CRm & 0xF) << 8 | (op2 & 0x7) << 5);
-	op_code >>= 5;
-	LOG_DEBUG("MRS p%d, %d, r0, c%d, c%d, %d", (int)op0,
-		(int) op1, (int) CRn,
-		(int) CRm, (int) op2);
-	/* read coprocessor register into R0; return via DCC */
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(op_code, 0),
-			value);
-
-	/* (void) */ dpm->finish(dpm);
-	return retval;
-}
-
-static int dpmv8_msr(struct target *target, uint32_t op0,
-	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-	uint32_t value)
-{
-	struct arm *arm = target_to_arm(target);
-	struct arm_dpm *dpm = arm->dpm;
-	int retval;
-	uint32_t op_code;
-
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		return retval;
-
-	op_code = ((op0 & 0x3) << 19 | (op1 & 0x7) << 16 | (CRn & 0xF) << 12 |\
-				(CRm & 0xF) << 8 | (op2 & 0x7) << 5);
-	op_code >>= 5;
-	LOG_DEBUG("MSR p%d, %d, r0, c%d, c%d, %d", (int)op0,
-		(int) op1, (int) CRn,
-		(int) CRm, (int) op2);
-
-	/* read DCC into r0; then write coprocessor register from R0 */
-	retval = dpm->instr_write_data_r0(dpm,
-			ARMV8_MSR_GP(op_code, 0),
-			value);
-
-	/* (void) */ dpm->finish(dpm);
-	return retval;
-}
-
 /*----------------------------------------------------------------------*/
 
 /*
@@ -780,14 +718,22 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 	cache = arm->core_cache;
 
 	/* read R0 first (it's used for scratch), then CPSR */
-	r = cache->reg_list + 0;
+	r = cache->reg_list + ARMV8_R0;
 	if (!r->valid) {
-		retval = dpmv8_read_reg(dpm, r, 0);
+		retval = dpmv8_read_reg(dpm, r, ARMV8_R0);
 		if (retval != ERROR_OK)
 			goto fail;
 	}
 	r->dirty = true;
 
+	/* read R1, too, it will be clobbered during memory access */
+	r = cache->reg_list + ARMV8_R1;
+	if (!r->valid) {
+		retval = dpmv8_read_reg(dpm, r, ARMV8_R1);
+		if (retval != ERROR_OK)
+			goto fail;
+	}
+
 	/* read cpsr to r0 and get it back */
 	retval = dpm->instr_read_data_r0(dpm,
 			armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
@@ -797,7 +743,7 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 	/* update core mode and state */
 	armv8_set_cpsr(arm, cpsr);
 
-	for (unsigned int i = 1; i < cache->num_regs ; i++) {
+	for (unsigned int i = ARMV8_PC; i < cache->num_regs ; i++) {
 		struct arm_reg *arm_reg;
 
 		r = armv8_reg_current(arm, i);
@@ -1305,7 +1251,7 @@ void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
 			/* ?? */
 			break;
 		default:
-			LOG_DEBUG("Unknow core_state");
+			LOG_DEBUG("Unknown core_state");
 			break;
 	}
 	dpm->wp_pc = addr;
@@ -1366,6 +1312,8 @@ void armv8_dpm_handle_exception(struct arm_dpm *dpm)
 	core_state = armv8_dpm_get_core_state(dpm);
 	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
 	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
+
+	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 }
 
 /*----------------------------------------------------------------------*/
@@ -1443,8 +1391,6 @@ int armv8_dpm_setup(struct arm_dpm *dpm)
 	/* coprocessor access setup */
 	arm->mrc = dpmv8_mrc;
 	arm->mcr = dpmv8_mcr;
-	arm->mrs = dpmv8_mrs;
-	arm->msr = dpmv8_msr;
 
 	dpm->prepare = dpmv8_dpm_prepare;
 	dpm->finish = dpmv8_dpm_finish;