lib: sbi: Add sbi_domain_root_add_memrange() API

This patch generalizes the logic to add a memory range with desired
alignment and flags of consecutive regions to the root domain.

Signed-off-by: Yu Chien Peter Lin <peterlin@andestech.com>
Reviewed-by: Anup Patel <anup@brainfault.org>

2 files changed, 41 insertions, 36 deletions

diff --git a/lib/sbi/sbi_domain.c b/lib/sbi/sbi_domain.c
index 4e4c1e1..f24a8e5 100644
--- a/lib/sbi/sbi_domain.c
+++ b/lib/sbi/sbi_domain.c
@@ -522,6 +522,33 @@ int sbi_domain_root_add_memregion(const struct sbi_domain_memregion *reg)
 	return 0;
 }
 
+int sbi_domain_root_add_memrange(unsigned long addr, unsigned long size,
+			   unsigned long align, unsigned long region_flags)
+{
+	int rc;
+	unsigned long pos, end, rsize;
+	struct sbi_domain_memregion reg;
+
+	pos = addr;
+	end = addr + size;
+	while (pos < end) {
+		rsize = pos & (align - 1);
+		if (rsize)
+			rsize = 1UL << sbi_ffs(pos);
+		else
+			rsize = ((end - pos) < align) ?
+				(end - pos) : align;
+
+		sbi_domain_memregion_init(pos, rsize, region_flags, &reg);
+		rc = sbi_domain_root_add_memregion(&reg);
+		if (rc)
+			return rc;
+		pos += rsize;
+	}
+
+	return 0;
+}
+
 int sbi_domain_finalize(struct sbi_scratch *scratch, u32 cold_hartid)
 {
 	int rc;
diff --git a/lib/utils/timer/aclint_mtimer.c b/lib/utils/timer/aclint_mtimer.c
index a957b1c..3f00c21 100644
--- a/lib/utils/timer/aclint_mtimer.c
+++ b/lib/utils/timer/aclint_mtimer.c
@@ -142,34 +142,6 @@ int aclint_mtimer_warm_init(void)
 	return 0;
 }
 
-static int aclint_mtimer_add_regions(unsigned long addr, unsigned long size)
-{
-#define MTIMER_ADD_REGION_ALIGN		0x1000
-	int rc;
-	unsigned long pos, end, rsize;
-	struct sbi_domain_memregion reg;
-
-	pos = addr;
-	end = addr + size;
-	while (pos < end) {
-		rsize = pos & (MTIMER_ADD_REGION_ALIGN - 1);
-		if (rsize)
-			rsize = 1UL << sbi_ffs(pos);
-		else
-			rsize = ((end - pos) < MTIMER_ADD_REGION_ALIGN) ?
-				(end - pos) : MTIMER_ADD_REGION_ALIGN;
-
-		sbi_domain_memregion_init(pos, rsize,
-					  SBI_DOMAIN_MEMREGION_MMIO, &reg);
-		rc = sbi_domain_root_add_memregion(&reg);
-		if (rc)
-			return rc;
-		pos += rsize;
-	}
-
-	return 0;
-}
-
 int aclint_mtimer_cold_init(struct aclint_mtimer_data *mt,
 			    struct aclint_mtimer_data *reference)
 {
@@ -208,23 +180,29 @@ int aclint_mtimer_cold_init(struct aclint_mtimer_data *mt,
 
 	/* Add MTIMER regions to the root domain */
 	if (mt->mtime_addr == (mt->mtimecmp_addr + mt->mtimecmp_size)) {
-		rc = aclint_mtimer_add_regions(mt->mtimecmp_addr,
-					mt->mtime_size + mt->mtimecmp_size);
+		rc = sbi_domain_root_add_memrange(mt->mtimecmp_addr,
+					mt->mtime_size + mt->mtimecmp_size,
+					MTIMER_REGION_ALIGN,
+					SBI_DOMAIN_MEMREGION_MMIO);
 		if (rc)
 			return rc;
 	} else if (mt->mtimecmp_addr == (mt->mtime_addr + mt->mtime_size)) {
-		rc = aclint_mtimer_add_regions(mt->mtime_addr,
-					mt->mtime_size + mt->mtimecmp_size);
+		rc = sbi_domain_root_add_memrange(mt->mtime_addr,
+					mt->mtime_size + mt->mtimecmp_size,
+					MTIMER_REGION_ALIGN,
+					SBI_DOMAIN_MEMREGION_MMIO);
 		if (rc)
 			return rc;
 	} else {
-		rc = aclint_mtimer_add_regions(mt->mtime_addr,
-						mt->mtime_size);
+		rc = sbi_domain_root_add_memrange(mt->mtime_addr,
+						mt->mtime_size, MTIMER_REGION_ALIGN,
+						SBI_DOMAIN_MEMREGION_MMIO);
 		if (rc)
 			return rc;
 
-		rc = aclint_mtimer_add_regions(mt->mtimecmp_addr,
-						mt->mtimecmp_size);
+		rc = sbi_domain_root_add_memrange(mt->mtimecmp_addr,
+						mt->mtimecmp_size, MTIMER_REGION_ALIGN,
+						SBI_DOMAIN_MEMREGION_MMIO);
 		if (rc)
 			return rc;
 	}