[MIPS P8700 v6 7/7] lib: sbi_platform: Add the platform pma_set function to set up cacheability

[Chao-ying Fu]

From: Chao-ying Fu <cfu at wavecomp.com>

Before the corresponding PMP is changed, we allow platforms to set up PMA.
Add mips_p8700_pma_set.

Signed-off-by: Chao-ying Fu <cfu at mips.com>
---
 include/sbi/sbi_platform.h    | 19 ++++++++
 lib/sbi/riscv_asm.c           |  3 ++
 platform/generic/mips/p8700.c | 91 +++++++++++++++++++++++++++++++++++
 3 files changed, 113 insertions(+)

diff --git a/include/sbi/sbi_platform.h b/include/sbi/sbi_platform.h
index 08ece32..b3a5eda 100644
--- a/include/sbi/sbi_platform.h
+++ b/include/sbi/sbi_platform.h
@@ -142,6 +142,9 @@ struct sbi_platform_operations {
 	/** platform specific handler to fixup store fault */
 	int (*emulate_store)(int wlen, unsigned long addr,
 			     union sbi_ldst_data in_val);
+
+	/** platform specific pma set up */
+	void (*pma_set)(unsigned int n, unsigned long prot, unsigned long addr);
 };
 
 /** Platform default per-HART stack size for exception/interrupt handling */
@@ -644,6 +647,22 @@ static inline int sbi_platform_emulate_store(const struct sbi_platform *plat,
 	return SBI_ENOTSUPP;
 }
 
+/**
+ * PMA set up for cacheability based on the corresponding PMP
+ *
+ * @param plat pointer to struct sbi_platform
+ * @param n index of the pmp/pma entry
+ * @param prot attribute of the pmp entry
+ * @param addr address of the pmp entry
+ */
+static inline void sbi_platform_pma_set(const struct sbi_platform *plat,
+					unsigned int n, unsigned long prot,
+					unsigned long addr)
+{
+	if (plat && sbi_platform_ops(plat)->pma_set)
+		sbi_platform_ops(plat)->pma_set(n, prot, addr);
+}
+
 #endif
 
 #endif
diff --git a/lib/sbi/riscv_asm.c b/lib/sbi/riscv_asm.c
index c7d75ac..3b0cde5 100644
--- a/lib/sbi/riscv_asm.c
+++ b/lib/sbi/riscv_asm.c
@@ -312,6 +312,9 @@ int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
 	if (n >= PMP_COUNT || log2len > __riscv_xlen || log2len < PMP_SHIFT)
 		return SBI_EINVAL;
 
+	/* Allow the platform to set up PMA if needed */
+	sbi_platform_pma_set(sbi_platform_thishart_ptr(), n, prot, addr);
+
 	/* calculate PMP register and offset */
 #if __riscv_xlen == 32
 	pmpcfg_csr   = CSR_PMPCFG0 + (n >> 2);
diff --git a/platform/generic/mips/p8700.c b/platform/generic/mips/p8700.c
index 6b68717..8ca497f 100644
--- a/platform/generic/mips/p8700.c
+++ b/platform/generic/mips/p8700.c
@@ -16,6 +16,96 @@
 #include <mips/p8700.h>
 #include <mips/mips-cm.h>
 
