From d588b8c462d780cea27489a0dc697bf0c5e36984 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Sun, 19 Jul 2015 18:12:56 -0700
Subject: [PATCH] GBA BIOS: Implement Stop

---
 CHANGES             | 1 +
 src/gba/bios.c      | 3 +++
 src/gba/gba.c       | 9 +++++++++
 src/gba/gba.h       | 2 ++
 src/gba/interface.h | 4 ++++
 src/gba/io.c        | 2 +-
 6 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/CHANGES b/CHANGES
index e79ad4979..ab17d9970 100644
--- a/CHANGES
+++ b/CHANGES
@@ -28,6 +28,7 @@ Features:
  - Controller profiles now store shortcut settings
  - Default controller profiles for several common controllers
  - Libretro now supports BIOS and rumble
+ - Implement BIOS call Stop, for sleep mode
 Bugfixes:
  - ARM7: Fix SWI and IRQ timings
  - GBA Audio: Force audio FIFOs to 32-bit
diff --git a/src/gba/bios.c b/src/gba/bios.c
index 3af31b9fd..05404af97 100644
--- a/src/gba/bios.c
+++ b/src/gba/bios.c
@@ -191,6 +191,9 @@ void GBASwi16(struct ARMCore* cpu, int immediate) {
 	case 0x2:
 		GBAHalt(gba);
 		break;
+	case 0x3:
+		GBAStop(gba);
+		break;
 	case 0x05:
 	// VBlankIntrWait
 	// Fall through:
diff --git a/src/gba/gba.c b/src/gba/gba.c
index 9cf50a3ef..d4aa5d98e 100644
--- a/src/gba/gba.c
+++ b/src/gba/gba.c
@@ -82,6 +82,7 @@ static void GBAInit(struct ARMCore* cpu, struct ARMComponent* component) {
 	gba->logLevel = GBA_LOG_WARN | GBA_LOG_ERROR | GBA_LOG_FATAL;
 	gba->stream = 0;
 	gba->keyCallback = 0;
+	gba->stopCallback = 0;
 
 	gba->biosChecksum = GBAChecksum(gba->memory.bios, SIZE_BIOS);
 
@@ -553,6 +554,14 @@ void GBAHalt(struct GBA* gba) {
 	gba->cpu->halted = 1;
 }
 
+void GBAStop(struct GBA* gba) {
+	if (!gba->stopCallback) {
+		return;
+	}
+	gba->cpu->nextEvent = 0;
+	gba->stopCallback->stop(gba->stopCallback);
+}
+
 static void _GBAVLog(struct GBA* gba, enum GBALogLevel level, const char* format, va_list args) {
 	struct GBAThread* threadContext = GBAThreadGetContext();
 	enum GBALogLevel logLevel = GBA_LOG_ALL;
diff --git a/src/gba/gba.h b/src/gba/gba.h
index 0e5d92a42..46eea057c 100644
--- a/src/gba/gba.h
+++ b/src/gba/gba.h
@@ -112,6 +112,7 @@ struct GBA {
 	enum GBALogLevel logLevel;
 	struct GBAAVStream* stream;
 	struct GBAKeyCallback* keyCallback;
+	struct GBAStopCallback* stopCallback;
 
 	enum GBAIdleLoopOptimization idleOptimization;
 	uint32_t idleLoop;
@@ -155,6 +156,7 @@ void GBAWriteIME(struct GBA* gba, uint16_t value);
 void GBARaiseIRQ(struct GBA* gba, enum GBAIRQ irq);
 void GBATestIRQ(struct ARMCore* cpu);
 void GBAHalt(struct GBA* gba);
+void GBAStop(struct GBA* gba);
 
 void GBAAttachDebugger(struct GBA* gba, struct ARMDebugger* debugger);
 void GBADetachDebugger(struct GBA* gba);
diff --git a/src/gba/interface.h b/src/gba/interface.h
index 7b5e1c9f5..3daf0dcb8 100644
--- a/src/gba/interface.h
+++ b/src/gba/interface.h
@@ -66,6 +66,10 @@ struct GBAKeyCallback {
 	uint16_t (*readKeys)(struct GBAKeyCallback*);
 };
 
+struct GBAStopCallback {
+	void (*stop)(struct GBAStopCallback*);
+};
+
 struct GBARotationSource {
 	void (*sample)(struct GBARotationSource*);
 
diff --git a/src/gba/io.c b/src/gba/io.c
index f5e383713..304a6253a 100644
--- a/src/gba/io.c
+++ b/src/gba/io.c
@@ -505,7 +505,7 @@ void GBAIOWrite8(struct GBA* gba, uint32_t address, uint8_t value) {
 		if (!value) {
 			GBAHalt(gba);
 		} else {
-			GBALog(gba, GBA_LOG_STUB, "Stop unimplemented");
+			GBAStop(gba);
 		}
 		return;
 	}