[PATCH 5/5] Allow physmap_common() to round address ranges to page boundaries.

[Stefan Tauner]

Automatically round address ranges requested from physmap_autocleanup()
to page boundaries. Other physmap() variants were not changed so this
is safe regarding unmapping.

Signed-off-by: Stefan Tauner 
---
 physmap.c |   47 ++++++++++++++++++++++++++++++++---------------
 1 file changed, 32 insertions(+), 15 deletions(-)

diff --git a/physmap.c b/physmap.c
index 8b9fea7..ea87d1a 100644
--- a/physmap.c
+++ b/physmap.c
@@ -218,6 +218,27 @@ void physunmap(void *virt_addr, size_t len)
 }
 #endif

+/* Round start to nearest page boundary below and set len so that the resulting
+ * address range ends at the lowest possible page boundary where the original
+ * address range is still entirely contained. It returns the difference between
+ * the rounded start address and the original start address. */
+static uintptr_t round_to_page_boundaries(uintptr_t *start, size_t *len)
+{
+	uintptr_t page_size = getpagesize();
+	uintptr_t page_mask = ~(page_size-1);
+	uintptr_t end = *start + *len;
+	uintptr_t old_start = *start;
+	msg_gspew("page_size=%lu\n", page_size);
+	msg_gspew("pre-rounding:  start=0x%0*" PRIxPTR ", len=0x%0zx, end=0x%0*" PRIxPTR "\n",
+		  PRIxPTR_WIDTH, *start, *len, PRIxPTR_WIDTH, end);
+	*start = *start & page_mask;
+	end = (end + page_size - 1) & page_mask;
+	*len = end - *start;
+	msg_gspew("post-rounding: start=0x%0*" PRIxPTR ", len=0x%0zx, end=0x%0*" PRIxPTR "\n",
+		  PRIxPTR_WIDTH, *start, *len, PRIxPTR_WIDTH, *start + *len);
+	return old_start - *start;
+}
+
 struct physmap_stutdown_data {
    void *addr;
    size_t len;
@@ -236,24 +257,18 @@ static int physmap_shutdown(void *data)
 }

static void *physmap_common(const char *descr, uintptr_t phys_addr, size_t len, bool mayfail,
-			    bool readonly, bool autocleanup)
+			    bool readonly, bool autocleanup, bool round)
 {
    void *virt_addr;
+	uintptr_t offset = 0;

if (len == 0) {
    	msg_pspew("Not mapping %s, zero size at 0x%0*" PRIxPTR ".\n", descr, PRIxPTR_WIDTH, phys_addr);
    	return ERROR_PTR;
    }

-	if ((getpagesize() - 1) & len) {
-		msg_perr("Mapping %s at 0x%08lx, unaligned size 0x%lx.\n",
-			 descr, phys_addr, (unsigned long)len);
-	}
-
-	if ((getpagesize() - 1) & phys_addr) {
-		msg_perr("Mapping %s, 0x%lx bytes at unaligned 0x%08lx.\n",
-			 descr, (unsigned long)len, phys_addr);
-	}
+	if (round)
+		offset = round_to_page_boundaries(&phys_addr, &len);

if (readonly)
    	virt_addr = sys_physmap_ro_cached(phys_addr, len);
@@ -279,7 +294,9 @@ static void *physmap_common(const char *descr, uintptr_t phys_addr, size_t len,
    		 "and reboot, or reboot into\n"
    		 "single user mode.\n");
 #endif
-		if (!mayfail)
+		if (mayfail)
+			return ERROR_PTR;
+		else
    		exit(3);
    }

@@ -298,7 +315,7 @@ static void *physmap_common(const char *descr, uintptr_t phys_addr, size_t len,
    	}
    }

-	return virt_addr;
+	return virt_addr + offset;
 unmap_out:
    physunmap(virt_addr, len);
    return ERROR_PTR;
@@ -306,17 +323,17 @@ unmap_out:

void *physmap(const char *descr, uintptr_t phys_addr, size_t len)
 {
-	return physmap_common(descr, phys_addr, len, false, false, false);
+	return physmap_common(descr, phys_addr, len, false, false, false, false);
 }

void *physmap_autocleanup(const char *descr, uintptr_t phys_addr, size_t len)
 {
-	return physmap_common(descr, phys_addr, len, false, false, true);
+	return physmap_common(descr, phys_addr, len, false, false, true, true);
 }

void *physmap_try_ro(const char *descr, uintptr_t phys_addr, size_t len)
 {
-	return physmap_common(descr, phys_addr, len, true, true, false);
+	return physmap_common(descr, phys_addr, len, true, true, false, false);
 }

#if defined(__i386__) || defined(__x86_64__)