X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftarget%2Farmv8_dpm.c;h=188e58822a24afa34fbdd7f0a42d75d4b3f3f467;hb=a0bd3c9924870c3b8f428648410181040dabc33c;hp=91b2f51718e6c0ac45feeea19764a7baf7eb628b;hpb=a48264414e53d99ffe69df0687abf1effb13be22;p=fw%2Fopenocd

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 91b2f5171..188e58822 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -253,12 +253,12 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 	/* 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);
+		LOG_DEBUG("EL %i -> %" PRIu32, 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);
-		armv8_dpm_handle_exception(dpm);
+		LOG_ERROR("Opcode 0x%08" PRIx32 ", DSCR.ERR=1, DSCR.EL=%i", opcode, dpm->last_el);
+		armv8_dpm_handle_exception(dpm, true);
 		retval = ERROR_FAIL;
 	}
 
@@ -490,7 +490,7 @@ static int dpmv8_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
 
 /* Read coprocessor */
 static int dpmv8_mrc(struct target *target, int cpnum,
-	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t op1, uint32_t op2, uint32_t crn, uint32_t crm,
 	uint32_t *value)
 {
 	struct arm *arm = target_to_arm(target);
@@ -502,12 +502,12 @@ static int dpmv8_mrc(struct target *target, int cpnum,
 		return retval;
 
 	LOG_DEBUG("MRC p%d, %d, r0, c%d, c%d, %d", cpnum,
-		(int) op1, (int) CRn,
-		(int) CRm, (int) op2);
+		(int) op1, (int) crn,
+		(int) crm, (int) op2);
 
 	/* read coprocessor register into R0; return via DCC */
 	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2),
+			ARMV4_5_MRC(cpnum, op1, 0, crn, crm, op2),
 			value);
 
 	/* (void) */ dpm->finish(dpm);
@@ -515,7 +515,7 @@ static int dpmv8_mrc(struct target *target, int cpnum,
 }
 
 static int dpmv8_mcr(struct target *target, int cpnum,
-	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t op1, uint32_t op2, uint32_t crn, uint32_t crm,
 	uint32_t value)
 {
 	struct arm *arm = target_to_arm(target);
@@ -527,12 +527,12 @@ static int dpmv8_mcr(struct target *target, int cpnum,
 		return retval;
 
 	LOG_DEBUG("MCR p%d, %d, r0, c%d, c%d, %d", cpnum,
-		(int) op1, (int) CRn,
-		(int) CRm, (int) op2);
+		(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,
-			ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2),
+			ARMV4_5_MCR(cpnum, op1, 0, crn, crm, op2),
 			value);
 
 	/* (void) */ dpm->finish(dpm);
@@ -560,7 +560,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 		LOG_DEBUG("restoring mode, cpsr = 0x%08"PRIx32, cpsr);
 
 	} else {
-		LOG_DEBUG("setting mode 0x%"PRIx32, mode);
+		LOG_DEBUG("setting mode 0x%x", mode);
 		cpsr = mode;
 	}
 
@@ -573,6 +573,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	case ARM_MODE_ABT:
 	case ARM_MODE_IRQ:
 	case ARM_MODE_FIQ:
+	case ARM_MODE_SYS:
 		target_el = 1;
 		break;
 	/*
@@ -600,7 +601,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 				armv8_opcode(armv8, ARMV8_OPC_DCPS) | target_el);
 
 		/* DCPS clobbers registers just like an exception taken */
-		armv8_dpm_handle_exception(dpm);
+		armv8_dpm_handle_exception(dpm, false);
 	} else {
 		core_state = armv8_dpm_get_core_state(dpm);
 		if (core_state != ARM_STATE_AARCH64) {
@@ -680,6 +681,10 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 			LOG_DEBUG("READ: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
 		}
 	}
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Failed to read %s register", r->name);
+
 	return retval;
 }
 
@@ -719,11 +724,14 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 		}
 	}
 
+	if (retval != ERROR_OK)
+		LOG_ERROR("Failed to write %s register", r->name);
+
 	return retval;
 }
 
 /**
- * Read basic registers of the the current context:  R0 to R15, and CPSR;
+ * Read basic registers of the current context:  R0 to R15, and CPSR;
  * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
  * In normal operation this is called on entry to halting debug state,
  * possibly after some other operations supporting restore of debug state
@@ -774,7 +782,7 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 		struct arm_reg *arm_reg;
 
 		r = armv8_reg_current(arm, i);
-		if (r->valid)
+		if (!r->exist || r->valid)
 			continue;
 
 		/* Skip reading FP-SIMD registers */
@@ -790,6 +798,10 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 				dpm->last_el != armv8_curel_from_core_mode(arm_reg->mode))
 			continue;
 
