X-Git-Url: https://scm.lunaixsky.com/lunaix-os.git/blobdiff_plain/22d06cbaaf660067bfea03f73bee9c8630c05d50..bb5ae5c85c0812c52e8d53187f2c6b0ee9525c8c:/lunaix-os/arch/aarch64/boot/init.c?ds=sidebyside

diff --git a/lunaix-os/arch/aarch64/boot/init.c b/lunaix-os/arch/aarch64/boot/init.c
index 06b2980..45c1621 100644
--- a/lunaix-os/arch/aarch64/boot/init.c
+++ b/lunaix-os/arch/aarch64/boot/init.c
@@ -1,5 +1,7 @@
 #include <lunaix/boot_generic.h>
-#include <asm/aa64_msrs.h>
+#include <asm/aa64.h>
+#include <asm/aa64_spsr.h>
+#include <hal/devtree.h>
 
 #include "init.h"
 
@@ -27,10 +29,12 @@ setup_pstate()
    set_sysreg(SPSel, 1);
 }
 
+extern void aa64_vbase();
+
 static inline void
 setup_evbar()
 {
-    // TODO install exception vectors, setup VBAR
+    set_sysreg(VBAR_EL1, __ptr(aa64_vbase));
 }
 
 static inline void
@@ -74,15 +78,131 @@ setup_ttbr()
     set_sysreg(TCR_EL1, tcr);
 }
 
+#define MMAP_ENTS_CHUNK_SIZE    16
+
 static inline void
 extract_dtb_bootinfo(ptr_t dtb, struct boot_handoff* handoff)
 {
+    struct fdt_blob fdt;
+    struct fdt_memscan mscan;
+    struct dt_memory_node mnode;
+
+    int mmap_len = 0, mmap_max_len = 16;
+    size_t pmem_size = 0;
+    struct boot_mmapent* mmap;
+
+    mmap = bootmem_alloc(sizeof(*mmap) * MMAP_ENTS_CHUNK_SIZE);
     handoff->kexec.dtb_pa = dtb;
     
     // TODO extract /memory, /reserved-memories from dtb
+
+    fdt_load(&fdt, dtb);
+    fdt_memscan_begin(&mscan, &fdt);
+
+    struct boot_mmapent* mmap_ent;
+
+    while(fdt_memscan_nextnode(&mscan, &fdt))
+    {
+        while (fdt_memscan_nextrange(&mscan, &mnode)) 
+        {
+            mmap_ent = &mmap[mmap_len++];
+            *mmap_ent = (struct boot_mmapent) {
+                .size = mnode.size,
+                .start = mnode.base
+            };
+
+            if (mnode.type == FDT_MEM_FREE) {
+                mmap_ent->type = BOOT_MMAP_FREE;
+                pmem_size += mnode.size;
+            }
+            else {
+                mmap_ent->type = BOOT_MMAP_RSVD;
+            }
+
+
+            if (mmap_len < mmap_max_len) {
+                continue;
+            }
+
+            mmap_max_len += MMAP_ENTS_CHUNK_SIZE;
+
+            // another allocation is just expanding the previous allocation.
+            bootmem_alloc(sizeof(*mmap) * MMAP_ENTS_CHUNK_SIZE);
+        }
+    }
+
+    handoff->mem.mmap = mmap;
+    handoff->mem.mmap_len = mmap_len;
+    handoff->mem.size = pmem_size;
+}
+
+static inline void
+setup_gic_sysreg()
+{
+    int el;
+    bool has_el2;
+    u64_t pfr;
+
+    pfr = read_sysreg(ID_AA64PFR0_EL1);
+    el = read_sysreg(CurrentEL) >> 2;
+
+    // Arm A-Profile: D19.2.69, check for GIC sysreg avaliability
+
+    if (!BITS_GET(pfr, BITFIELD(27, 24))) {
+        return;
+    }
+
+    has_el2 = !!BITS_GET(pfr, BITFIELD(11, 8));
+
+    // GIC spec (v3,v4): 12.1.7, table 12-2
+    // GIC spec (v3,v4): ICC_SRE_ELx accessing
+    // Arm A-Profile: R_PCDTX, PSTATE.EL reset value
+
+    if (el == 3) {
+        sysreg_flagging(ICC_SRE_EL3, ICC_SRE_SRE, 0);
+    }
+    
+    if (has_el2) {
+        // el must be >= 2
+        sysreg_flagging(ICC_SRE_EL2, ICC_SRE_SRE, 0);
+    }
+
+    sysreg_flagging(ICC_SRE_EL1, ICC_SRE_SRE, 0);
+}
+
+void boot_text
+aarch64_pre_el1_init()
+{
+    setup_gic_sysreg();
+}
+
+bool boot_text
+aarch64_prepare_el1_transfer()
+{
+    ptr_t spsr;
+    int el;
+
+    el = read_sysreg(CurrentEL) >> 2;
+
+    if (el == 1) {
+        // no transfer required
+        return false;
+    }
+
+    spsr  = SPSR_EL1_preset;
+    spsr |= SPSR_AllInt | SPSR_I | SPSR_F | SPSR_A;
+
+    if (el == 2) {
+        set_sysreg(SPSR_EL2, spsr);
+    }
+    else {
+        set_sysreg(SPSR_EL3, spsr);
+    }
+
+    return true;
 }
 
-struct boot_handoff*
+struct boot_handoff* boot_text
 aarch64_init(ptr_t dtb)
 {
     setup_evbar();