target/armv8: Handle instruction cache invalidate

Some armv8 target have separate i-cache and d-cache.
The actual code only handles the flush of the d-cache.

Change-Id: I61a223b43c71646bbbed8fa63825360c67700988
Signed-off-by: Adrien Grassein <agrassein@nanoxplore.com>
Signed-off-by: Adrien Charruel <acharruel@nanoxplore.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/8655
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
Tested-by: jenkins

4 files changed, 68 insertions, 0 deletions

diff --git a/src/target/armv8.h b/src/target/armv8.h
index 49ab3e5..dba12f9 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -162,6 +162,7 @@ struct armv8_cache_common {
 	/* l2 external unified cache if some */
 	void *l2_cache;
 	int (*flush_all_data_cache)(struct target *target);
+	int (*invalidate_all_instruction_cache)(struct target *target);
 	int (*display_cache_info)(struct command_invocation *cmd,
 			struct armv8_cache_common *armv8_cache);
 };
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
index 74d063b..1c251be 100644
--- a/src/target/armv8_cache.c
+++ b/src/target/armv8_cache.c
@@ -141,6 +141,36 @@ done:
 	return retval;
 }
 
+static int armv8_cache_i_inner_clean_inval_all(struct armv8_common *armv8)
+{
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	int retval;
+
+	retval = armv8_i_cache_sanity_check(armv8);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("flushing cache");
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_ICIALLU));
+	if (retval != ERROR_OK)
+		goto done;
+
+	dpm->finish(dpm);
+	LOG_DEBUG("flushing cache done");
+	return retval;
+
+done:
+	LOG_ERROR("i-cache invalidate failed");
+	dpm->finish(dpm);
+
+	return retval;
+}
+
 int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va, size_t size)
 {
 	struct arm_dpm *dpm = armv8->arm.dpm;
@@ -253,6 +283,32 @@ static int  armv8_flush_all_data(struct target *target)
 	return retval;
 }
 
+static int  armv8_flush_all_instruction(struct target *target)
+{
+	int retval = ERROR_FAIL;
+	/*  check that armv8_cache is correctly identify */
+	struct armv8_common *armv8 = target_to_armv8(target);
+	if (armv8->armv8_mmu.armv8_cache.info == -1) {
+		LOG_ERROR("trying to flush un-identified cache");
+		return retval;
+	}
+
+	if (target->smp) {
+		/* look if all the other target have been flushed in order to flush icache */
+		struct target_list *head;
+		foreach_smp_target(head, target->smp_targets) {
+			struct target *curr = head->target;
+			if (curr->state == TARGET_HALTED) {
+				LOG_TARGET_INFO(curr, "Wait flushing instruction l1.");
+				retval = armv8_cache_i_inner_clean_inval_all(target_to_armv8(curr));
+			}
+		}
+	} else {
+		retval = armv8_cache_i_inner_clean_inval_all(armv8);
+	}
+	return retval;
+}
+
 static int get_cache_info(struct arm_dpm *dpm, int cl, int ct, uint32_t *cache_reg)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
@@ -412,6 +468,12 @@ int armv8_identify_cache(struct armv8_common *armv8)
 		armv8->armv8_mmu.armv8_cache.flush_all_data_cache =
 			armv8_flush_all_data;
 	}
+	if (!armv8->armv8_mmu.armv8_cache.invalidate_all_instruction_cache) {
+		armv8->armv8_mmu.armv8_cache.display_cache_info =
+			armv8_handle_inner_cache_info_command;
+		armv8->armv8_mmu.armv8_cache.invalidate_all_instruction_cache =
+			armv8_flush_all_instruction;
+	}
 
 done:
 	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 2635b3e..0f6c810 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -41,6 +41,7 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_STRH_IP]	= ARMV8_STRH_IP(1, 0),
 		[ARMV8_OPC_STRW_IP]	= ARMV8_STRW_IP(1, 0),
 		[ARMV8_OPC_STRD_IP]	= ARMV8_STRD_IP(1, 0),
+		[ARMV8_OPC_ICIALLU]	= ARMV8_SYS(SYSTEM_ICIALLU, 0x1F),
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -68,6 +69,7 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_STRB_IP]	= ARMV8_STRB_IP_T3(1, 0),
 		[ARMV8_OPC_STRH_IP]	= ARMV8_STRH_IP_T3(1, 0),
 		[ARMV8_OPC_STRW_IP]	= ARMV8_STRW_IP_T3(1, 0),
+		[ARMV8_OPC_ICIALLU]	= ARMV4_5_MCR(15, 0, 0, 7, 5, 0),
 };
 
 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 9200dac..9f18e94 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -72,6 +72,8 @@
 #define SYSTEM_DCCISW			0x43F2
 #define SYSTEM_DCCSW			0x43D2
 #define SYSTEM_ICIVAU			0x5BA9
+/* Attention, SYSTEM_ICIALLU requires rt=0x1f */
+#define SYSTEM_ICIALLU			0x03A8
 #define SYSTEM_DCCVAU			0x5BD9
 #define SYSTEM_DCCIVAC			0x5BF1
 
@@ -207,6 +209,7 @@ enum armv8_opcode {
 	ARMV8_OPC_LDRH_IP,
 	ARMV8_OPC_LDRW_IP,
 	ARMV8_OPC_LDRD_IP,
+	ARMV8_OPC_ICIALLU,
 	ARMV8_OPC_NUM,
 };