[PATCH 2/3] lib: sbi: Add pmp_set_tor for setting TOR regions

[Bo Gan]

TOR can be utilized to cover memory regions that are not aligned with
their sizes. Given that the address matching is formed bt 2 consecutive
pmpaddr, i.e., pmpaddr(i-1) and pmpaddr(i), TOR should not be used
generically to avoid pmpaddr conflict with other NA4/NAPOT regions.

It's advised to only use them in platform code where the caller can
ensure the index and order of every pmp region especially when there's
a mixture of TOR/NA4/NAPOT.

Signed-off-by: Bo Gan <ganboing at gmail.com>
---
 include/sbi/riscv_asm.h |  4 +++
 lib/sbi/riscv_asm.c     | 75 +++++++++++++++++++++++++++++++----------
 2 files changed, 62 insertions(+), 17 deletions(-)

diff --git a/include/sbi/riscv_asm.h b/include/sbi/riscv_asm.h
index ef48dc89..c23feab6 100644
--- a/include/sbi/riscv_asm.h
+++ b/include/sbi/riscv_asm.h
@@ -218,6 +218,10 @@ int is_pmp_entry_mapped(unsigned long entry);
 int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
 	    unsigned long log2len);
 
+/* Top of range (TOR) matching mode. pmpaddr(n-1) will also be changed */
+int pmp_set_tor(unsigned int n, unsigned long prot, unsigned long addr,
+		unsigned long size);
+
 int pmp_get(unsigned int n, unsigned long *prot_out, unsigned long *addr_out,
 	    unsigned long *log2len);
 
diff --git a/lib/sbi/riscv_asm.c b/lib/sbi/riscv_asm.c
index 3e44320f..8f64f2b5 100644
--- a/lib/sbi/riscv_asm.c
+++ b/lib/sbi/riscv_asm.c
@@ -330,16 +330,10 @@ int is_pmp_entry_mapped(unsigned long entry)
 	return false;
 }
 
-int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
-	    unsigned long log2len)
+static void pmp_set_prot(unsigned int n, unsigned long prot)
 {
-	int pmpcfg_csr, pmpcfg_shift, pmpaddr_csr;
+	int pmpcfg_csr, pmpcfg_shift;
 	unsigned long cfgmask, pmpcfg;
-	unsigned long addrmask, pmpaddr;
-
-	/* check parameters */
-	if (n >= PMP_COUNT || log2len > __riscv_xlen || log2len < PMP_SHIFT)
-		return SBI_EINVAL;
 
 	/* calculate PMP register and offset */
 #if __riscv_xlen == 32
@@ -351,15 +345,29 @@ int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
 #else
 # error "Unexpected __riscv_xlen"
 #endif
-	pmpaddr_csr = CSR_PMPADDR0 + n;
-
-	/* encode PMP config */
-	prot &= ~PMP_A;
-	prot |= (log2len == PMP_SHIFT) ? PMP_A_NA4 : PMP_A_NAPOT;
 	cfgmask = ~(0xffUL << pmpcfg_shift);
 	pmpcfg	= (csr_read_num(pmpcfg_csr) & cfgmask);
 	pmpcfg |= ((prot << pmpcfg_shift) & ~cfgmask);
 
+	csr_write_num(pmpcfg_csr, pmpcfg);
+}
+
+static void pmp_set_addr(unsigned int n, unsigned long pmpaddr)
+{
+	int pmpaddr_csr = CSR_PMPADDR0 + n;
+
+	csr_write_num(pmpaddr_csr, pmpaddr);
+}
+
+int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
+	    unsigned long log2len)
+{
+	unsigned long addrmask, pmpaddr;
+
+	/* check parameters */
+	if (n >= PMP_COUNT || log2len > __riscv_xlen || log2len < PMP_SHIFT)
+		return SBI_EINVAL;
+
 	/* encode PMP address */
 	if (log2len == PMP_SHIFT) {
 		pmpaddr = (addr >> PMP_SHIFT);
@@ -373,10 +381,41 @@ int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
 		}
 	}
 
+	/* encode PMP config */
+	prot &= ~PMP_A;
+	prot |= (log2len == PMP_SHIFT) ? PMP_A_NA4 : PMP_A_NAPOT;
+
 	/* write csrs */
-	csr_write_num(pmpaddr_csr, pmpaddr);
-	csr_write_num(pmpcfg_csr, pmpcfg);
+	pmp_set_addr(n, pmpaddr);
+	pmp_set_prot(n, prot);
+	return 0;
+}
+
+int pmp_set_tor(unsigned int n, unsigned long prot, unsigned long addr,
+		unsigned long size)
+{
+	unsigned long pmpaddr, pmpaddrp;
+
+	/* check parameters */
+	if (n >= PMP_COUNT)
+		return SBI_EINVAL;
+
+	if (n == 0 && addr != 0)
+		return SBI_EINVAL;
+
+	/* encode PMP address */
+	pmpaddrp = addr >> PMP_SHIFT;
+	pmpaddr = (addr + size) >> PMP_SHIFT;
 
+	/* encode PMP config */
+	prot &= ~PMP_A;
+	prot |= PMP_A_TOR;
+
+	/* write csrs */
+	if (n)
+		pmp_set_addr(n - 1, pmpaddrp);
+	pmp_set_addr(n, pmpaddr);
+	pmp_set_prot(n, prot);
 	return 0;
 }
 
@@ -420,10 +459,12 @@ int pmp_get(unsigned int n, unsigned long *prot_out, unsigned long *addr_out,
 			addr	= (addr & ~((1UL << t1) - 1)) << PMP_SHIFT;
 			len	= (t1 + PMP_SHIFT + 1);
 		}
-	} else {
+	} else if ((prot & PMP_A) == PMP_A_NA4) {
 		addr	= csr_read_num(pmpaddr_csr) << PMP_SHIFT;
 		len	= PMP_SHIFT;
-	}
+	} else
+		/* Error out for TOR region */
+		return SBI_EINVAL;
 
 	/* return details */
 	*prot_out    = prot;
-- 
2.34.1