From 064f821cf0da0fa09d825405ead6bb70db2a4056 Mon Sep 17 00:00:00 2001
From: Rinnegatamante <rinnegatamante@gmail.com>
Date: Thu, 31 Oct 2019 09:33:16 +0100
Subject: [PATCH] [VITA] Added accelerometer and gyroscope support.

---
 Makefile.vita             |  2 +-
 Makefile.vita.salamander  |  2 +-
 input/drivers/psp_input.c | 86 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 88 insertions(+), 2 deletions(-)

diff --git a/Makefile.vita b/Makefile.vita
index 9727a869ce..58f7c75b12 100644
--- a/Makefile.vita
+++ b/Makefile.vita
@@ -114,7 +114,7 @@ CXXFLAGS := $(CFLAGS) -fno-rtti -fno-exceptions
 
 VITA_LIBS := -lSceDisplay_stub -lSceGxm_stub -lSceNet_stub -lSceNetCtl_stub -lSceAppUtil_stub \
 	-lSceSysmodule_stub -lSceCtrl_stub -lSceHid_stub -lSceTouch_stub -lSceAudio_stub \
-	-lScePower_stub -lSceRtc_stub -lSceCommonDialog_stub -lScePgf_stub \
+	-lScePower_stub -lSceRtc_stub -lSceCommonDialog_stub -lScePgf_stub -lSceMotion_stub \
 	-lSceFiber_stub -lSceMotion_stub -lSceAppMgr_stub -lpthread -lpng -lz
 
 LIBS	:= $(WHOLE_START) -lretro_vita $(WHOLE_END) $(VITA_LIBS) -lm -lc
diff --git a/Makefile.vita.salamander b/Makefile.vita.salamander
index eaee618249..165e03f42e 100644
--- a/Makefile.vita.salamander
+++ b/Makefile.vita.salamander
@@ -23,7 +23,7 @@ LIBDIR =
 LDFLAGS =
 LIBS = -lSceDisplay_stub -lSceGxm_stub -lSceNet_stub -lSceNetCtl_stub -lSceAppUtil_stub\
      -lSceSysmodule_stub -lSceCtrl_stub -lSceHid_stub -lSceAudio_stub -lSceFiber_stub\
-     -lScePower_stub -lSceRtc_stub -lSceCommonDialog_stub -lScePgf_stub \
+     -lScePower_stub -lSceRtc_stub -lSceCommonDialog_stub -lScePgf_stub -lSceMotion_stub \
      -lSceMotion_stub -lSceAppMgr_stub -lfreetype -lpng -lm -lc
 
 ifeq ($(HAVE_FILE_LOGGER), 1)
diff --git a/input/drivers/psp_input.c b/input/drivers/psp_input.c
index db0000c46a..8f9958f565 100644
--- a/input/drivers/psp_input.c
+++ b/input/drivers/psp_input.c
@@ -29,6 +29,7 @@
 #include <psp2/ctrl.h>
 #include <psp2/kernel/processmgr.h>
 #include <psp2/hid.h>
+#include <psp2/motion.h>
 #define VITA_NUM_SCANCODES 115 /* size of rarch_key_map_vita */
 #define VITA_MAX_SCANCODE 0xE7
 #define VITA_NUM_MODIFIERS 11 /* number of modifiers reported */
@@ -86,6 +87,8 @@ typedef struct psp_input
    bool mouse_button_left;
    bool mouse_button_right;
    bool mouse_button_middle;
+   
+   bool sensors_enabled;
 #endif
 } psp_input_t;
 
@@ -445,13 +448,96 @@ static bool psp_input_set_rumble(void *data, unsigned port,
    return false;
 }
 
+#ifdef VITA
+static bool psp_input_set_sensor_state(void *data, unsigned port,
+      enum retro_sensor_action action, unsigned event_rate)
+{
+   psp_input_t *psp = (psp_input_t*)data;
+	
+   if(!psp)
+      return false;
+  
+   switch(action)
+   {
+      case RETRO_SENSOR_ILLUMINANCE_ENABLE:
+         return false;
+
+      case RETRO_SENSOR_ILLUMINANCE_DISABLE:
+         return true;
+		 
+      case RETRO_SENSOR_ACCELEROMETER_DISABLE:
+      case RETRO_SENSOR_GYROSCOPE_DISABLE:
+         if(psp->sensors_enabled)
+         {
+           psp->sensors_enabled = false;
+           sceMotionMagnetometerOff();
+           sceMotionStopSampling();
+         }
+         return true;
+
+      case RETRO_SENSOR_ACCELEROMETER_ENABLE:
+      case RETRO_SENSOR_GYROSCOPE_ENABLE:
+         if(!psp->sensors_enabled)
+         {
+           psp->sensors_enabled = true;
+           sceMotionStartSampling();
+           sceMotionMagnetometerOn();
+         }
+         return true;
+   }
+   
+   return false;
+}
+
+static float psp_input_get_sensor_input(void *data,
+      unsigned port, unsigned id)
+{
+   SceMotionSensorState sixaxis;
+   
+   psp_input_t *psp = (psp_input_t*)data;
+	
+   if(!psp || !psp->sensors_enabled)
+      return false;
+
+   if(id >= RETRO_SENSOR_ACCELEROMETER_X && id <= RETRO_SENSOR_GYROSCOPE_Z)
+   {
+      sceMotionGetSensorState(&sixaxis, port);
+
+      switch(id)
+      {
+         case RETRO_SENSOR_ACCELEROMETER_X:
+            return sixaxis.accelerometer.x;
+         case RETRO_SENSOR_ACCELEROMETER_Y:
+            return sixaxis.accelerometer.y;
+         case RETRO_SENSOR_ACCELEROMETER_Z:
+            return sixaxis.accelerometer.z;
+         case RETRO_SENSOR_GYROSCOPE_X:
+            return sixaxis.gyro.x;
+         case RETRO_SENSOR_GYROSCOPE_Y:
+            return sixaxis.gyro.y;
+         case RETRO_SENSOR_GYROSCOPE_Z:
+            return sixaxis.gyro.z;
+      }
+
+   }
+
+   return 0.0f;
+}
+	
+#endif
+
 input_driver_t input_psp = {
    psp_input_initialize,
    psp_input_poll,
    psp_input_state,
    psp_input_free_input,
+#ifdef VITA
+   psp_input_set_sensor_state,
+   psp_input_get_sensor_input,
+#else
    NULL,
    NULL,
+#endif
    psp_input_get_capabilities,
 #ifdef VITA
    "vita",