From dea143829a6e45633f1a70702ad40584130e17b1 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Sat, 5 Jul 2014 01:01:29 -0700
Subject: [PATCH] Prevent lr from being trampled in the middle of long-running
 CpuSet

---
 src/gba/hle-bios.c | 44 +++++++++++++++++++++++---------------------
 src/gba/hle-bios.s | 10 ++++++++--
 2 files changed, 31 insertions(+), 23 deletions(-)

diff --git a/src/gba/hle-bios.c b/src/gba/hle-bios.c
index ef33f5d55..58f1933f3 100644
--- a/src/gba/hle-bios.c
+++ b/src/gba/hle-bios.c
@@ -2,7 +2,7 @@
 
 #include "gba-memory.h"
 
-const size_t hleBiosLength = 480;
+const size_t hleBiosLength = 504;
 const uint8_t hleBios[SIZE_BIOS] = {
   0x06, 0x00, 0x00, 0xea, 0xfe, 0xff, 0xff, 0xea, 0x05, 0x00, 0x00, 0xea,
   0xfe, 0xff, 0xff, 0xea, 0xfe, 0xff, 0xff, 0xea, 0x00, 0x00, 0xa0, 0xe1,
@@ -15,7 +15,7 @@ const uint8_t hleBios[SIZE_BIOS] = {
   0x00, 0x00, 0x00, 0x00, 0xa8, 0x00, 0x00, 0x00, 0xa0, 0x00, 0x00, 0x00,
   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf4, 0x00, 0x00, 0x00,
-  0x7c, 0x01, 0x00, 0x00, 0x0f, 0x50, 0x2d, 0xe9, 0x01, 0x03, 0xa0, 0xe3,
+  0x88, 0x01, 0x00, 0x00, 0x0f, 0x50, 0x2d, 0xe9, 0x01, 0x03, 0xa0, 0xe3,
   0x00, 0xe0, 0x8f, 0xe2, 0x04, 0xf0, 0x10, 0xe5, 0x0f, 0x50, 0xbd, 0xe8,
   0x04, 0xf0, 0x5e, 0xe2, 0x01, 0x00, 0xa0, 0xe3, 0x01, 0x10, 0xa0, 0xe3,
   0x0c, 0x40, 0x2d, 0xe9, 0x00, 0x50, 0x4f, 0xe1, 0x1f, 0xf0, 0x29, 0xe3,
@@ -24,24 +24,26 @@ const uint8_t hleBios[SIZE_BIOS] = {
   0x08, 0x02, 0xc4, 0xe5, 0xb8, 0x30, 0x54, 0xe1, 0x01, 0x30, 0x13, 0xe0,
   0x01, 0x30, 0x23, 0x10, 0xb8, 0x30, 0x44, 0x11, 0x08, 0x22, 0xc4, 0xe5,
   0xf7, 0xff, 0xff, 0x0a, 0x93, 0xf0, 0x29, 0xe3, 0x05, 0xf0, 0x69, 0xe1,
-  0x0c, 0x80, 0xbd, 0xe8, 0x1f, 0xf0, 0x29, 0xe3, 0x02, 0x36, 0xa0, 0xe1,
-  0x23, 0x36, 0xa0, 0xe1, 0x01, 0x04, 0x12, 0xe3, 0x0d, 0x00, 0x00, 0x0a,
-  0x01, 0x03, 0x12, 0xe3, 0x04, 0x00, 0x00, 0x0a, 0x04, 0x00, 0xb0, 0xe8,
-  0x04, 0x00, 0xa1, 0xe8, 0x01, 0x30, 0x53, 0xe2, 0xfc, 0xff, 0xff, 0x1a,
-  0x13, 0x00, 0x00, 0xea, 0x01, 0x00, 0xc0, 0xe3, 0x01, 0x10, 0xc1, 0xe3,
-  0xb0, 0x20, 0xd0, 0xe1, 0xb2, 0x20, 0xc1, 0xe0, 0x01, 0x30, 0x53, 0xe2,
-  0xfc, 0xff, 0xff, 0x1a, 0x0c, 0x00, 0x00, 0xea, 0x01, 0x03, 0x12, 0xe3,
+  0x0c, 0x80, 0xbd, 0xe8, 0x00, 0x40, 0x2d, 0xe9, 0x00, 0x50, 0x4f, 0xe1,
+  0x1f, 0xf0, 0x29, 0xe3, 0x02, 0x36, 0xa0, 0xe1, 0x23, 0x36, 0xa0, 0xe1,
+  0x01, 0x04, 0x12, 0xe3, 0x0d, 0x00, 0x00, 0x0a, 0x01, 0x03, 0x12, 0xe3,
   0x04, 0x00, 0x00, 0x0a, 0x04, 0x00, 0xb0, 0xe8, 0x04, 0x00, 0xa1, 0xe8,
-  0x01, 0x30, 0x53, 0xe2, 0xfb, 0xff, 0xff, 0x1a, 0x05, 0x00, 0x00, 0xea,
-  0x01, 0x00, 0xc0, 0xe3, 0x01, 0x10, 0xc1, 0xe3, 0xb2, 0x20, 0xd0, 0xe0,
-  0xb2, 0x20, 0xc1, 0xe0, 0x01, 0x30, 0x53, 0xe2, 0xfb, 0xff, 0xff, 0x1a,
-  0x93, 0xf0, 0x29, 0xe3, 0x1e, 0xff, 0x2f, 0xe1, 0x1f, 0xf0, 0x29, 0xe3,
-  0xf0, 0x07, 0x2d, 0xe9, 0x01, 0x04, 0x12, 0xe3, 0x02, 0x36, 0xa0, 0xe1,
-  0x23, 0x26, 0xa0, 0xe1, 0x0b, 0x00, 0x00, 0x0a, 0x10, 0x00, 0xb0, 0xe8,
-  0x04, 0x50, 0xa0, 0xe1, 0x04, 0x30, 0xa0, 0xe1, 0x04, 0x60, 0xa0, 0xe1,
-  0x04, 0x70, 0xa0, 0xe1, 0x04, 0x80, 0xa0, 0xe1, 0x04, 0x90, 0xa0, 0xe1,
-  0x04, 0xa0, 0xa0, 0xe1, 0xf8, 0x07, 0xa1, 0xe8, 0x08, 0x20, 0x52, 0xe2,
-  0xfc, 0xff, 0xff, 0xca, 0x03, 0x00, 0x00, 0xea, 0xf8, 0x07, 0xb0, 0xe8,
-  0xf8, 0x07, 0xa1, 0xe8, 0x08, 0x20, 0x52, 0xe2, 0xfb, 0xff, 0xff, 0xca,
-  0xf0, 0x07, 0xbd, 0xe8, 0x93, 0xf0, 0x29, 0xe3, 0x1e, 0xff, 0x2f, 0xe1
+  0x01, 0x30, 0x53, 0xe2, 0xfc, 0xff, 0xff, 0x1a, 0x13, 0x00, 0x00, 0xea,
+  0x01, 0x00, 0xc0, 0xe3, 0x01, 0x10, 0xc1, 0xe3, 0xb0, 0x20, 0xd0, 0xe1,
+  0xb2, 0x20, 0xc1, 0xe0, 0x01, 0x30, 0x53, 0xe2, 0xfc, 0xff, 0xff, 0x1a,
+  0x0c, 0x00, 0x00, 0xea, 0x01, 0x03, 0x12, 0xe3, 0x04, 0x00, 0x00, 0x0a,
+  0x04, 0x00, 0xb0, 0xe8, 0x04, 0x00, 0xa1, 0xe8, 0x01, 0x30, 0x53, 0xe2,
+  0xfb, 0xff, 0xff, 0x1a, 0x05, 0x00, 0x00, 0xea, 0x01, 0x00, 0xc0, 0xe3,
+  0x01, 0x10, 0xc1, 0xe3, 0xb2, 0x20, 0xd0, 0xe0, 0xb2, 0x20, 0xc1, 0xe0,
+  0x01, 0x30, 0x53, 0xe2, 0xfb, 0xff, 0xff, 0x1a, 0x93, 0xf0, 0x29, 0xe3,
+  0x05, 0xf0, 0x69, 0xe1, 0x00, 0x80, 0xbd, 0xe8, 0x00, 0x40, 0x2d, 0xe9,
+  0x00, 0x50, 0x4f, 0xe1, 0x1f, 0xf0, 0x29, 0xe3, 0xf0, 0x07, 0x2d, 0xe9,
+  0x01, 0x04, 0x12, 0xe3, 0x02, 0x36, 0xa0, 0xe1, 0x23, 0x26, 0xa0, 0xe1,
+  0x0b, 0x00, 0x00, 0x0a, 0x10, 0x00, 0xb0, 0xe8, 0x04, 0x50, 0xa0, 0xe1,
+  0x04, 0x30, 0xa0, 0xe1, 0x04, 0x60, 0xa0, 0xe1, 0x04, 0x70, 0xa0, 0xe1,
+  0x04, 0x80, 0xa0, 0xe1, 0x04, 0x90, 0xa0, 0xe1, 0x04, 0xa0, 0xa0, 0xe1,
+  0xf8, 0x07, 0xa1, 0xe8, 0x08, 0x20, 0x52, 0xe2, 0xfc, 0xff, 0xff, 0xca,
+  0x03, 0x00, 0x00, 0xea, 0xf8, 0x07, 0xb0, 0xe8, 0xf8, 0x07, 0xa1, 0xe8,
+  0x08, 0x20, 0x52, 0xe2, 0xfb, 0xff, 0xff, 0xca, 0xf0, 0x07, 0xbd, 0xe8,
+  0x93, 0xf0, 0x29, 0xe3, 0x05, 0xf0, 0x69, 0xe1, 0x00, 0x80, 0xbd, 0xe8
 };
diff --git a/src/gba/hle-bios.s b/src/gba/hle-bios.s
index ad85da6f5..e6cd3c742 100644
--- a/src/gba/hle-bios.s
+++ b/src/gba/hle-bios.s
@@ -83,6 +83,8 @@ msr    spsr, r5
 ldmfd  sp!, {r2-r3, pc}
 
 CpuSet:
+stmfd  sp!, {lr}
+mrs    r5, spsr
 msr    cpsr, #0x1F
 mov    r3, r2, lsl #12
 mov    r3, r3, lsr #12
@@ -130,9 +132,12 @@ subs   r3, #1
 bne    2b
 3:
 msr    cpsr, #0x93
-bx     lr
+msr    spsr, r5
+ldmfd  sp!, {pc}
 
 CpuFastSet:
+stmfd  sp!, {lr}
+mrs    r5, spsr
 msr    cpsr, #0x1F
 stmfd  sp!, {r4-r10}
 tst    r2, #0x01000000
@@ -162,4 +167,5 @@ bgt    0b
 2:
 ldmfd  sp!, {r4-r10}
 msr    cpsr, #0x93
-bx     lr
+msr    spsr, r5
+ldmfd  sp!, {pc}