+		/* Special case: ARM_MODE_SYS has no SPSR at EL1 */
+		if (r->number == ARMV8_SPSR_EL1 && arm->core_mode == ARM_MODE_SYS)
+			continue;
+
 		retval = dpmv8_read_reg(dpm, r, i);
 		if (retval != ERROR_OK)
 			goto fail;
@@ -910,6 +922,9 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 	for (unsigned i = 1; i < cache->num_regs; i++) {
 		struct arm_reg *r;
 
+		/* skip non-existent */
+		if (!cache->reg_list[i].exist)
+			continue;
 		/* skip PC and CPSR */
 		if (i == ARMV8_PC || i == ARMV8_xPSR)
 			continue;
@@ -1035,7 +1050,7 @@ static int armv8_dpm_full_context(struct target *target)
 		for (unsigned i = 0; i < cache->num_regs; i++) {
 			struct arm_reg *r;
 
-			if (cache->reg_list[i].valid)
+			if (!cache->reg_list[i].exist || cache->reg_list[i].valid)
 				continue;
 			r = cache->reg_list[i].arch_info;
 
@@ -1267,27 +1282,6 @@ static int dpmv8_remove_watchpoint(struct target *target, struct watchpoint *wp)
 	return retval;
 }
 
-void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
-{
-	switch (dpm->arm->core_state) {
-		case ARM_STATE_ARM:
-		case ARM_STATE_AARCH64:
-			addr -= 8;
-			break;
-		case ARM_STATE_THUMB:
-		case ARM_STATE_THUMB_EE:
-			addr -= 4;
-			break;
-		case ARM_STATE_JAZELLE:
-			/* ?? */
-			break;
-		default:
-			LOG_DEBUG("Unknown core_state");
-			break;
-	}
-	dpm->wp_pc = addr;
-}
-
 /*
  * Handle exceptions taken in debug state. This happens mostly for memory
  * accesses that violated a MMU policy. Taking an exception while in debug
@@ -1298,7 +1292,7 @@ void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
  * This function must not perform any actions that trigger another exception
  * or a recursion will happen.
  */
-void armv8_dpm_handle_exception(struct arm_dpm *dpm)
+void armv8_dpm_handle_exception(struct arm_dpm *dpm, bool do_restore)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
 	struct reg_cache *cache = dpm->arm->core_cache;
@@ -1344,7 +1338,8 @@ void armv8_dpm_handle_exception(struct arm_dpm *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);
+	if (do_restore)
+		armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 }
 
 /*----------------------------------------------------------------------*/
@@ -1375,13 +1370,15 @@ void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 		case DSCRV8_ENTRY_BKPT:	/* SW BKPT (?) */
 		case DSCRV8_ENTRY_RESET_CATCH:	/* Reset catch */
 		case DSCRV8_ENTRY_OS_UNLOCK:  /*OS unlock catch*/
-		case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
 		case DSCRV8_ENTRY_SW_ACCESS_DBG: /*SW access dbg register*/
 			target->debug_reason = DBG_REASON_BREAKPOINT;
 			break;
 		case DSCRV8_ENTRY_WATCHPOINT:	/* asynch watchpoint */
 			target->debug_reason = DBG_REASON_WATCHPOINT;
 			break;
+		case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
+			target->debug_reason = DBG_REASON_EXC_CATCH;
+			break;
 		default:
 			target->debug_reason = DBG_REASON_UNDEFINED;
 			break;
@@ -1413,7 +1410,7 @@ int armv8_dpm_setup(struct arm_dpm *dpm)
 	arm->read_core_reg = armv8_dpm_read_core_reg;
 	arm->write_core_reg = armv8_dpm_write_core_reg;
 
-	if (arm->core_cache == NULL) {
+	if (!arm->core_cache) {
 		cache = armv8_build_reg_cache(target);
 		if (!cache)
 			return ERROR_FAIL;
@@ -1450,16 +1447,18 @@ int armv8_dpm_setup(struct arm_dpm *dpm)
 	}
 
 	/* watchpoint setup */
-	target->type->add_watchpoint = dpmv8_add_watchpoint;
-	target->type->remove_watchpoint = dpmv8_remove_watchpoint;
+	if (!target->type->add_watchpoint) {
+		target->type->add_watchpoint = dpmv8_add_watchpoint;
+		target->type->remove_watchpoint = dpmv8_remove_watchpoint;
+	}
 
 	/* FIXME add vector catch support */
 
 	dpm->nbp = 1 + ((dpm->didr >> 12) & 0xf);
-	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
+	dpm->dbp = calloc(dpm->nbp, sizeof(*dpm->dbp));
 
 	dpm->nwp = 1 + ((dpm->didr >> 20) & 0xf);
-	dpm->dwp = calloc(dpm->nwp, sizeof *dpm->dwp);
+	dpm->dwp = calloc(dpm->nwp, sizeof(*dpm->dwp));
 
 	if (!dpm->dbp || !dpm->dwp) {
 		free(dpm->dbp);