X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Farmv8_dpm.c;h=1e88a445fdbc8bcd36f782ae82a356c1da3f79b9;hp=e5d10df6e791d6a6e16119bba3d446d230e15e1c;hb=HEAD;hpb=b73628141a932881953bb816ec7bfa47d9e40680

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index e5d10df6e7..8bb24f225b 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -1,16 +1,7 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
 /*
  * Copyright (C) 2009 by David Brownell
- *
- * 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
@@ -55,7 +46,7 @@ enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
 	dpm->last_el = el;
 
 	/* In Debug state, each bit gives the current Execution state of each EL */
-	if ((rw >> el) & 0b1)
+	if ((rw >> el) & 1)
 		return ARM_STATE_AARCH64;
 
 	return ARM_STATE_ARM;
@@ -253,12 +244,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;
 	}
 
@@ -283,7 +274,7 @@ static int dpmv8_instr_write_data_dcc(struct arm_dpm *dpm,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_exec_opcode(dpm, opcode, 0);
+	return dpmv8_exec_opcode(dpm, opcode, NULL);
 }
 
 static int dpmv8_instr_write_data_dcc_64(struct arm_dpm *dpm,
@@ -296,7 +287,7 @@ static int dpmv8_instr_write_data_dcc_64(struct arm_dpm *dpm,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_exec_opcode(dpm, opcode, 0);
+	return dpmv8_exec_opcode(dpm, opcode, NULL);
 }
 
 static int dpmv8_instr_write_data_r0(struct arm_dpm *dpm,
@@ -490,7 +481,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 +493,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 +506,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 +518,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,13 +551,8 @@ 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);
-
-		/* else force to the specified mode */
-		if (is_arm_mode(mode))
-			cpsr = mode;
-		else
-			cpsr = mode >> 4;
+		LOG_DEBUG("setting mode 0x%x", mode);
+		cpsr = mode;
 	}
 
 	switch (cpsr & 0x1f) {
@@ -578,6 +564,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,12 +587,15 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	}
 
 	LOG_DEBUG("target_el = %i, last_el = %i", target_el, dpm->last_el);
+	if (dpm->last_el == target_el)
+		return ERROR_OK; /* nothing to do */
+
 	if (target_el > dpm->last_el) {
 		retval = dpm->instr_execute(dpm,
 				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) {
@@ -614,7 +604,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 				/* load SPSR with the desired mode and execute DRPS */
 				LOG_DEBUG("SPSR = 0x%08"PRIx32, cpsr);
 				retval = dpm->instr_write_data_r0(dpm,
-						ARMV8_MSR_GP_xPSR_T1(1, 0, 15), cpsr);
+						ARMV8_MSR_GP_XPSR_T1(1, 0, 15), cpsr);
 				if (retval == ERROR_OK)
 					retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
 			}
@@ -655,21 +645,41 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint64_t value_64;
-	int retval;
+	int retval = ERROR_FAIL;
+
+	if (r->size <= 64) {
+		uint64_t value_64;
+		retval = armv8->read_reg_u64(armv8, regnum, &value_64);
+
+		if (retval == ERROR_OK) {
+			r->valid = true;
+			r->dirty = false;
+			buf_set_u64(r->value, 0, r->size, value_64);
+			if (r->size == 64)
+				LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
+			else
+				LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned int) value_64);
+		}
+	} else if (r->size <= 128) {
+		uint64_t lvalue = 0, hvalue = 0;
+		retval = armv8->read_reg_u128(armv8, regnum, &lvalue, &hvalue);
+
+		if (retval == ERROR_OK) {
+			r->valid = true;
+			r->dirty = false;
 
-	retval = armv8->read_reg_u64(armv8, regnum, &value_64);
+			buf_set_u64(r->value, 0, 64, lvalue);
+			buf_set_u64(r->value + 8, 0, r->size - 64, hvalue);
 
-	if (retval == ERROR_OK) {
-		r->valid = true;
-		r->dirty = false;
-		buf_set_u64(r->value, 0, r->size, value_64);
-		if (r->size == 64)
-			LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
-		else
-			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned int) value_64);
+			LOG_DEBUG("READ: %s, lvalue=%16.8llx", r->name, (unsigned long long) lvalue);
+			LOG_DEBUG("READ: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
+		}
 	}
-	return ERROR_OK;
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Failed to read %s register", r->name);
+
+	return retval;
 }
 
 /*
@@ -679,24 +689,44 @@ 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;
-	uint64_t value_64;
 
-	value_64 = buf_get_u64(r->value, 0, r->size);
+	if (r->size <= 64) {
+		uint64_t value_64;
 
-	retval = armv8->write_reg_u64(armv8, regnum, value_64);
-	if (retval == ERROR_OK) {
-		r->dirty = false;
-		if (r->size == 64)
-			LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
-		else
-			LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
+		value_64 = buf_get_u64(r->value, 0, r->size);
+		retval = armv8->write_reg_u64(armv8, regnum, value_64);
+
+		if (retval == ERROR_OK) {
+			r->dirty = false;
+			if (r->size == 64)
+				LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
+			else
+				LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
+		}
+	} else if (r->size <= 128) {
+		uint64_t lvalue, hvalue;
+
+		lvalue = buf_get_u64(r->value, 0, 64);
+		hvalue = buf_get_u64(r->value + 8, 0, r->size - 64);
+		retval = armv8->write_reg_u128(armv8, regnum, lvalue, hvalue);
+
+		if (retval == ERROR_OK) {
+			r->dirty = false;
+
+			LOG_DEBUG("WRITE: %s, lvalue=%16.8llx", r->name, (unsigned long long) lvalue);
+			LOG_DEBUG("WRITE: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
+		}
 	}
 
-	return ERROR_OK;
+	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 in AArch32
+ * state or R0 to R31, PC and CPSR in AArch64 state;
  * 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
@@ -743,11 +773,21 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 	/* update core mode and state */
 	armv8_set_cpsr(arm, cpsr);
 
