X-Git-Url: https://review.openocd.org/gitweb?a=blobdiff_plain;f=src%2Ftarget%2Farmv8_dpm.c;h=91b2f51718e6c0ac45feeea19764a7baf7eb628b;hb=a48264414e53d99ffe69df0687abf1effb13be22;hp=f4e7a07996c651546c92812b04f4f085a76305cd;hpb=9981093ce076a388700b2f307a9dc79e14a6e0f1;p=openocd.git

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index f4e7a07996..91b2f51718 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -561,12 +561,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 
 	} 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;
+		cpsr = mode;
 	}
 
 	switch (cpsr & 0x1f) {
@@ -655,21 +650,37 @@ 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);
 
-	retval = armv8->read_reg_u64(armv8, regnum, &value_64);
+		if (retval == ERROR_OK) {
+			r->valid = true;
+			r->dirty = false;
 
-	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);
+			buf_set_u64(r->value, 0, 64, lvalue);
+			buf_set_u64(r->value + 8, 0, r->size - 64, hvalue);
+
+			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;
+	return retval;
 }
 
 /*
@@ -679,20 +690,36 @@ 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;
+
+		value_64 = buf_get_u64(r->value, 0, r->size);
+		retval = armv8->write_reg_u64(armv8, regnum, 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);
+		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;
+	return retval;
 }
 
 /**
@@ -750,6 +777,10 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 		if (r->valid)
 			continue;
 
+		/* Skip reading FP-SIMD registers */
+		if (r->number >= ARMV8_V0 && r->number <= ARMV8_FPCR)
+			continue;
+
 		/*
 		 * Only read registers that are available from the
 		 * current EL (or core mode).