From 7ac20b1841bd668d6e25562ef88759d0ee02e2a8 Mon Sep 17 00:00:00 2001
From: Mark Poliakov <mark@alnyan.me>
Date: Mon, 11 Oct 2021 18:36:08 +0300
Subject: [PATCH] fix: orangepi3 higher-half boot with uImage

---
 Makefile                             | 20 ++++++++++++++------
 etc/aarch64-orangepi3.ld             |  2 +-
 kernel/src/arch/aarch64/boot/mod.rs  | 13 ++++++++++---
 kernel/src/arch/aarch64/boot/uboot.S |  5 +++++
 4 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/Makefile b/Makefile
index cb4c5ed..feb5942 100644
--- a/Makefile
+++ b/Makefile
@@ -25,7 +25,7 @@ ifeq ($(ARCH),x86_64)
 $(error TODO)
 else
 ifeq ($(MACH),qemu)
-QEMU_OPTS+=-kernel $(O)/kernel.bin \
+QEMU_OPTS+=-kernel $(O)/kernel \
 		   -M virt,virtualization=off \
 		   -cpu cortex-a72 \
 		   -m 512 \
@@ -53,12 +53,20 @@ all: kernel
 
 kernel:
 	cd kernel && cargo build $(CARGO_BUILD_OPTS)
-ifeq ($(ARCH),aarch64)
-	$(OBJCOPY) -O binary $(O)/kernel $(O)/kernel.bin
-endif
 ifeq ($(MACH),orangepi3)
-#	$(LLVM_BASE)/llvm-strip $(O)/kernel
-	$(LLVM_BASE)/llvm-size $(O)/kernel
+	$(LLVM_BASE)/llvm-strip -o $(O)/kernel.strip $(O)/kernel
+	$(LLVM_BASE)/llvm-size $(O)/kernel.strip
+	$(OBJCOPY) -O binary $(O)/kernel.strip $(O)/kernel.bin
+	$(MKIMAGE) \
+		-A arm64 \
+		-O linux \
+		-T kernel \
+		-C none \
+		-a 0x48000000 \
+		-e 0x48000000 \
+		-n kernel \
+		-d $(O)/kernel.bin \
+		$(O)/uImage
 endif
 
 clean:
diff --git a/etc/aarch64-orangepi3.ld b/etc/aarch64-orangepi3.ld
index 54bab70..c5e69fb 100644
--- a/etc/aarch64-orangepi3.ld
+++ b/etc/aarch64-orangepi3.ld
@@ -1,7 +1,7 @@
 ENTRY(_entry);
 
 KERNEL_OFFSET = 0xFFFFFF8000000000;
-BASE_OFFSET = 0x42000000;
+BASE_OFFSET = 0x48000000;
 
 SECTIONS {
     . = BASE_OFFSET;
diff --git a/kernel/src/arch/aarch64/boot/mod.rs b/kernel/src/arch/aarch64/boot/mod.rs
index 842d6a5..1ec2507 100644
--- a/kernel/src/arch/aarch64/boot/mod.rs
+++ b/kernel/src/arch/aarch64/boot/mod.rs
@@ -8,7 +8,7 @@ use cortex_a::registers::{DAIF, SCTLR_EL1, VBAR_EL1, CurrentEL};
 use tock_registers::interfaces::{ReadWriteable, Writeable, Readable};
 
 #[no_mangle]
-fn __aa64_bsp_main(fdt_base: usize) {
+extern "C" fn __aa64_bsp_main(fdt_base: usize) {
     // Disable FP instruction trapping
     CPACR_EL1.modify(CPACR_EL1::FPEN::TrapNone);
 
@@ -34,8 +34,15 @@ fn __aa64_bsp_main(fdt_base: usize) {
 
     machine::init_board().unwrap();
 
-    // let fdt = DeviceTree::from_phys(fdt_base + 0xFFFFFF8000000000).expect("Failed to obtain a device tree");
-    // fdt.dump();
+    if fdt_base != 0 {
+        debugln!("fdt_base = {:#x}", fdt_base);
+        let fdt = DeviceTree::from_phys(fdt_base + 0xFFFFFF8000000000);
+        if let Ok(fdt) = fdt {
+            fdt.dump();
+        } else {
+            debugln!("Failed to init FDT");
+        }
+    }
 
     debugln!("Machine init finished");
 
diff --git a/kernel/src/arch/aarch64/boot/uboot.S b/kernel/src/arch/aarch64/boot/uboot.S
index 10f12d7..86a960f 100644
--- a/kernel/src/arch/aarch64/boot/uboot.S
+++ b/kernel/src/arch/aarch64/boot/uboot.S
@@ -77,3 +77,8 @@ __aa64_entry_upper:
 bsp_stack_bottom:
     .skip 32768
 bsp_stack_top:
+
+.section .data
+.p2align 4
+fdt_base_phys:
+    .skip 8