From 5c3fc0ac0dd73d53cd33e1598d1847e0f8d6418c Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Thu, 23 Feb 2017 16:36:59 -0800
Subject: [PATCH] DS SPI: Implement firmware access

---
 include/mgba/internal/ds/spi.h |  8 +++-
 src/ds/spi.c                   | 71 ++++++++++++++++++++++++++++++----
 2 files changed, 69 insertions(+), 10 deletions(-)

diff --git a/include/mgba/internal/ds/spi.h b/include/mgba/internal/ds/spi.h
index 26f82e95b..f166b8ac0 100644
--- a/include/mgba/internal/ds/spi.h
+++ b/include/mgba/internal/ds/spi.h
@@ -49,14 +49,18 @@ enum {
 struct DSSPIBus {
 	bool holdEnabled;
 
-	uint8_t firmwareMode;
+	uint8_t firmCommand;
+	uint8_t firmStatusReg;
+	int firmAddressingRemaining;
+	uint32_t firmAddress;
 
-	struct mTimingEvent tscEvent;
 	uint8_t tscControlByte;
 	uint16_t tscRegister;
 	int tscOffset;
 
 	uint8_t powmgrByte;
+
+	struct mTimingEvent event;
 };
 
 struct DS;
diff --git a/src/ds/spi.c b/src/ds/spi.c
index 06b3814ed..d6c16d179 100644
--- a/src/ds/spi.c
+++ b/src/ds/spi.c
@@ -6,23 +6,26 @@
 #include <mgba/internal/ds/spi.h>
 
 #include <mgba/internal/ds/ds.h>
+#include <mgba-util/vfs.h>
 
 mLOG_DEFINE_CATEGORY(DS_SPI, "DS SPI");
 
 static void _tscEvent(struct mTiming*, void* context, uint32_t cyclesLate);
+static void _firmwareEvent(struct mTiming* timing, void* context, uint32_t cyclesLate);
 
 void DSSPIReset(struct DS* ds) {
 	memset(&ds->memory.spiBus, 0, sizeof(ds->memory.spiBus));
-	ds->memory.spiBus.tscEvent.name = "DS SPI TSC";
-	ds->memory.spiBus.tscEvent.context = ds;
-	ds->memory.spiBus.tscEvent.callback = _tscEvent;
-	ds->memory.spiBus.tscEvent.priority = 0x60;
+	ds->memory.spiBus.event.name = "DS SPI Event";
+	ds->memory.spiBus.event.context = ds;
+	ds->memory.spiBus.event.callback = _tscEvent;
+	ds->memory.spiBus.event.priority = 0x60;
 }
 
 DSSPICNT DSSPIWriteControl(struct DS* ds, uint16_t control) {
 	// TODO
 	if (!ds->memory.spiBus.holdEnabled) {
 		ds->memory.spiBus.tscControlByte = 0;
+		ds->memory.spiBus.firmCommand = 0;
 	}
 	ds->memory.spiBus.holdEnabled = DSSPICNTIsCSHold(control);
 	return control;
@@ -37,16 +40,19 @@ void DSSPIWrite(struct DS* ds, uint8_t datum) {
 	baud = DS_ARM7TDMI_FREQUENCY >> baud;
 	switch (DSSPICNTGetChipSelect(control)) {
 	case DS_SPI_DEV_TSC:
-		control = DSSPICNTFillBusy(control);
-		mTimingDeschedule(&ds->ds7.timing, &ds->memory.spiBus.tscEvent);
-		mTimingSchedule(&ds->ds7.timing, &ds->memory.spiBus.tscEvent, baud);
+		ds->memory.spiBus.event.callback = _tscEvent;
+		break;
+	case DS_SPI_DEV_FIRMWARE:
+		ds->memory.spiBus.event.callback = _firmwareEvent;
 		break;
 	case DS_SPI_DEV_POWERMAN:
-	case DS_SPI_DEV_FIRMWARE:
 	default:
 		mLOG(DS_SPI, STUB, "Unimplemented data write: %04X:%02X", control, datum);
 		break;
 	}
+	control = DSSPICNTFillBusy(control);
+	mTimingDeschedule(&ds->ds7.timing, &ds->memory.spiBus.event);
+	mTimingSchedule(&ds->ds7.timing, &ds->memory.spiBus.event, baud);
 	ds->memory.io7[DS7_REG_SPICNT >> 1] = control;
 }
 
@@ -102,3 +108,52 @@ static void _tscEvent(struct mTiming* timing, void* context, uint32_t cyclesLate
 		DSRaiseIRQ(ds->ds7.cpu, ds->ds7.memory.io, DS_IRQ_SPI);
 	}
 }
+
+static void _firmwareEvent(struct mTiming* timing, void* context, uint32_t cyclesLate) {
+	UNUSED(timing);
+	UNUSED(cyclesLate);
+	struct DS* ds = context;
+	uint8_t oldValue = ds->memory.io7[DS7_REG_SPIDATA >> 1];
+	DSSPICNT control = ds->memory.io7[DS7_REG_SPICNT >> 1];
+	uint8_t newValue = 0;
+
+	if (!ds->memory.spiBus.firmCommand) {
+		ds->memory.spiBus.firmCommand = oldValue;
+		ds->memory.spiBus.firmAddress = 0;
+		ds->memory.spiBus.firmAddressingRemaining = 24;
+	} else if (ds->memory.spiBus.firmAddressingRemaining) {
+		ds->memory.spiBus.firmAddress <<= 8;
+		ds->memory.spiBus.firmAddress |= oldValue;
+		ds->memory.spiBus.firmAddressingRemaining -= 8;
+		ds->firmwareVf->seek(ds->firmwareVf, ds->memory.spiBus.firmAddress, SEEK_SET);
+	} else {
+		switch (ds->memory.spiBus.firmCommand) {
+		case 0x02: // WR
+			ds->firmwareVf->write(ds->firmwareVf, &oldValue, 1);
+			++ds->memory.spiBus.firmAddress;
+			break;
+		case 0x03: // RD
+			ds->firmwareVf->read(ds->firmwareVf, &newValue, 1);
+			++ds->memory.spiBus.firmAddress;
+		case 0x04: // WRDI
+			ds->memory.spiBus.firmStatusReg &= ~2;
+			break;
+		case 0x05: // RDSR
+			newValue = ds->memory.spiBus.firmStatusReg;
+			break;
+		case 0x06: // WREN
+			ds->memory.spiBus.firmStatusReg |= 2;
+			break;
+		default:
+			mLOG(DS_SPI, STUB, "Unimplemented Firmware write: %04X:%02X:%02X", control, ds->memory.spiBus.firmCommand, newValue);
+			break;
+		}
+	}
+
+	control = DSSPICNTClearBusy(control);
+	ds->memory.io7[DS7_REG_SPIDATA >> 1] = newValue;
+	ds->memory.io7[DS7_REG_SPICNT >> 1] = control;
+	if (DSSPICNTIsDoIRQ(control)) {
+		DSRaiseIRQ(ds->ds7.cpu, ds->ds7.memory.io, DS_IRQ_SPI);
+	}
+}