[RFC PATCH v3 3/6] lib: sbi: riscv_asm: support reserved PMP allocator

[Yu-Chien Peter Lin]

Add reserved PMP entry allocation and management functions to enable
dynamic allocation of high-priority PMP entries. The allocator uses
per-hart bitmaps stored in scratch space to track reserved PMP usage.

New functions:
- reserved_pmp_init(): Initialize allocator scratch space
- reserved_pmp_alloc(): Allocate unused reserved PMP entry
- reserved_pmp_free(): Release allocated PMP entry

The coldboot hart calls reserved_pmp_init() during sbi_hart_init()
to set up the tracking bitmaps for all harts.

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..6c81708f 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 allocated PMP entry should be used with the following
+ * programming sequence:
+ * - 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();
+	u32 reserved_pmp_count = sbi_platform_reserved_pmp_count(plat);
+	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 < 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;
+	}
+
+	/* PMP allocation failed - all reserved entries in use */
+	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();
+	u32 reserved_pmp_count = sbi_platform_reserved_pmp_count(plat);
+	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 >= reserved_pmp_count ||
+	    !bitmap_test(reserved_pmp_used, 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 a91703b4..548fdecd 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -1031,6 +1031,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