[RFC PATCH v2 5/7] lib: sbi: riscv_asm: support reserved PMP allocator

[Yu-Chien Peter Lin]

In sbi_hart_init(), the coldboot hart will call reserved_pmp_init()
to create scratch space to store reserved_pmp_used bitmaps for each
hart, then user can use reserved_pmp_{alloc,free}() function pair
to allocate and release high-priority reserved PMP entries.

Signed-off-by: Yu-Chien Peter Lin <peter.lin at sifive.com>
---
 include/sbi/riscv_asm.h |  6 +++
 lib/sbi/riscv_asm.c     | 92 +++++++++++++++++++++++++++++++++++++++++
 lib/sbi/sbi_hart.c      |  4 ++
 3 files changed, 102 insertions(+)

diff --git a/include/sbi/riscv_asm.h b/include/sbi/riscv_asm.h
index ef48dc89..4fd0be2b 100644
--- a/include/sbi/riscv_asm.h
+++ b/include/sbi/riscv_asm.h
@@ -221,6 +221,12 @@ int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
 int pmp_get(unsigned int n, unsigned long *prot_out, unsigned long *addr_out,
 	    unsigned long *log2len);
 
+int reserved_pmp_init(void);
+
+int reserved_pmp_alloc(unsigned int *pmp_id);
+
+int reserved_pmp_free(unsigned int pmp_id);
+
 #endif /* !__ASSEMBLER__ */
 
 #endif
diff --git a/lib/sbi/riscv_asm.c b/lib/sbi/riscv_asm.c
index 3e44320f..2beb3ba4 100644
--- a/lib/sbi/riscv_asm.c
+++ b/lib/sbi/riscv_asm.c
@@ -9,10 +9,14 @@
 
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_encoding.h>
+#include <sbi/sbi_bitmap.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_platform.h>
+#include <sbi/sbi_scratch.h>
 #include <sbi/sbi_console.h>
 
+static unsigned long reserved_pmp_used_offset;
+
 /* determine CPU extension, return non-zero support */
 int misa_extension_imp(char ext)
 {
@@ -432,3 +436,91 @@ int pmp_get(unsigned int n, unsigned long *prot_out, unsigned long *addr_out,
 
 	return 0;
 }
+
+/**
+ * reserved_pmp_init() - Initialize the reserved PMP allocator
+ *
+ * This function initializes the reserved PMP allocator by allocating
+ * scratch space to track which reserved PMP entries are in use.
+ *
+ * Returns: 0 on success, negative error code on failure
+ */
+int reserved_pmp_init(void)
+{
+	if (reserved_pmp_used_offset)
+		return SBI_EINVAL;
+
+	reserved_pmp_used_offset = sbi_scratch_alloc_offset(
+		sizeof(unsigned long) * BITS_TO_LONGS(PMP_COUNT));
+	if (!reserved_pmp_used_offset)
+		return SBI_ENOMEM;
+
+	return SBI_SUCCESS;
+}
+
+/**
+ * reserved_pmp_alloc() - Allocate an unused reserved PMP entry
+ * @pmp_id: Pointer to store the allocated PMP entry ID
+ *
+ * Returns: 0 on success, negative error code on failure
+ *
+ * The following programming sequence is expected to use the
+ * allocated PMP entry:
+ * - reserved_pmp_alloc(&pmp_id)
+ * - pmp_set(pmp_id, ...)
+ * - pmp_disable(pmp_id)
+ * - reserved_pmp_free(pmp_id)
+ */
+int reserved_pmp_alloc(unsigned int *pmp_id)
+{
+	const struct sbi_platform *plat = sbi_platform_thishart_ptr();
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	unsigned long *reserved_pmp_used;
+
+	if (!reserved_pmp_used_offset)
+		return SBI_EINVAL;
+
+	reserved_pmp_used = sbi_scratch_offset_ptr(scratch,
+						   reserved_pmp_used_offset);
+
+	for (int n = 0; n < plat->reserved_pmp_count; n++) {
+		if (bitmap_test(reserved_pmp_used, n))
+			continue;
+		bitmap_set(reserved_pmp_used, n, 1);
+		*pmp_id = n;
+		return SBI_SUCCESS;
+	}
+
+	sbi_printf("%s: Failed to allocate PMP entry. "
+		   "Please increase reserved-pmp-count\n", __func__);
+	return SBI_EFAIL;
+}
+
+/**
+ * reserved_pmp_free() - Free a reserved PMP entry
+ * @pmp_id: PMP entry ID to free
+ *
+ * Returns: 0 on success, negative error code on failure
+ */
+int reserved_pmp_free(unsigned int pmp_id)
+{
+	const struct sbi_platform *plat = sbi_platform_thishart_ptr();
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	unsigned long *reserved_pmp_used;
+
+	if (!reserved_pmp_used_offset)
+		return SBI_EINVAL;
+
+	reserved_pmp_used = sbi_scratch_offset_ptr(scratch,
+						   reserved_pmp_used_offset);
+
+	if (pmp_id >= plat->reserved_pmp_count ||
+	    !bitmap_test(reserved_pmp_used, pmp_id) ||
+	    is_pmp_entry_mapped(pmp_id)) {
+		return SBI_EINVAL;
+	}
+
+	bitmap_clear(reserved_pmp_used, pmp_id, 1);
+
+	return SBI_SUCCESS;
+}
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index 1b50f671..f173187c 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -1058,6 +1058,10 @@ int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot)
 					sizeof(struct sbi_hart_features));
 		if (!hart_features_offset)
 			return SBI_ENOMEM;
+
+		rc = reserved_pmp_init();
+		if (rc)
+			return rc;
 	}
 
 	rc = hart_detect_features(scratch);
-- 
2.39.3