From 0ef9f7ccc8100ec659a78e24879412951610703b Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Sat, 4 Sep 2021 23:00:06 -0700
Subject: [PATCH] fusee_cpp: improved relocation logic

---
 .../loader_stub/source/fusee_loader_start.s   | 40 +++++++++++++++----
 1 file changed, 33 insertions(+), 7 deletions(-)

diff --git a/fusee_cpp/loader_stub/source/fusee_loader_start.s b/fusee_cpp/loader_stub/source/fusee_loader_start.s
index 70ced975e..8ed578fe3 100644
--- a/fusee_cpp/loader_stub/source/fusee_loader_start.s
+++ b/fusee_cpp/loader_stub/source/fusee_loader_start.s
@@ -25,21 +25,49 @@ _ZN3ams6nxboot6loader5StartEv:
     /* Switch to system mode, mask all interrupts, clear all flags */
     msr cpsr_cxsf, #0xDF
 
-    /* Relocate loader stub. */
+    /* Relocate main program. */
     ldr r0, =_ZN3ams6nxboot6loader5StartEv
     adr r1, _ZN3ams6nxboot6loader5StartEv
+    cmp r0, r1
+    beq 3f
+
+    /* Relocate first 0x100. */
+    mov r4, #0x100
+    0:
+        ldmia r1!, {r5-r12}
+        stmia r0!, {r5-r12}
+        subs  r4, #0x20
+        bne 0b
+
+    /* Jump, continue relocating. */
+    ldr r3, =1f
+    bx r3
+
+    1:
+    ldr r4, =__loader_stub_lma__
+    ldr r3, =__loader_stub_end__
+    add r4, r4, r3
+    ldr r3, =__loader_stub_start__
+    sub r4, r4, r3
+    sub r4, r4, r0
+    2:
+        ldmia r1!, {r5-r12}
+        stmia r0!, {r5-r12}
+        subs  r4, #0x20
+        bne 2b
+
+    /* Relocate loader stub. */
+    3:
     ldr r2, =__loader_stub_lma__
-    sub r2, r2, r0
-    add r2, r2, r1
 
     ldr r3, =__loader_stub_start__
     ldr r4, =__loader_stub_end__
     sub r4, r4, r3
-    0:
+    4:
         ldmia r2!, {r5-r12}
         stmia r3!, {r5-r12}
         subs  r4, #0x20
-        bne 0b
+        bne 4b
 
     /* Set the stack pointer */
     ldr  sp, =0x40001000
@@ -49,8 +77,6 @@ _ZN3ams6nxboot6loader5StartEv:
     ldr r3, =program_lz4
     ldr r4, =program_lz4_end
     sub r4, r4, r3
-    sub r3, r3, r0
-    add r3, r3, r1
     mov r0, r3
     mov r1, r4