[PATCH v1 4/5] xen/riscv: introduce device tree maping function

[Oleksii Kurochko <oleksii.kurochko@xxxxxxxxx>]

Signed-off-by: Oleksii Kurochko <oleksii.kurochko@xxxxxxxxx>
---
 xen/arch/riscv/include/asm/config.h |  6 +++++
 xen/arch/riscv/include/asm/mm.h     |  2 ++
 xen/arch/riscv/mm.c                 | 37 +++++++++++++++++++++++++----
 3 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/xen/arch/riscv/include/asm/config.h 
b/xen/arch/riscv/include/asm/config.h
index 50583aafdc..2395bafb91 100644
--- a/xen/arch/riscv/include/asm/config.h
+++ b/xen/arch/riscv/include/asm/config.h
@@ -74,6 +74,9 @@
 #error "unsupported RV_STAGE1_MODE"
 #endif
 
+#define XEN_SIZE                MB(2)
+#define XEN_VIRT_END            (XEN_VIRT_START + XEN_SIZE)
+
 #define DIRECTMAP_SLOT_END      509
 #define DIRECTMAP_SLOT_START    200
 #define DIRECTMAP_VIRT_START    SLOTN(DIRECTMAP_SLOT_START)
@@ -99,6 +102,9 @@
 #define VMAP_VIRT_START         SLOTN(VMAP_SLOT_START)
 #define VMAP_VIRT_SIZE          GB(1)
 
+#define BOOT_FDT_VIRT_START     XEN_VIRT_END
+#define BOOT_FDT_VIRT_SIZE      MB(4)
+
 #else
 #error "RV32 isn't supported"
 #endif
diff --git a/xen/arch/riscv/include/asm/mm.h b/xen/arch/riscv/include/asm/mm.h
index 25af9e1aaa..d1db7ce098 100644
--- a/xen/arch/riscv/include/asm/mm.h
+++ b/xen/arch/riscv/include/asm/mm.h
@@ -255,4 +255,6 @@ static inline unsigned int arch_get_dma_bitsize(void)
     return 32; /* TODO */
 }
 
+void* early_fdt_map(paddr_t fdt_paddr);
+
 #endif /* _ASM_RISCV_MM_H */
diff --git a/xen/arch/riscv/mm.c b/xen/arch/riscv/mm.c
index 7d09e781bf..ccc91f9a01 100644
--- a/xen/arch/riscv/mm.c
+++ b/xen/arch/riscv/mm.c
@@ -1,5 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
+#include <xen/bootfdt.h>
 #include <xen/bug.h>
 #include <xen/compiler.h>
 #include <xen/init.h>
@@ -7,7 +8,9 @@
 #include <xen/macros.h>
 #include <xen/mm.h>
 #include <xen/pfn.h>
+#include <xen/libfdt/libfdt.h>
 #include <xen/sections.h>
+#include <xen/sizes.h>
 
 #include <asm/early_printk.h>
 #include <asm/csr.h>
@@ -20,7 +23,7 @@ struct mmu_desc {
     unsigned int pgtbl_count;
     pte_t *next_pgtbl;
     pte_t *pgtbl_base;
-};
+} mmu_desc = { CONFIG_PAGING_LEVELS, 0, NULL, 0 };
 
 static unsigned long __ro_after_init phys_offset;
 
@@ -39,9 +42,11 @@ static unsigned long __ro_after_init phys_offset;
  * isn't 2 MB aligned.
  *
  * CONFIG_PAGING_LEVELS page tables are needed for the identity mapping,
- * except that the root page table is shared with the initial mapping
+ * except that the root page table is shared with the initial mapping.
+ *
+ * CONFIG_PAGING_LEVELS page tables are needed for device tree mapping.
  */
-#define PGTBL_INITIAL_COUNT ((CONFIG_PAGING_LEVELS - 1) * 2 + 1)
+#define PGTBL_INITIAL_COUNT ((CONFIG_PAGING_LEVELS - 1) * 3 + 1 + 1)
 
 pte_t __section(".bss.page_aligned") __aligned(PAGE_SIZE)
 stage1_pgtbl_root[PAGETABLE_ENTRIES];
@@ -207,8 +212,6 @@ static bool __init check_pgtbl_mode_support(struct mmu_desc 
*mmu_desc,
  */
 void __init setup_initial_pagetables(void)
 {
-    struct mmu_desc mmu_desc = { CONFIG_PAGING_LEVELS, 0, NULL, NULL };
-
     /*
      * Access to _start, _end is always PC-relative thereby when access
      * them we will get load adresses of start and end of Xen.
@@ -296,6 +299,30 @@ unsigned long __init calc_phys_offset(void)
     return phys_offset;
 }
 
+void* __init early_fdt_map(paddr_t fdt_paddr)
+{
+    unsigned long dt_phys_base = fdt_paddr;
+    unsigned long dt_virt_base;
+    unsigned long dt_virt_size;
+
+    BUILD_BUG_ON(MIN_FDT_ALIGN < 8);
+    if ( !fdt_paddr || fdt_paddr % MIN_FDT_ALIGN || fdt_paddr % SZ_2M ||
+         fdt_totalsize(fdt_paddr) > BOOT_FDT_VIRT_SIZE )
+        return NULL;
+
+    BUILD_BUG_ON(BOOT_FDT_VIRT_START % SZ_2M);
+
+    dt_virt_base = BOOT_FDT_VIRT_START;
+    dt_virt_size = BOOT_FDT_VIRT_SIZE;
+
+    /* Map device tree */
+    setup_initial_mapping(&mmu_desc, dt_virt_base,
+                    dt_virt_base + dt_virt_size,
+                    dt_phys_base);
+
+    return (void *)dt_virt_base;
+}
+
 void put_page(struct page_info *page)
 {
     BUG_ON("unimplemented");
-- 
2.45.2