+static unsigned long mips_csr_read_num(int csr_num)
+{
+#define switchcase_csr_read(__csr_num, __val)		\
+	case __csr_num:					\
+		__val = csr_read(__csr_num);		\
+		break;
+#define switchcase_csr_read_2(__csr_num, __val)		\
+	switchcase_csr_read(__csr_num + 0, __val)	\
+	switchcase_csr_read(__csr_num + 1, __val)
+#define switchcase_csr_read_4(__csr_num, __val)		\
+	switchcase_csr_read_2(__csr_num + 0, __val)	\
+	switchcase_csr_read_2(__csr_num + 2, __val)
+#define switchcase_csr_read_8(__csr_num, __val)		\
+	switchcase_csr_read_4(__csr_num + 0, __val)	\
+	switchcase_csr_read_4(__csr_num + 4, __val)
+#define switchcase_csr_read_16(__csr_num, __val)	\
+	switchcase_csr_read_8(__csr_num + 0, __val)	\
+	switchcase_csr_read_8(__csr_num + 8, __val)
+
+	unsigned long ret = 0;
+
+	switch(csr_num) {
+	switchcase_csr_read_16(CSR_MIPSPMACFG0, ret)
+
+	default:
+		sbi_panic("%s: Unknown CSR %#x", __func__, csr_num);
+		break;
+	}
+
+	return ret;
+
+#undef switchcase_csr_read_16
+#undef switchcase_csr_read_8
+#undef switchcase_csr_read_4
+#undef switchcase_csr_read_2
+#undef switchcase_csr_read
+}
+
+static void mips_csr_write_num(int csr_num, unsigned long val)
+{
+#define switchcase_csr_write(__csr_num, __val)		\
+	case __csr_num:					\
+		csr_write(__csr_num, __val);		\
+		break;
+#define switchcase_csr_write_2(__csr_num, __val)	\
+	switchcase_csr_write(__csr_num + 0, __val)	\
+	switchcase_csr_write(__csr_num + 1, __val)
+#define switchcase_csr_write_4(__csr_num, __val)	\
+	switchcase_csr_write_2(__csr_num + 0, __val)	\
+	switchcase_csr_write_2(__csr_num + 2, __val)
+#define switchcase_csr_write_8(__csr_num, __val)	\
+	switchcase_csr_write_4(__csr_num + 0, __val)	\
+	switchcase_csr_write_4(__csr_num + 4, __val)
+#define switchcase_csr_write_16(__csr_num, __val)	\
+	switchcase_csr_write_8(__csr_num + 0, __val)	\
+	switchcase_csr_write_8(__csr_num + 8, __val)
+
+	switch(csr_num) {
+	switchcase_csr_write_16(CSR_MIPSPMACFG0, val)
+
+	default:
+		sbi_panic("%s: Unknown CSR %#x", __func__, csr_num);
+		break;
+	}
+
+#undef switchcase_csr_write_16
+#undef switchcase_csr_write_8
+#undef switchcase_csr_write_4
+#undef switchcase_csr_write_2
+#undef switchcase_csr_write
+}
+
+static void mips_p8700_pma_set(unsigned int n, unsigned long prot, unsigned long addr)
+{
+	int pmacfg_csr, pmacfg_shift;
+	unsigned long cfgmask;
+	unsigned long pmacfg, cca;
+
+	pmacfg_csr = (CSR_MIPSPMACFG0 + (n >> 2)) & ~1;
+	pmacfg_shift = (n & 7) << 3;
+	cfgmask = ~(0xffUL << pmacfg_shift);
+
+	/* Read pmacfg to change cacheability */
+	pmacfg = (mips_csr_read_num(pmacfg_csr) & cfgmask);
+	cca = (prot & PMP_MMIO) ? CCA_CACHE_DISABLE :
+				  CCA_CACHE_ENABLE | PMA_SPECULATION;
+	pmacfg |= ((cca << pmacfg_shift) & ~cfgmask);
+	mips_csr_write_num(pmacfg_csr, pmacfg);
+}
+
 #if CLUSTERS_IN_PLATFORM > 1
 static void power_up_other_cluster(u32 hartid)
 {
@@ -255,6 +345,7 @@ static int mips_p8700_platform_init(const void *fdt, int nodeoff, const struct f
 	generic_platform_ops.early_init = mips_p8700_early_init;
 	generic_platform_ops.final_init = mips_p8700_final_init;
 	generic_platform_ops.nascent_init = mips_p8700_nascent_init;
+	generic_platform_ops.pma_set = mips_p8700_pma_set;
 
 	return 0;
 }
-- 
2.47.1