From 0faa6dd8acef62296cd9f352f404bf0555f7ae73 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Thu, 5 Nov 2015 20:57:56 -0800
Subject: [PATCH] GBA BIOS: Fix misaligned CpuSet

---
 CHANGES            |  1 +
 src/gba/bios.c     |  6 ++++++
 src/gba/hle-bios.c | 28 ++++++++++++++--------------
 src/gba/hle-bios.s |  2 --
 4 files changed, 21 insertions(+), 16 deletions(-)

diff --git a/CHANGES b/CHANGES
index 84c759178..564a515fb 100644
--- a/CHANGES
+++ b/CHANGES
@@ -15,6 +15,7 @@ Bugfixes:
  - Qt: Fix font size in memory viewer
  - GBA Memory: Fix DMA register writing behavior
  - Qt: Fix a crash in the memory viewer
+ - GBA BIOS: Fix misaligned CpuSet
 Misc:
  - Qt: Window size command line options are now supported
  - Qt: Increase usability of key mapper
diff --git a/src/gba/bios.c b/src/gba/bios.c
index 9ea5b5dc9..a00555c1e 100644
--- a/src/gba/bios.c
+++ b/src/gba/bios.c
@@ -224,6 +224,12 @@ void GBASwi16(struct ARMCore* cpu, int immediate) {
 			GBALog(gba, GBA_LOG_GAME_ERROR, "Cannot CpuSet from BIOS");
 			return;
 		}
+		if (cpu->gprs[0] & (cpu->gprs[2] & (1 << 26) ? 3 : 1)) {
+			GBALog(gba, GBA_LOG_GAME_ERROR, "Misaligned CpuSet source");
+		}
+		if (cpu->gprs[1] & (cpu->gprs[2] & (1 << 26) ? 3 : 1)) {
+			GBALog(gba, GBA_LOG_GAME_ERROR, "Misaligned CpuSet destination");
+		}
 		ARMRaiseSWI(cpu);
 		break;
 	case 0xD:
