From 9c23eb8428b2fd81d00c95ea1be56b508ba2b115 Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Fri, 2 Jun 2017 18:34:24 -0700
Subject: [PATCH] GBA: Add keypad IRQs (fixes #733)

---
 CHANGES                         |  1 +
 include/mgba/internal/gba/gba.h |  2 ++
 src/gba/core.c                  |  3 +++
 src/gba/gba.c                   | 33 ++++++++++++++++++++++++++++-----
 src/gba/io.c                    | 11 ++++++++++-
 5 files changed, 44 insertions(+), 6 deletions(-)

diff --git a/CHANGES b/CHANGES
index a265a0d4e..2767ecde0 100644
--- a/CHANGES
+++ b/CHANGES
@@ -21,6 +21,7 @@ Features:
  - Debugger: Segment/bank support
  - GB: Symbol table support
  - GB MBC: Add MBC1 multicart support
+ - GBA: Implement keypad interrupts
 Bugfixes:
  - LR35902: Fix core never exiting with certain event patterns
  - GB Timer: Improve DIV reset behavior
diff --git a/include/mgba/internal/gba/gba.h b/include/mgba/internal/gba/gba.h
index 732a4aba6..5710864e0 100644
--- a/include/mgba/internal/gba/gba.h
+++ b/include/mgba/internal/gba/gba.h
@@ -175,6 +175,8 @@ bool GBAIsBIOS(struct VFile* vf);
 void GBAGetGameCode(const struct GBA* gba, char* out);
 void GBAGetGameTitle(const struct GBA* gba, char* out);
 
+void GBATestKeypadIRQ(struct GBA* gba);
+
 void GBAFrameStarted(struct GBA* gba);
 void GBAFrameEnded(struct GBA* gba);
 
diff --git a/src/gba/core.c b/src/gba/core.c
index 6f81e2570..32272bb50 100644
--- a/src/gba/core.c
+++ b/src/gba/core.c
@@ -398,16 +398,19 @@ static bool _GBACoreSaveState(struct mCore* core, void* state) {
 static void _GBACoreSetKeys(struct mCore* core, uint32_t keys) {
 	struct GBACore* gbacore = (struct GBACore*) core;
 	gbacore->keys = keys;
+	GBATestKeypadIRQ(core->board);
 }
 
 static void _GBACoreAddKeys(struct mCore* core, uint32_t keys) {
 	struct GBACore* gbacore = (struct GBACore*) core;
 	gbacore->keys |= keys;
+	GBATestKeypadIRQ(core->board);
 }
 
 static void _GBACoreClearKeys(struct mCore* core, uint32_t keys) {
 	struct GBACore* gbacore = (struct GBACore*) core;
 	gbacore->keys &= ~keys;
+	GBATestKeypadIRQ(core->board);
 }
 
 static int32_t _GBACoreFrameCounter(const struct mCore* core) {
diff --git a/src/gba/gba.c b/src/gba/gba.c
index 4a145f5d8..1a1ee4b2d 100644
--- a/src/gba/gba.c
+++ b/src/gba/gba.c
@@ -409,10 +409,6 @@ void GBAApplyPatch(struct GBA* gba, struct Patch* patch) {
 }
 
 void GBAWriteIE(struct GBA* gba, uint16_t value) {
-	if (value & (1 << IRQ_KEYPAD)) {
-		mLOG(GBA, STUB, "Keypad interrupts not implemented");
-	}
-
 	if (gba->memory.io[REG_IME >> 1] && value & gba->memory.io[REG_IF >> 1]) {
 		ARMRaiseIRQ(gba->cpu);
 	}
@@ -637,7 +633,7 @@ void GBABreakpoint(struct ARMCore* cpu, int immediate) {
 }
 
 void GBAFrameStarted(struct GBA* gba) {
-	UNUSED(gba);
+	GBATestKeypadIRQ(gba);
 
 	size_t c;
 	for (c = 0; c < mCoreCallbacksListSize(&gba->coreCallbacks); ++c) {
@@ -684,6 +680,33 @@ void GBAFrameEnded(struct GBA* gba) {
 	}
 }
 
+void GBATestKeypadIRQ(struct GBA* gba) {
+	uint16_t keycnt = gba->memory.io[REG_KEYCNT >> 1];
+	if (!(keycnt & 0x4000)) {
+		return;
+	}
+	int isAnd = keycnt & 0x8000;
+	uint16_t keyInput;
+
+	if (!gba->keySource) {
+		// TODO?
+		return;
+	}
+
+	keycnt &= 0x3FF;
+	keyInput = *gba->keySource;
+
+	if (popcount32(keyInput) > 2) {
+		keycnt = keycnt;
+	}
+
+	if (isAnd && keycnt == keyInput) {
+		GBARaiseIRQ(gba, IRQ_KEYPAD);
+	} else if (!isAnd && keyInput) {
+		GBARaiseIRQ(gba, IRQ_KEYPAD);
+	}
+}
+
 void GBASetBreakpoint(struct GBA* gba, struct mCPUComponent* component, uint32_t address, enum ExecutionMode mode, uint32_t* opcode) {
 	size_t immediate;
 	for (immediate = 0; immediate < gba->cpu->numComponents; ++immediate) {
diff --git a/src/gba/io.c b/src/gba/io.c
index 9574ec9f9..4d36f2800 100644
--- a/src/gba/io.c
+++ b/src/gba/io.c
@@ -537,6 +537,11 @@ void GBAIOWrite(struct GBA* gba, uint32_t address, uint16_t value) {
 			break;
 
 		// Interrupts and misc
+		case REG_KEYCNT:
+			value &= 0xC3FF;
+			gba->memory.io[address >> 1] = value;
+			GBATestKeypadIRQ(gba);
+			return;
 		case REG_WAITCNT:
 			value &= 0x5FFF;
 			GBAAdjustWaitstates(gba, value);
@@ -689,6 +694,7 @@ bool GBAIOIsReadConstant(uint32_t address) {
 	case REG_TM2CNT_HI:
 	case REG_TM3CNT_HI:
 	case REG_KEYINPUT:
+	case REG_KEYCNT:
 	case REG_IE:
 		return true;
 	}
@@ -721,6 +727,9 @@ uint16_t GBAIORead(struct GBA* gba, uint32_t address) {
 			uint16_t input = 0x3FF;
 			if (gba->keyCallback) {
 				input = gba->keyCallback->readKeys(gba->keyCallback);
+				if (gba->keySource) {
+					*gba->keySource = input;
+				}
 			} else if (gba->keySource) {
 				input = *gba->keySource;
 			}
@@ -814,7 +823,6 @@ uint16_t GBAIORead(struct GBA* gba, uint32_t address) {
 		break;
 
 	case REG_SOUNDBIAS:
-	case REG_KEYCNT:
 	case REG_POSTFLG:
 		mLOG(GBA_IO, STUB, "Stub I/O register read: %03x", address);
 		break;
@@ -863,6 +871,7 @@ uint16_t GBAIORead(struct GBA* gba, uint32_t address) {
 	case REG_TM1CNT_HI:
 	case REG_TM2CNT_HI:
 	case REG_TM3CNT_HI:
+	case REG_KEYCNT:
 	case REG_SIOMULTI0:
 	case REG_SIOMULTI1:
 	case REG_SIOMULTI2: