[PATCH v2 6/7] arm/mpu: Implement early_fdt_map support in MPU systems

[Luca Fancellu <luca.fancellu@xxxxxxx>]

Implement early_fdt_map() function, that is responsible to map the
device tree blob in the early stages of the boot process, since at
this stage the MPU C data structure are not yet initialised, it is
using low level APIs to write into the MPU registers at a fixed
MPU region number.

The MPU memory management is designed to work on pages of PAGE_SIZE
in order to reuse helpers and macros already available on the Xen
memory management system.

Signed-off-by: Luca Fancellu <luca.fancellu@xxxxxxx>
---
 xen/arch/arm/mpu/setup.c | 54 ++++++++++++++++++++++++++++++++++++++--
 1 file changed, 52 insertions(+), 2 deletions(-)

diff --git a/xen/arch/arm/mpu/setup.c b/xen/arch/arm/mpu/setup.c
index b4da77003f47..5969065250d4 100644
--- a/xen/arch/arm/mpu/setup.c
+++ b/xen/arch/arm/mpu/setup.c
@@ -1,17 +1,67 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
+#include <xen/bootfdt.h>
 #include <xen/bug.h>
 #include <xen/init.h>
+#include <xen/libfdt/libfdt.h>
 #include <xen/mm.h>
+#include <xen/pfn.h>
 #include <xen/types.h>
+#include <asm/mpu.h>
+#include <asm/page.h>
 #include <asm/setup.h>
 
+/* Needs to be kept in sync with the regions programmed in arm64/mpu/head.S */
+#define EARLY_FDT_MAP_REGION_NUMBER 6
+
 void __init setup_pagetables(void) {}
 
 void * __init early_fdt_map(paddr_t fdt_paddr)
 {
-    BUG_ON("unimplemented");
-    return NULL;
+    /* Map at least a page containing the DTB address, exclusive range */
+    paddr_t base_paddr = round_pgdown(fdt_paddr);
+    paddr_t end_paddr = round_pgup(fdt_paddr + sizeof(struct fdt_header));
+    unsigned int flags = PAGE_HYPERVISOR_RO;
+    void *fdt_virt = (void *)fdt_paddr; /* virt == paddr for MPU */
+    pr_t fdt_region;
+
+    /*
+     * Check whether the physical FDT address is set and meets the minimum
+     * alignment requirement. Since we are relying on MIN_FDT_ALIGN to be at
+     * least 8 bytes so that we always access the magic and size fields
+     * of the FDT header after mapping the first chunk, double check if
+     * that is indeed the case.
+     */
+    BUILD_BUG_ON(MIN_FDT_ALIGN < 8);
+    if ( !fdt_paddr || fdt_paddr % MIN_FDT_ALIGN )
+        return NULL;
+
+    /* Map the device tree blob header  */
+    fdt_region = pr_of_xenaddr(base_paddr, end_paddr, PAGE_AI_MASK(flags));
+    fdt_region.prbar.reg.ap = PAGE_AP_MASK(flags);
+    fdt_region.prbar.reg.xn = PAGE_XN_MASK(flags);
+
+    write_protection_region(&fdt_region, EARLY_FDT_MAP_REGION_NUMBER);
+    context_sync_mpu();
+
+    if ( fdt_magic(fdt_virt) != FDT_MAGIC )
+        return NULL;
+
+    end_paddr = round_pgup(fdt_paddr + fdt_totalsize(fdt_virt));
+
+    /*
+     * If the mapped range is not enough, map the rest of the DTB, pr_get_limit
+     * returns an inclusive address of the range, hence the increment.
+     */
+    if ( end_paddr > (pr_get_limit(&fdt_region) + 1) )
+    {
+        pr_set_limit(&fdt_region, end_paddr);
+
+        write_protection_region(&fdt_region, EARLY_FDT_MAP_REGION_NUMBER);
+        context_sync_mpu();
+    }
+
+    return fdt_virt;
 }
 
 /*
-- 
2.34.1