diff --git a/src/gba/hle-bios.c b/src/gba/hle-bios.c
index 8b9b60c94..eff5e682f 100644
--- a/src/gba/hle-bios.c
+++ b/src/gba/hle-bios.c
@@ -20,7 +20,7 @@ const uint8_t hleBios[SIZE_BIOS] = {
 	0x00, 0x00, 0x00, 0x00, 0xf8, 0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x00,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x01, 0x00, 0x00,
-	0xd0, 0x01, 0x00, 0x00, 0x0f, 0x50, 0x2d, 0xe9, 0x01, 0x03, 0xa0, 0xe3,
+	0xc8, 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, 0x00, 0x00, 0x00, 0x00, 0x02, 0xc0, 0x5e, 0xe5,
 	0x01, 0x00, 0xa0, 0xe3, 0x01, 0x10, 0xa0, 0xe3, 0x0c, 0x40, 0x2d, 0xe9,
@@ -33,20 +33,20 @@ const uint8_t hleBios[SIZE_BIOS] = {
 	0x02, 0x36, 0xa0, 0xe1, 0x01, 0x04, 0x12, 0xe3, 0x0f, 0x00, 0x00, 0x0a,
 	0x01, 0x03, 0x12, 0xe3, 0x05, 0x00, 0x00, 0x0a, 0x23, 0x35, 0x81, 0xe0,
 	0x04, 0x00, 0xb0, 0xe8, 0x03, 0x00, 0x51, 0xe1, 0x04, 0x00, 0xa1, 0xb8,
-	0xfc, 0xff, 0xff, 0xba, 0x16, 0x00, 0x00, 0xea, 0x01, 0x00, 0xc0, 0xe3,
+	0xfc, 0xff, 0xff, 0xba, 0x14, 0x00, 0x00, 0xea, 0x01, 0x00, 0xc0, 0xe3,
 	0x01, 0x10, 0xc1, 0xe3, 0xa3, 0x35, 0x81, 0xe0, 0xb0, 0x20, 0xd0, 0xe1,
 	0x03, 0x00, 0x51, 0xe1, 0xb2, 0x20, 0xc1, 0xb0, 0xfc, 0xff, 0xff, 0xba,
-	0x0e, 0x00, 0x00, 0xea, 0x01, 0x03, 0x12, 0xe3, 0x05, 0x00, 0x00, 0x0a,
+	0x0c, 0x00, 0x00, 0xea, 0x01, 0x03, 0x12, 0xe3, 0x05, 0x00, 0x00, 0x0a,
 	0x23, 0x35, 0x81, 0xe0, 0x03, 0x00, 0x51, 0xe1, 0x04, 0x00, 0xb0, 0xb8,
-	0x04, 0x00, 0xa1, 0xb8, 0xfb, 0xff, 0xff, 0xba, 0x06, 0x00, 0x00, 0xea,
-	0xa3, 0x35, 0x81, 0xe0, 0x01, 0x00, 0xc0, 0xe3, 0x01, 0x10, 0xc1, 0xe3,
-	0x03, 0x00, 0x51, 0xe1, 0xb2, 0x20, 0xd0, 0xb0, 0xb2, 0x20, 0xc1, 0xb0,
-	0xfb, 0xff, 0xff, 0xba, 0x00, 0x80, 0xbd, 0xe8, 0xf0, 0x47, 0x2d, 0xe9,
-	0x01, 0x04, 0x12, 0xe3, 0x02, 0x36, 0xa0, 0xe1, 0x23, 0x25, 0x81, 0xe0,
-	0x0b, 0x00, 0x00, 0x0a, 0x00, 0x30, 0x90, 0xe5, 0x03, 0x40, 0xa0, 0xe1,
-	0x03, 0x50, 0xa0, 0xe1, 0x03, 0x60, 0xa0, 0xe1, 0x03, 0x70, 0xa0, 0xe1,
-	0x03, 0x80, 0xa0, 0xe1, 0x03, 0x90, 0xa0, 0xe1, 0x03, 0xa0, 0xa0, 0xe1,
-	0x02, 0x00, 0x51, 0xe1, 0xf8, 0x07, 0xa1, 0xb8, 0xfc, 0xff, 0xff, 0xba,
-	0x03, 0x00, 0x00, 0xea, 0x02, 0x00, 0x51, 0xe1, 0xf8, 0x07, 0xb0, 0xb8,
-	0xf8, 0x07, 0xa1, 0xb8, 0xfb, 0xff, 0xff, 0xba, 0xf0, 0x87, 0xbd, 0xe8
+	0x04, 0x00, 0xa1, 0xb8, 0xfb, 0xff, 0xff, 0xba, 0x04, 0x00, 0x00, 0xea,
+	0xa3, 0x35, 0x81, 0xe0, 0x03, 0x00, 0x51, 0xe1, 0xb2, 0x20, 0xd0, 0xb0,
+	0xb2, 0x20, 0xc1, 0xb0, 0xfb, 0xff, 0xff, 0xba, 0x00, 0x80, 0xbd, 0xe8,
+	0xf0, 0x47, 0x2d, 0xe9, 0x01, 0x04, 0x12, 0xe3, 0x02, 0x36, 0xa0, 0xe1,
+	0x23, 0x25, 0x81, 0xe0, 0x0b, 0x00, 0x00, 0x0a, 0x00, 0x30, 0x90, 0xe5,
+	0x03, 0x40, 0xa0, 0xe1, 0x03, 0x50, 0xa0, 0xe1, 0x03, 0x60, 0xa0, 0xe1,
+	0x03, 0x70, 0xa0, 0xe1, 0x03, 0x80, 0xa0, 0xe1, 0x03, 0x90, 0xa0, 0xe1,
+	0x03, 0xa0, 0xa0, 0xe1, 0x02, 0x00, 0x51, 0xe1, 0xf8, 0x07, 0xa1, 0xb8,
+	0xfc, 0xff, 0xff, 0xba, 0x03, 0x00, 0x00, 0xea, 0x02, 0x00, 0x51, 0xe1,
+	0xf8, 0x07, 0xb0, 0xb8, 0xf8, 0x07, 0xa1, 0xb8, 0xfb, 0xff, 0xff, 0xba,
+	0xf0, 0x87, 0xbd, 0xe8
 };
diff --git a/src/gba/hle-bios.s b/src/gba/hle-bios.s
index 5f83cb381..19e00127d 100644
--- a/src/gba/hle-bios.s
+++ b/src/gba/hle-bios.s
@@ -147,8 +147,6 @@ b      3f
 # Halfword
 1:
 add    r3, r1, r3, lsr #11
-bic    r0, #1
-bic    r1, #1
 2:
 cmp    r1, r3
 ldrlth r2, [r0], #2