From 65e2c1b4d84ece3487f56de98f2aed8a73759085 Mon Sep 17 00:00:00 2001
From: xvanc <xvancm@gmail.com>
Date: Thu, 30 May 2024 10:53:21 -0500
Subject: [PATCH] riscv: add support for device tree

---
 common/lib/misc.c      | 12 ++++++
 common/lib/misc.h      |  2 +
 common/sys/cpu_riscv.c | 84 +++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 96 insertions(+), 2 deletions(-)

diff --git a/common/lib/misc.c b/common/lib/misc.c
index 9a9b09dc..e34f039d 100644
--- a/common/lib/misc.c
+++ b/common/lib/misc.c
@@ -110,6 +110,18 @@ uint32_t hex2bin(uint8_t *str, uint32_t size) {
 
 #if defined (UEFI)
 
+void *get_device_tree_blob(void) {
+    EFI_GUID dtb_guid = EFI_DTB_TABLE_GUID;
+    for (size_t i = 0; i < gST->NumberOfTableEntries; i++) {
+        EFI_CONFIGURATION_TABLE *cur_table = &gST->ConfigurationTable[i];
+        if (memcmp(&cur_table->VendorGuid, &dtb_guid, sizeof(EFI_GUID)))
+            continue;
+        printv("efi: found dtb at %p\n", cur_table->VendorTable);
+        return cur_table->VendorTable;
+    }
+    return NULL;
+}
+
 #if defined (__riscv)
 
 RISCV_EFI_BOOT_PROTOCOL *get_riscv_boot_protocol(void) {
diff --git a/common/lib/misc.h b/common/lib/misc.h
index 7a9797a5..e48d7bb9 100644
--- a/common/lib/misc.h
+++ b/common/lib/misc.h
@@ -26,6 +26,8 @@ extern UINT32 efi_desc_ver;
 
 extern bool efi_boot_services_exited;
 bool efi_exit_boot_services(void);
+
+void *get_device_tree_blob(void);
 #endif
 
 extern struct volume *boot_volume;
diff --git a/common/sys/cpu_riscv.c b/common/sys/cpu_riscv.c
index 40f28844..9de27307 100644
--- a/common/sys/cpu_riscv.c
+++ b/common/sys/cpu_riscv.c
@@ -8,6 +8,7 @@
 #include <mm/pmm.h>
 #include <stddef.h>
 #include <stdint.h>
+#include <libfdt/libfdt.h>
 
 // ACPI RISC-V Hart Capabilities Table
 struct rhct {
@@ -81,11 +82,11 @@ static inline struct rhct_hart_info *rhct_get_hart_info(struct rhct *rhct, uint3
     return NULL;
 }
 
-void init_riscv(void) {
+static void init_riscv_acpi(void) {
     struct madt *madt = acpi_get_table("APIC", 0);
     struct rhct *rhct = acpi_get_table("RHCT", 0);
     if (madt == NULL || rhct == NULL) {
-        panic(false, "riscv: requires acpi");
+        panic(false, "riscv: requires `APIC` and `RHCT` ACPI tables");
     }
 
     for (uint8_t *madt_ptr = (uint8_t *)madt->madt_entries_begin;
@@ -153,6 +154,85 @@ void init_riscv(void) {
             bsp_hart = hart;
         }
     }
+}
+
+static void init_riscv_fdt(const void *fdt) {
+    if (fdt_check_header(fdt)) {
+        panic(false, "riscv: invalid device tree");
+    }
+
+    int cpus = fdt_path_offset(fdt, "/cpus");
+    if (cpus < 0) {
+        panic(false, "riscv: missing `/cpus` node");
+    }
+
+    int node;
+    fdt_for_each_subnode(node, fdt, cpus) {
+        const void *prop;
+
+        if (!(prop = fdt_getprop(fdt, node, "device_type", NULL)) || strcmp(prop, "cpu")) {
+            continue;
+        }
+
+        if (!(prop = fdt_getprop(fdt, node, "reg", NULL))) {
+            continue;
+        }
+        size_t hartid = fdt32_ld(prop);
+
+        uint8_t flags = 0;
+        uint8_t mmu_type = 0;
+        if ((prop = fdt_getprop(fdt, node, "mmu-type", NULL))) {
+            if (!strcmp(prop, "riscv,sv39")) {
+                mmu_type = RISCV_MMU_TYPE_SV39;
+                flags |= RISCV_HART_HAS_MMU;
+            } else if (!strcmp(prop, "riscv,sv48")) {
+                mmu_type = RISCV_MMU_TYPE_SV48;
+                flags |= RISCV_HART_HAS_MMU;
+            } else if (!strcmp(prop, "riscv,sv57")) {
+                mmu_type = RISCV_MMU_TYPE_SV57;
+                flags |= RISCV_HART_HAS_MMU;
+            }
+        }
+
+        const char *isa_string = fdt_getprop(fdt, node, "riscv,isa", NULL);
+        if (isa_string == NULL) {
+            print("riscv: missing isa string for hartid %u, skipping.\n", hartid);
+            continue;
+        }
+
+        if (strncmp("rv64", isa_string, 4) && strncmp("rv32", isa_string, 4)) {
+            print("riscv: skipping hartid %u with invalid isa string: %s", hartid, isa_string);
+        }
+
+        struct riscv_hart *hart = ext_mem_alloc(sizeof(struct riscv_hart));
+        if (hart == NULL) {
+            panic(false, "out of memory");
+        }
+
+        hart->hartid = hartid;
+        hart->acpi_uid = 0;
+        hart->isa_string = isa_string;
+        hart->mmu_type = mmu_type;
+        hart->flags = flags;
+
+        hart->next = hart_list;
+        hart_list = hart;
+
+        if (hart->hartid == bsp_hartid) {
+            bsp_hart = hart;
+        }
+    }
+}
+
+void init_riscv(void) {
+    void *fdt = get_device_tree_blob();
+    if (fdt != NULL) {
+        init_riscv_fdt(fdt);
+    } else if (acpi_get_rsdp()) {
+        init_riscv_acpi();
+    } else {
+        panic(false, "riscv: requires DTB or ACPI");
+    }
 
     if (bsp_hart == NULL) {
         panic(false, "riscv: missing `struct riscv_hart` for BSP");