aarch64: fix software breakpoints when in aarch32 state

Use the correct opcode for Aarch32 state, both for the breakpoint
instruction itself and the cache handling functions.

Change-Id: I975fa67b1e577b54f5c672a01d516419c6a614b2
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3981
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>

5 files changed, 31 insertions, 6 deletions

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 0a6b67f..833dc7a 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -915,7 +915,7 @@ static int aarch64_set_breakpoint(struct target *target,
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
 
-		buf_set_u32(code, 0, 32, ARMV8_HLT(0x11));
+		buf_set_u32(code, 0, 32, armv8_opcode(armv8, ARMV8_OPC_HLT));
 		retval = target_read_memory(target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1,
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
index 31e4c79..7f610c9 100644
--- a/src/target/armv8_cache.c
+++ b/src/target/armv8_cache.c
@@ -49,8 +49,9 @@ static int armv8_i_cache_sanity_check(struct armv8_common *armv8)
 	return ERROR_TARGET_INVALID;
 }
 
-static int armv8_cache_d_inner_flush_level(struct arm_dpm *dpm, struct armv8_cachesize *size, int cl)
+static int armv8_cache_d_inner_flush_level(struct armv8_common *armv8, struct armv8_cachesize *size, int cl)
 {
+	struct arm_dpm *dpm = armv8->arm.dpm;
 	int retval = ERROR_OK;
 	int32_t c_way, c_index = size->index;
 
@@ -65,7 +66,7 @@ static int armv8_cache_d_inner_flush_level(struct arm_dpm *dpm, struct armv8_cac
 			 * line by Set/Way.
 			 */
 			retval = dpm->instr_write_data_r0(dpm,
-					ARMV8_SYS(SYSTEM_DCCISW, 0), value);
+					armv8_opcode(armv8, ARMV8_OPC_DCCISW), value);
 			if (retval != ERROR_OK)
 				goto done;
 			c_way -= 1;
@@ -97,7 +98,7 @@ static int armv8_cache_d_inner_clean_inval_all(struct armv8_common *armv8)
 		if (cache->arch[cl].ctype < CACHE_LEVEL_HAS_D_CACHE)
 			continue;
 
-		armv8_cache_d_inner_flush_level(dpm, &cache->arch[cl].d_u_size, cl);
+		armv8_cache_d_inner_flush_level(armv8, &cache->arch[cl].d_u_size, cl);
 	}
 
 	retval = dpm->finish(dpm);
@@ -133,7 +134,7 @@ int armv8_cache_d_inner_flush_virt(struct armv8_common *armv8, target_addr_t va,
 		/* DC CIVAC */
 		/* Aarch32: DCCIMVAC: ARMV4_5_MCR(15, 0, 0, 7, 14, 1) */
 		retval = dpm->instr_write_data_r0_64(dpm,
-				ARMV8_SYS(SYSTEM_DCCIVAC, 0), va_line);
+				armv8_opcode(armv8, ARMV8_OPC_DCCIVAC), va_line);
 		if (retval != ERROR_OK)
 			goto done;
 		va_line += linelen;
@@ -171,7 +172,7 @@ int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va,
 	while (va_line < va_end) {
 		/* IC IVAU - Invalidate instruction cache by VA to PoU. */
 		retval = dpm->instr_write_data_r0_64(dpm,
-				ARMV8_SYS(SYSTEM_ICIVAU, 0), va_line);
+				armv8_opcode(armv8, ARMV8_OPC_ICIVAU), va_line);
 		if (retval != ERROR_OK)
 			goto done;
 		va_line += linelen;
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index b4c14c2..b06e456 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -336,6 +336,9 @@ static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
 	struct armv8_common *armv8 = dpm->arm->arch_info;
 	int retval;
 
+	if (dpm->arm->core_state != ARM_STATE_AARCH64)
+		return dpmv8_instr_write_data_r0(dpm, opcode, data);
+
 	/* transfer data from DCC to R0 */
 	retval = dpmv8_write_dcc_64(armv8, data);
 	if (retval == ERROR_OK)
@@ -413,6 +416,14 @@ static int dpmv8_instr_read_data_r0_64(struct arm_dpm *dpm,
 	struct armv8_common *armv8 = dpm->arm->arch_info;
 	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, &dpm->dscr);
 	if (retval != ERROR_OK)
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 7790144..d3c0b3f 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -38,6 +38,10 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCPS]	= ARMV8_DCPS(0, 11),
 		[ARMV8_OPC_DRPS]	= ARMV8_DRPS,
 		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB,
+		[ARMV8_OPC_DCCISW]	= ARMV8_SYS(SYSTEM_DCCISW, 0),
+		[ARMV8_OPC_DCCIVAC]	= ARMV8_SYS(SYSTEM_DCCIVAC, 0),
+		[ARMV8_OPC_ICIVAU]	= ARMV8_SYS(SYSTEM_ICIVAU, 0),
+		[ARMV8_OPC_HLT]		= ARMV8_HLT(11),
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -55,6 +59,10 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCPS]	= ARMV8_DCPS_T1(0),
 		[ARMV8_OPC_DRPS]	= ARMV8_ERET_T1,
 		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB_SY_T1,
+		[ARMV8_OPC_DCCISW]	= ARMV4_5_MCR(15, 0, 0, 7, 14, 2),
+		[ARMV8_OPC_DCCIVAC]	= ARMV4_5_MCR(15, 0, 0, 7, 14, 1),
+		[ARMV8_OPC_ICIVAU]	= ARMV4_5_MCR(15, 0, 0, 7, 5, 1),
+		[ARMV8_OPC_HLT]		= ARMV8_HLT_A1(11),
 };
 
 void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 53dcb7e..94694bc 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -148,6 +148,7 @@
 
 #define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5))
 #define ARMV8_HLT(Im) (0x0D4400000 | ((Im & 0xffff) << 5))
+#define ARMV8_HLT_A1(Im) (0xE1000070 | ((Im & 0xFFF0) << 4) | (Im & 0xF))
 
 #define ARMV8_MOVFSP_64(Rt) ((1 << 31) | 0x11000000 | (0x1f << 5) | (Rt))
 #define ARMV8_MOVTSP_64(Rt) ((1 << 31) | 0x11000000 | (Rt << 5) | (0x1F))
@@ -171,6 +172,10 @@ enum armv8_opcode {
 	ARMV8_OPC_DCPS,
 	ARMV8_OPC_DRPS,
 	ARMV8_OPC_ISB_SY,
+	ARMV8_OPC_DCCISW,
+	ARMV8_OPC_DCCIVAC,
+	ARMV8_OPC_ICIVAU,
+	ARMV8_OPC_HLT,
 	ARMV8_OPC_NUM,
 };