-	for (unsigned int i = ARMV8_PC; i < cache->num_regs ; i++) {
+	/* read the remaining registers that would be required by GDB 'g' packet */
+	for (unsigned int i = ARMV8_R2; i <= ARMV8_PC ; i++) {
 		struct arm_reg *arm_reg;
 
+		/* in AArch32 skip AArch64 registers */
+		/* TODO: this should be detected below through arm_reg->mode */
+		if (arm->core_state != ARM_STATE_AARCH64 && i > ARMV8_R14 && i < ARMV8_PC)
+			continue;
+
 		r = armv8_reg_current(arm, i);
-		if (r->valid)
+		if (!r->exist || r->valid)
+			continue;
+
+		/* Skip reading FP-SIMD registers */
+		if (r->number >= ARMV8_V0 && r->number <= ARMV8_FPCR)
 			continue;
 
 		/*
@@ -759,6 +799,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;
@@ -775,7 +819,7 @@ fail:
  * or running debugger code.
  */
 static int dpmv8_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
-	struct dpm_bpwp *xp, int *set_p)
+	struct dpm_bpwp *xp, bool *set_p)
 {
 	int retval = ERROR_OK;
 	bool disable;
@@ -849,7 +893,7 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 			struct breakpoint *bp = dbp->bp;
 
 			retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
-					bp ? &bp->set : NULL);
+					bp ? &bp->is_set : NULL);
 			if (retval != ERROR_OK)
 				goto done;
 		}
@@ -861,7 +905,7 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		struct watchpoint *wp = dwp->wp;
 
 		retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
-				wp ? &wp->set : NULL);
+				wp ? &wp->is_set : NULL);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -879,8 +923,11 @@ 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)
+		if (i == ARMV8_PC || i == ARMV8_XPSR)
 			continue;
 		/* skip invalid */
 		if (!cache->reg_list[i].valid)
@@ -902,7 +949,7 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 
 	/* flush CPSR and PC */
 	if (retval == ERROR_OK)
-		retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_xPSR], ARMV8_xPSR);
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_XPSR], ARMV8_XPSR);
 	if (retval == ERROR_OK)
 		retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_PC], ARMV8_PC);
 	/* flush R0 -- it's *very* dirty by now */
@@ -1004,7 +1051,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;
 
@@ -1170,7 +1217,7 @@ static int dpmv8_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
 	uint32_t control;
 
 	/* this hardware doesn't support data value matching or masking */
-	if (wp->value || wp->mask != ~(uint32_t)0) {
+	if (wp->mask != WATCHPOINT_IGNORE_DATA_VALUE_MASK) {
 		LOG_DEBUG("watchpoint values and masking not supported");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
@@ -1236,27 +1283,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
@@ -1267,7 +1293,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;
@@ -1277,9 +1303,9 @@ void armv8_dpm_handle_exception(struct arm_dpm *dpm)
 	unsigned int el;
 
 	static const int clobbered_regs_by_el[3][5] = {
-		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL1, ARMV8_ESR_EL1, ARMV8_SPSR_EL1 },
-		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL2, ARMV8_ESR_EL2, ARMV8_SPSR_EL2 },
-		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL3, ARMV8_ESR_EL3, ARMV8_SPSR_EL3 },
+		{ ARMV8_PC, ARMV8_XPSR, ARMV8_ELR_EL1, ARMV8_ESR_EL1, ARMV8_SPSR_EL1 },
+		{ ARMV8_PC, ARMV8_XPSR, ARMV8_ELR_EL2, ARMV8_ESR_EL2, ARMV8_SPSR_EL2 },
+		{ ARMV8_PC, ARMV8_XPSR, ARMV8_ELR_EL3, ARMV8_ESR_EL3, ARMV8_SPSR_EL3 },
 	};
 
 	el = (dpm->dscr >> 8) & 3;
@@ -1294,7 +1320,7 @@ void armv8_dpm_handle_exception(struct arm_dpm *dpm)
 	mem_ap_write_u32(armv8->debug_ap,
 		armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 
-	armv8->read_reg_u64(armv8, ARMV8_xPSR, &dlr);
+	armv8->read_reg_u64(armv8, ARMV8_XPSR, &dlr);
 	dspsr = dlr;
 	armv8->read_reg_u64(armv8, ARMV8_PC, &dlr);
 
@@ -1312,6 +1338,9 @@ 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);
+
+	if (do_restore)
+		armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 }
 
 /*----------------------------------------------------------------------*/
@@ -1342,13 +1371,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;
@@ -1380,7 +1411,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;
@@ -1417,16 +1448,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);