From c141063101955a837f0dc26884604443d8035d1d Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Wed, 31 Dec 2014 14:43:02 -0800
Subject: [PATCH] GBA: Implement tilt sensor

---
 src/gba/gba-gpio.c   | 54 ++++++++++++++++++++++++++++++++++++++++++++
 src/gba/gba-gpio.h   |  7 ++++++
 src/gba/gba-memory.c |  5 ++--
 3 files changed, 63 insertions(+), 3 deletions(-)

diff --git a/src/gba/gba-gpio.c b/src/gba/gba-gpio.c
index 3108f0a92..eaa262c44 100644
--- a/src/gba/gba-gpio.c
+++ b/src/gba/gba-gpio.c
@@ -364,6 +364,60 @@ void _lightReadPins(struct GBACartridgeGPIO* gpio) {
 
 void GBAGPIOInitTilt(struct GBACartridgeGPIO* gpio) {
 	gpio->gpioDevices |= GPIO_TILT;
+	gpio->tiltX = 0xFFF;
+	gpio->tiltY = 0xFFF;
+	gpio->tiltState = 0;
+}
+
+void GBAGPIOTiltWrite(struct GBACartridgeGPIO* gpio, uint32_t address, uint8_t value) {
+	switch (address) {
+	case 0x8000:
+		if (value == 0x55) {
+			gpio->tiltState = 1;
+		} else {
+			GBALog(0, GBA_LOG_GAME_ERROR, "Tilt sensor wrote wrong byte to %04x: %02x", address, value);
+		}
+		break;
+	case 0x8100:
+		if (value == 0xAA && gpio->tiltState == 1) {
+			gpio->tiltState = 0;
+			struct GBARotationSource* rotationSource = gpio->p->rotationSource;
+			if (!rotationSource || !rotationSource->readTiltX || !rotationSource->readTiltY) {
+				return;
+			}
+			if (rotationSource->sample) {
+				rotationSource->sample(rotationSource);
+			}
+			int32_t x = rotationSource->readTiltX(rotationSource);
+			int32_t y = rotationSource->readTiltY(rotationSource);
+			// Normalize to ~12 bits, focused on 0x3A0
+			gpio->tiltX = (x >> 21) + 0x3A0; // Crop off an extra bit so that we can't go negative
+			gpio->tiltY = (y >> 21) + 0x3A0;
+		} else {
+			GBALog(0, GBA_LOG_GAME_ERROR, "Tilt sensor wrote wrong byte to %04x: %02x", address, value);
+		}
+		break;
+	default:
+		GBALog(0, GBA_LOG_GAME_ERROR, "Invalid tilt sensor write to %04x: %02x", address, value);
+		break;
+	}
+}
+
+uint8_t GBAGPIOTiltRead(struct GBACartridgeGPIO* gpio, uint32_t address) {
+	switch (address) {
+	case 0x8200:
+		return gpio->tiltX & 0xFF;
+	case 0x8300:
+		return ((gpio->tiltX >> 8) & 0xF) | 0x80;
+	case 0x8400:
+		return gpio->tiltY & 0xFF;
+	case 0x8500:
+		return (gpio->tiltY >> 8) & 0xF;
+	default:
+		GBALog(0, GBA_LOG_GAME_ERROR, "Invalid tilt sensor read from %04x", address);
+		break;
+	}
+	return 0xFF;
 }
 
 // == Serialization
diff --git a/src/gba/gba-gpio.h b/src/gba/gba-gpio.h
index e6e3d2ebb..24716320a 100644
--- a/src/gba/gba-gpio.h
+++ b/src/gba/gba-gpio.h
@@ -107,6 +107,10 @@ struct GBACartridgeGPIO {
 	unsigned lightCounter : 12;
 	uint8_t lightSample;
 	bool lightEdge;
+
+	uint16_t tiltX;
+	uint16_t tiltY;
+	int tiltState;
 };
 
 void GBAGPIOInit(struct GBACartridgeGPIO* gpio, uint16_t* gpioBase);
@@ -118,6 +122,9 @@ void GBAGPIOInitRumble(struct GBACartridgeGPIO* gpio);
 void GBAGPIOInitLightSensor(struct GBACartridgeGPIO* gpio);
 void GBAGPIOInitTilt(struct GBACartridgeGPIO* gpio);
 
+void GBAGPIOTiltWrite(struct GBACartridgeGPIO* gpio, uint32_t address, uint8_t value);
+uint8_t GBAGPIOTiltRead(struct GBACartridgeGPIO* gpio, uint32_t address);
+
 struct GBASerializedState;
 void GBAGPIOSerialize(struct GBACartridgeGPIO* gpio, struct GBASerializedState* state);
 void GBAGPIODeserialize(struct GBACartridgeGPIO* gpio, struct GBASerializedState* state);
diff --git a/src/gba/gba-memory.c b/src/gba/gba-memory.c
index 718ea2ea8..7f7568398 100644
--- a/src/gba/gba-memory.c
+++ b/src/gba/gba-memory.c
@@ -449,8 +449,7 @@ int8_t GBALoad8(struct ARMCore* cpu, uint32_t address, int* cycleCounter) {
 		} else if (memory->savedata.type == SAVEDATA_FLASH512 || memory->savedata.type == SAVEDATA_FLASH1M) {
 			value = GBASavedataReadFlash(&memory->savedata, address);
 		} else if (memory->gpio.gpioDevices & GPIO_TILT) {
-			GBALog(gba, GBA_LOG_STUB, "Unimplemented tilt sensor read: 0x%08X", address);
-			value = 0xFF;
+			value = GBAGPIOTiltRead(&memory->gpio, address & OFFSET_MASK);
 		} else {
 			GBALog(gba, GBA_LOG_GAME_ERROR, "Reading from non-existent SRAM: 0x%08X", address);
 			value = 0xFF;
@@ -667,7 +666,7 @@ void GBAStore8(struct ARMCore* cpu, uint32_t address, int8_t value, int* cycleCo
 		} else if (memory->savedata.type == SAVEDATA_SRAM) {
 			memory->savedata.data[address & (SIZE_CART_SRAM - 1)] = value;
 		} else if (memory->gpio.gpioDevices & GPIO_TILT) {
-			GBALog(gba, GBA_LOG_STUB, "Unimplemented tilt sensor write: 0x%08X", address);
+			GBAGPIOTiltWrite(&memory->gpio, address & OFFSET_MASK, value);
 		} else {
 			GBALog(gba, GBA_LOG_GAME_ERROR, "Writing to non-existent SRAM: 0x%08X", address);
 		}