From d8c3306bf233812bc7487c2e1958d15bdabc3539 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Sat, 29 Aug 2015 15:41:09 -0700
Subject: [PATCH] PSP2: Gyro and accel support

---
 src/platform/psp2/CMakeLists.txt |  2 +-
 src/platform/psp2/psp2-context.c | 40 ++++++++++++++++++++++++++++++++
 2 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/src/platform/psp2/CMakeLists.txt b/src/platform/psp2/CMakeLists.txt
index de698325f..a4cb16122 100644
--- a/src/platform/psp2/CMakeLists.txt
+++ b/src/platform/psp2/CMakeLists.txt
@@ -2,7 +2,7 @@ file(GLOB PLATFORM_SRC ${CMAKE_SOURCE_DIR}/src/platform/psp2/*.c)
 
 execute_process(COMMAND ${OBJCOPY} -I binary -O elf32-littlearm -B arm font.png ${CMAKE_BINARY_DIR}/font.o WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/res)
 
-set(PLATFORM_LIBRARY -lvita2d -lSceCtrl_stub -lSceRtc_stub -lSceGxm_stub -lSceDisplay_stub -lSceAudio_stub -lpng -lz -l${M_LIBRARY})
+set(PLATFORM_LIBRARY -lvita2d -lSceCtrl_stub -lSceRtc_stub -lSceGxm_stub -lSceDisplay_stub -lSceAudio_stub -lSceMotion_stub -lpng -lz -l${M_LIBRARY})
 
 add_executable(${BINARY_NAME}.elf ${PLATFORM_SRC} ${GUI_SRC} ${CMAKE_BINARY_DIR}/font.o)
 target_link_libraries(${BINARY_NAME}.elf ${BINARY_NAME} ${PLATFORM_LIBRARY})
diff --git a/src/platform/psp2/psp2-context.c b/src/platform/psp2/psp2-context.c
index 67be76f2f..cf7adc2fd 100644
--- a/src/platform/psp2/psp2-context.c
+++ b/src/platform/psp2/psp2-context.c
@@ -23,6 +23,7 @@
 #include <psp2/display.h>
 #include <psp2/gxm.h>
 #include <psp2/kernel/sysmem.h>
+#include <psp2/motion.h>
 
 #include <vita2d.h>
 
@@ -30,6 +31,10 @@ static struct GBAContext context;
 static struct GBAVideoSoftwareRenderer renderer;
 static vita2d_texture* tex;
 static Thread audioThread;
+static struct GBASceRotationSource {
+	struct GBARotationSource d;
+	struct SceMotionSensorState state;
+} rotation;
 
 static bool fullscreen = false;
 
@@ -74,6 +79,26 @@ static THREAD_ENTRY _audioThread(void* context) {
 	return 0;
 }
 
+static void _sampleRotation(struct GBARotationSource* source) {
+	struct GBASceRotationSource* rotation = (struct GBASceRotationSource*) source;
+	sceMotionGetSensorState(&rotation->state, 1);
+}
+
+static int32_t _readTiltX(struct GBARotationSource* source) {
+	struct GBASceRotationSource* rotation = (struct GBASceRotationSource*) source;
+	return rotation->state.accelerometer.x * 0x60000000;
+}
+
+static int32_t _readTiltY(struct GBARotationSource* source) {
+	struct GBASceRotationSource* rotation = (struct GBASceRotationSource*) source;
+	return rotation->state.accelerometer.y * 0x60000000;
+}
+
+static int32_t _readGyroZ(struct GBARotationSource* source) {
+	struct GBASceRotationSource* rotation = (struct GBASceRotationSource*) source;
+	return rotation->state.gyro.z * 0x10000000;
+}
+
 void GBAPSP2Setup() {
 	GBAContextInit(&context, 0);
 	struct GBAOptions opts = {
@@ -104,6 +129,13 @@ void GBAPSP2Setup() {
 	renderer.outputBuffer = vita2d_texture_get_datap(tex);
 	renderer.outputBufferStride = 256;
 	context.renderer = &renderer.d;
+
+	rotation.d.sample = _sampleRotation;
+	rotation.d.readTiltX = _readTiltX;
+	rotation.d.readTiltY = _readTiltY;
+	rotation.d.readGyroZ = _readGyroZ;
+	context.gba->rotationSource = &rotation.d;
+
 	printf("%s starting", projectName);
 }
 
@@ -121,6 +153,10 @@ bool GBAPSP2LoadROM(const char* path) {
 	blip_set_rates(context.gba->audio.left, GBA_ARM7TDMI_FREQUENCY, 48000 * ratio);
 	blip_set_rates(context.gba->audio.right, GBA_ARM7TDMI_FREQUENCY, 48000 * ratio);
 
+	if (context.gba->memory.hw.devices & (HW_TILT | HW_GYRO)) {
+		sceMotionStartSampling();
+	}
+
 	CircleBufferInit(&audioContext.buffer, PSP2_AUDIO_BUFFER_SIZE * sizeof(struct GBAStereoSample));
 	MutexInit(&audioContext.mutex);
 	ConditionInit(&audioContext.cond);
@@ -194,6 +230,10 @@ void GBAPSP2Runloop(void) {
 }
 
 void GBAPSP2UnloadROM(void) {
+	if (context.gba->memory.hw.devices & (HW_TILT | HW_GYRO)) {
+		sceMotionStopSampling();
+	}
+
 	GBAContextStop(&context);
 }