[PATCH 1/1] riscv: ae350: enable Coherence Manager for ae350

[Leo Yu-Chi Liang <ycliang@andestech.com>]

If Coherence Manager were not set in the beginning,
u-boot-spl would sometimes fail to boot to u-boot proper.

Enable CM and I/D cache at the same time in harts_early_init

Signed-off-by: Leo Yu-Chi Liang <ycliang@andestech.com>
---
 arch/riscv/cpu/ax25/cpu.c | 42 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)

diff --git a/arch/riscv/cpu/ax25/cpu.c b/arch/riscv/cpu/ax25/cpu.c
index f092600e14..c4c2de2ef0 100644
--- a/arch/riscv/cpu/ax25/cpu.c
+++ b/arch/riscv/cpu/ax25/cpu.c
@@ -9,6 +9,22 @@
 #include <cpu_func.h>
 #include <irq_func.h>
 #include <asm/cache.h>
+#include <asm/csr.h>
+
+#define CSR_MCACHE_CTL	0x7ca
+#define CSR_MMISC_CTL	0x7d0
+#define CSR_MARCHID		0xf12
+
+#define V5_MCACHE_CTL_IC_EN_OFFSET      0
+#define V5_MCACHE_CTL_DC_EN_OFFSET      1
+#define V5_MCACHE_CTL_DC_COHEN_OFFSET	19
+#define V5_MCACHE_CTL_DC_COHSTA_OFFSET	20
+
+#define V5_MCACHE_CTL_IC_EN		BIT(V5_MCACHE_CTL_IC_EN_OFFSET)
+#define V5_MCACHE_CTL_DC_EN				BIT(V5_MCACHE_CTL_DC_EN_OFFSET)
+#define V5_MCACHE_CTL_DC_COHEN_EN       BIT(V5_MCACHE_CTL_DC_COHEN_OFFSET)
+#define V5_MCACHE_CTL_DC_COHSTA_EN      BIT(V5_MCACHE_CTL_DC_COHSTA_OFFSET)
+
 
 /*
  * cleanup_before_linux() is called just before we call linux
@@ -27,3 +43,29 @@ int cleanup_before_linux(void)
 
 	return 0;
 }
+
+void harts_early_init(void)
+{
+	if (CONFIG_IS_ENABLED(RISCV_MMODE)) {
+		unsigned long long mcache_ctl_val = csr_read(CSR_MCACHE_CTL);
+
+		if (!(mcache_ctl_val & V5_MCACHE_CTL_DC_COHEN_EN))
+			mcache_ctl_val |= V5_MCACHE_CTL_DC_COHEN_EN;
+		if (!(mcache_ctl_val & V5_MCACHE_CTL_IC_EN))
+			mcache_ctl_val |= V5_MCACHE_CTL_IC_EN;
+		if (!(mcache_ctl_val & V5_MCACHE_CTL_DC_EN))
+			mcache_ctl_val |= V5_MCACHE_CTL_DC_EN;
+		csr_write(CSR_MCACHE_CTL, mcache_ctl_val);
+
+		/*
+		 * Check DC_COHEN_EN, if cannot write to mcache_ctl,
+		 * we assume this bitmap not support L2 CM
+		 */
+		mcache_ctl_val = csr_read(CSR_MCACHE_CTL);
+		if ((mcache_ctl_val & V5_MCACHE_CTL_DC_COHEN_EN)) {
+		/* Wait for DC_COHSTA bit be set */
+			while (!(mcache_ctl_val & V5_MCACHE_CTL_DC_COHSTA_EN))
+				mcache_ctl_val = csr_read(CSR_MCACHE_CTL);
+		}
+	}
+}
-- 
2.17.0