From 31b509e0333e27aedd9cafe0e0447ddc389d9dd1 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Sun, 17 Jan 2016 00:25:52 -0800
Subject: [PATCH] GB: Revamp STAT register

---
 src/gb/io.c    |  3 +++
 src/gb/video.c | 51 ++++++++++++++++++++++++++++++++++++++++----------
 src/gb/video.h |  8 ++++++++
 3 files changed, 52 insertions(+), 10 deletions(-)

diff --git a/src/gb/io.c b/src/gb/io.c
index 64fd82388..4635615ef 100644
--- a/src/gb/io.c
+++ b/src/gb/io.c
@@ -21,6 +21,9 @@ void GBIOWrite(struct GB* gb, unsigned address, uint8_t value) {
 		// TODO: handle GBC differences
 		GBVideoWriteLCDC(&gb->video, value);
 		break;
+	case REG_STAT:
+		GBVideoWriteSTAT(&gb->video, value);
+		break;
 	case REG_IE:
 		gb->memory.ie = value;
 		GBUpdateIRQs(gb);
diff --git a/src/gb/video.c b/src/gb/video.c
index 2a74e83c4..bac5edabd 100644
--- a/src/gb/video.c
+++ b/src/gb/video.c
@@ -77,27 +77,52 @@ int32_t GBVideoProcessEvents(struct GBVideo* video, int32_t cycles) {
 			video->nextMode -= video->eventDiff;
 		}
 		if (video->nextMode <= 0) {
-			video->mode = (video->mode + 1) & 3;
 			switch (video->mode) {
 			case 0:
-				video->nextMode = GB_VIDEO_MODE_0_LENGTH;
-				break;
-			case 1:
-				video->nextMode = GB_VIDEO_MODE_1_LENGTH;
-				break;
-			case 2:
-				video->nextMode = GB_VIDEO_MODE_2_LENGTH;
 				++video->ly;
+				video->p->memory.io[REG_LY] = video->ly;
 				if (video->ly >= GB_VIDEO_VERTICAL_TOTAL_PIXELS) {
 					video->ly = 0;
 					++video->frameCounter;
+					video->nextMode = GB_VIDEO_HORIZONTAL_LENGTH;
+					video->mode = 1;
+					if (GBRegisterSTATIsVblankIRQ(video->stat)) {
+						video->p->memory.io[REG_IF] |= (1 << GB_IRQ_LCDSTAT);
+					}
+					video->p->memory.io[REG_IF] |= (1 << GB_IRQ_VBLANK);
+					GBUpdateIRQs(video->p);
+				} else {
+					video->nextMode = GB_VIDEO_MODE_2_LENGTH;
+					video->mode = 2;
+					if (GBRegisterSTATIsOAMIRQ(video->stat)) {
+						video->p->memory.io[REG_IF] |= (1 << GB_IRQ_LCDSTAT);
+						GBUpdateIRQs(video->p);
+					}
 				}
-				video->p->memory.io[REG_LY] = video->ly;
+				break;
+			case 1:
+				video->nextMode = GB_VIDEO_MODE_2_LENGTH;
+				video->mode = 2;
+				if (GBRegisterSTATIsOAMIRQ(video->stat)) {
+					video->p->memory.io[REG_IF] |= (1 << GB_IRQ_LCDSTAT);
+					GBUpdateIRQs(video->p);
+				}
+				break;
+			case 2:
+				video->nextMode = GB_VIDEO_MODE_3_LENGTH;
+				video->mode = 3;
 				break;
 			case 3:
-				video->nextMode = GB_VIDEO_MODE_3_LENGTH;
+				video->nextMode = GB_VIDEO_MODE_0_LENGTH;
+				video->mode = 0;
+				if (GBRegisterSTATIsHblankIRQ(video->stat)) {
+					video->p->memory.io[REG_IF] |= (1 << GB_IRQ_LCDSTAT);
+					GBUpdateIRQs(video->p);
+				}
 				break;
 			}
+			video->stat = GBRegisterSTATSetMode(video->stat, video->mode);
+			video->p->memory.io[REG_STAT] = video->stat;
 		}
 
 		video->nextEvent = video->nextMode;
@@ -112,6 +137,8 @@ void GBVideoWriteLCDC(struct GBVideo* video, GBRegisterLCDC value) {
 		video->mode = 2;
 		video->nextMode = GB_VIDEO_MODE_2_LENGTH;
 		video->nextEvent = video->nextMode;
+		video->stat = GBRegisterSTATSetMode(video->stat, video->mode);
+		video->p->memory.io[REG_STAT] = video->stat;
 		video->eventDiff = 0;
 		if (video->nextEvent < video->p->cpu->nextEvent) {
 			video->p->cpu->nextEvent = video->nextEvent;
@@ -120,6 +147,10 @@ void GBVideoWriteLCDC(struct GBVideo* video, GBRegisterLCDC value) {
 	}
 }
 
+void GBVideoWriteSTAT(struct GBVideo* video, GBRegisterSTAT value) {
+	video->stat = (video->stat & 0x7) | (value & 0x78);
+}
+
 static void GBVideoDummyRendererInit(struct GBVideoRenderer* renderer) {
 	UNUSED(renderer);
 	// Nothing to do
diff --git a/src/gb/video.h b/src/gb/video.h
index ee22e17d3..f42efbf23 100644
--- a/src/gb/video.h
+++ b/src/gb/video.h
@@ -46,12 +46,20 @@ DECL_BITFIELD(GBRegisterLCDC, uint8_t);
 DECL_BIT(GBRegisterLCDC, Enable, 7);
 
 DECL_BITFIELD(GBRegisterSTAT, uint8_t);
+DECL_BITS(GBRegisterSTAT, Mode, 0, 2);
+DECL_BIT(GBRegisterSTAT, LYC, 2);
+DECL_BIT(GBRegisterSTAT, HblankIRQ, 3);
+DECL_BIT(GBRegisterSTAT, VblankIRQ, 4);
+DECL_BIT(GBRegisterSTAT, OAMIRQ, 5);
+DECL_BIT(GBRegisterSTAT, LYCIRQ, 6);
 
 struct GBVideo {
 	struct GB* p;
 	struct GBVideoRenderer* renderer;
 
 	int ly;
+	GBRegisterSTAT stat;
+
 	int mode;
 
 	int32_t nextEvent;