From f03565a8eb469f34cb308d64fc0d93e1052904bf Mon Sep 17 00:00:00 2001
From: Icenowy Zheng <uwu@icenowy.me>
Date: Wed, 6 Jul 2022 04:54:15 +0800
Subject: [PATCH] fel: implement I-Cache hack

On some new Allwinner SoCs, I-Cache is enabled by BROM but FEL write
command cannot correctly invalidate I-Cache, so thunks will not get
properly executed.

Implement a hack that tries to disable I-Cache each time trying to write
data via FEL, to prevent stall thunks in I-Cache.

Signed-off-by: Icenowy Zheng <uwu@icenowy.me>
---
 fel_lib.c  | 30 +++++++++++++++++++++++++++++-
 soc_info.h |  2 ++
 2 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/fel_lib.c b/fel_lib.c
index f038224..2db6154 100644
--- a/fel_lib.c
+++ b/fel_lib.c
@@ -40,6 +40,7 @@ struct _felusb_handle {
 	libusb_device_handle *handle;
 	int endpoint_out, endpoint_in;
 	bool iface_detached;
+	bool icache_hacked;
 };
 
 /* a helper function to report libusb errors */
@@ -214,7 +215,7 @@ void aw_fel_read(feldev_handle *dev, uint32_t offset, void *buf, size_t len)
 }
 
 /* AW_FEL_1_WRITE request */
-void aw_fel_write(feldev_handle *dev, const void *buf, uint32_t offset, size_t len)
+static void aw_fel_write_raw(feldev_handle *dev, const void *buf, uint32_t offset, size_t len)
 {
 	if (len == 0)
 		return;
@@ -231,6 +232,33 @@ void aw_fel_execute(feldev_handle *dev, uint32_t offset)
 	aw_read_fel_status(dev);
 }
 
+static void aw_disable_icache(feldev_handle *dev)
+{
+	soc_info_t *soc_info = dev->soc_info;
+	uint32_t arm_code[] = {
+		/* Clear SCTLR.I */
+		htole32(0xee110f10), /* mrc 15, 0, r0, cr1, cr0, {0} ;SCTLR */
+		htole32(0xe3c00a01), /* bic r0, r0, #0x1000 */
+		htole32(0xee010f10), /* mcr 15, 0, r0, cr1, cr0, {0} ;SCTLR */
+		/* Invalidate I-Cache */
+		htole32(0xee070f15), /* mcr 15, 0, r0, cr7, cr5, {0} ;ICIALLU */
+		/* Barrier to force instruction refetching */
+		htole32(0xf57ff06f), /* isb sy */
+		htole32(0xe12fff1e), /* bx  lr */
+	};
+	aw_fel_write_raw(dev, arm_code, soc_info->scratch_addr, sizeof(arm_code));
+	aw_fel_execute(dev, soc_info->scratch_addr);
+}
+
+void aw_fel_write(feldev_handle *dev, const void *buf, uint32_t offset, size_t len)
+{
+	if (dev->soc_info->icache_fix && !dev->usb->icache_hacked) {
+		aw_disable_icache(dev);
+		dev->usb->icache_hacked = true;
+	}
+	aw_fel_write_raw(dev, buf, offset, len);
+}
+
 /*
  * This function is a higher-level wrapper for the FEL write functionality.
  * Unlike aw_fel_write() above - which is reserved for internal use - this
diff --git a/soc_info.h b/soc_info.h
index 5609fe6..f7ab8e5 100644
--- a/soc_info.h
+++ b/soc_info.h
@@ -113,6 +113,8 @@ typedef struct {
 	uint32_t           rvbar_reg;    /* MMIO address of RVBARADDR0_L register */
 	const watchdog_info *watchdog;   /* Used for reset */
 	bool               sid_fix;      /* Use SID workaround (read via register) */
+	/* Use I$ workaround (disable I$ before first write to prevent stale thunk */
+	bool               icache_fix;
 	/* Use SMC workaround (enter secure mode) if can't read from this address */
 	uint32_t           needs_smc_workaround_if_zero_word_at_addr;
 	uint32_t           sram_size;	/* Usable contiguous SRAM at spl_addr */