From 22b259e8184d514a6fce69717981ead5c2071662 Mon Sep 17 00:00:00 2001
From: SashegDev <gdsasheg@gmail.com>
Date: Fri, 11 Sep 2026 06:29:06 +0000
Subject: [PATCH] sbl b14: S6 pop redirected to stublet 0x8062140 (was
 null-sled)

---
 qemu/s6stub.S      | 13 +++++++++++++
 qemu/saimaa.c      | 17 +++++++++++++++++
 tools/sbl_patch.py | 22 ++++++++++++++++++++++
 3 files changed, 52 insertions(+)
 create mode 100644 qemu/s6stub.S

diff --git a/qemu/s6stub.S b/qemu/s6stub.S
new file mode 100644
index 0000000..96ea36c
--- /dev/null
+++ b/qemu/s6stub.S
@@ -0,0 +1,13 @@
+    .syntax unified
+    .cpu cortex-a7
+    .thumb
+    @ S6: compensation for ldmia.w sp!,{r2-r9,sl,pc} at 0x8017afc
+    @ (pc slot gets clobbered to 0 at runtime). Rebuild frame regs
+    @ from stack, return to caller 0x8013ba5.
+    .text
+    .global s6_stub
+s6_stub:
+    ldmia.w sp!, {r2-r9, sl, lr}
+    movw lr, #:lower16:0x8013ba5
+    movt lr, #:upper16:0x8013ba5
+    bx lr
diff --git a/qemu/saimaa.c b/qemu/saimaa.c
index c5d2e68..514a1d5 100644
--- a/qemu/saimaa.c
+++ b/qemu/saimaa.c
@@ -422,6 +422,23 @@ static void saimaa_machine_init(MachineState *machine)
             cpu_physical_memory_write(0x087c29d18, &S, 4);
             cpu_physical_memory_write(0x08050b28, &S, 4);
             cpu_physical_memory_write(0x08070000, &S, 4);
+            /* S6 stublet at 0x08062140: rebuild popped frame regs,
+             * return to 0x8013ba5 (b14 redirects 0x8017afc pop here) */
+            {
+                static const uint8_t s6[] = {
+                    0xbd, 0xe8, 0xfc, 0x47, /* ldmia.w sp!,{r2-r9,sl,lr} */
+                    0x43, 0xf6, 0xa5, 0x3e, /* movw lr,#0x3ba5 */
+                    0xc0, 0xf6, 0x01, 0x0e, /* movt lr,#0x801 */
+                    0x70, 0x47,             /* bx lr */
+                };
+                size_t k;
+                for (k = 0; k < sizeof(s6); k += 4) {
+                    uint32_t word = 0;
+                    size_t n = sizeof(s6) - k < 4 ? sizeof(s6) - k : 4;
+                    __builtin_memcpy(&word, &s6[k], n);
+                    cpu_physical_memory_write(0x08062140 + k, &word, 4);
+                }
+            }
             /* stack zone defaults to plain-stub addr: every register
              * popped from untouched stack becomes callable (returns 0). */
             {
diff --git a/tools/sbl_patch.py b/tools/sbl_patch.py
index 7d1c1b6..6fc9730 100644
--- a/tools/sbl_patch.py
+++ b/tools/sbl_patch.py
@@ -133,6 +133,28 @@ def main():
     assert d[o:o + 4].hex() == '07f026be', hx(d, 0x8013AA8)
     d[o:o + 4] = bytes.fromhex('c046c046')
     save(d, 'img0_b12.elf')
+    # b14 = b13 + redirect S6 pop (0x8017afc ldmia.w sp!,{r2-r9,sl,pc})
+    # to S6 stublet at 0x08062140 (pc slot clobbered to 0 at runtime)
+    d = bytearray(base)
+    for va, (a, b) in BLX2BL.items():
+        set4(d, va, b)
+    for va, exp, new in [
+        (0x802F664, 'e4f77efc', 'c046c046'),
+        (0x802F672, 'e6f7bbfa', 'c046c046'),
+        (0x802F678, 'e9f7fef9', 'c046c046'),
+        (0x801B71E, '9847fee7', 'c046c046'),
+        (0x8008396, 'fef780f9', 'c046c046'),
+        (0x8016184, 'fdf790fc', 'c046c046'),
+        (0x801B76C, '280b0508', '00000708'),
+        (0x8006684, '01f807bd', '46c046c0'),
+        (0x8017AFC, 'e8bd87fc', '4af020bb'),
+    ]:
+        assert hx(d, va) == exp, (hex(va), hx(d, va))
+        set4(d, va, new)
+    o = off(0x8013AA8)
+    assert d[o:o + 4].hex() == '07f026be', hx(d, 0x8013AA8)
+    d[o:o + 4] = bytes.fromhex('c046c046')
+    save(d, 'img0_b14.elf')
     # b13 = b12 + NOP strb.w fp,[r1,#-7]! at 0x8006684 (helper stores
     # fp into struct r1, but r1 arrives NULL from 0x8007e82 chain;
     # store to [0-7]=0xfffffff9 faults; struct unrecoverable here)