From eb9d377320d4d47e917e8c2bdbf2cadd24765872 Mon Sep 17 00:00:00 2001 From: yellows8 Date: Mon, 1 Jun 2020 12:07:38 -0400 Subject: [PATCH] hid: Implemented the remaining functionality for supporting SevenSixAxisSensor, and added the required structs. Updated hidInitializeSevenSixAxisSensor for the [10.0.0+] change. --- nx/include/switch/services/hid.h | 55 ++++++++++++++++++++++- nx/source/services/hid.c | 75 +++++++++++++++++++++++++++++++- 2 files changed, 127 insertions(+), 3 deletions(-) diff --git a/nx/include/switch/services/hid.h b/nx/include/switch/services/hid.h index 0c8beaa6..c85e13ca 100644 --- a/nx/include/switch/services/hid.h +++ b/nx/include/switch/services/hid.h @@ -670,6 +670,16 @@ typedef struct HidController { // End HidController +/// HidConsoleSixAxisSensor +typedef struct { + u64 timestamp; ///< Timestamp in samples + u8 is_at_rest; ///< IsSevenSixAxisSensorAtRest + u8 pad[0x3]; + float verticalization_error; ///< VerticalizationError + UtilFloat3 gyro_bias; ///< GyroBias + u8 pad2[0x4]; +} HidConsoleSixAxisSensor; + /// HidSharedMemory typedef struct HidSharedMemory { u8 header[0x400]; @@ -686,9 +696,40 @@ typedef struct HidSharedMemory { u8 unkSection8[0x800]; u8 controllerSerials[0x4000]; HidController controllers[10]; - u8 unkSection9[0x4600]; + u8 gesture[0x800]; + HidConsoleSixAxisSensor console_six_axis_sensor; ///< [5.0.0+] + u8 unk_x3C220[0x3DE0]; } HidSharedMemory; +typedef struct { + u64 unk_x0; + u64 unk_x8; + u64 latest_entry; + u64 total_entries; +} HidSevenSixAxisSensorStatesHeader; + +/// HidSevenSixAxisSensorState +typedef struct { + u64 timestamp0; + u64 timestamp1; + + u64 unk_x10; + float unk_x18[10]; +} HidSevenSixAxisSensorState; + +/// HidSevenSixAxisSensorStateEntry +typedef struct { + u64 timestamp; + u64 unused; + HidSevenSixAxisSensorState state; +} HidSevenSixAxisSensorStateEntry; + +/// HidSevenSixAxisSensorStates +typedef struct { + HidSevenSixAxisSensorStatesHeader header; + HidSevenSixAxisSensorStateEntry entries[0x21]; +} HidSevenSixAxisSensorStates; + /// HidVibrationDeviceInfo typedef struct HidVibrationDeviceInfo { u32 unk_x0; @@ -883,6 +924,18 @@ Result hidGetSevenSixAxisSensorFusionStrength(float *strength); /// Resets the timestamp for the SevenSixAxisSensor. Only available on [6.0.0+]. Result hidResetSevenSixAxisSensorTimestamp(void); +/// GetSevenSixAxisSensorStates. Only available when \ref hidInitializeSevenSixAxisSensor was previously used. +Result hidGetSevenSixAxisSensorStates(HidSevenSixAxisSensorState *states, size_t count, size_t *total_out); + +/// IsSevenSixAxisSensorAtRest. Only available when \ref hidInitializeSevenSixAxisSensor was previously used. +Result hidIsSevenSixAxisSensorAtRest(bool *out); + +/// GetSensorFusionError. Only available when \ref hidInitializeSevenSixAxisSensor was previously used. +Result hidGetSensorFusionError(float *out); + +/// GetGyroBias. Only available when \ref hidInitializeSevenSixAxisSensor was previously used. +Result hidGetGyroBias(UtilFloat3 *out); + /// Gets the \ref HidNpadInterfaceType for the specified controller. /// Only available on [4.0.0+]. Result hidGetNpadInterfaceType(HidControllerID id, u8 *out); diff --git a/nx/source/services/hid.c b/nx/source/services/hid.c index 9cba884a..59e68e37 100644 --- a/nx/source/services/hid.c +++ b/nx/source/services/hid.c @@ -3,6 +3,7 @@ #include #include #include +#include #include "kernel/shmem.h" #include "kernel/rwlock.h" #include "services/applet.h" @@ -1150,8 +1151,10 @@ Result hidInitializeSevenSixAxisSensor(void) { if (g_sevenSixAxisSensorBuffer != NULL) return MAKERESULT(Module_Libnx, LibnxError_AlreadyInitialized); - rc = _hidActivateConsoleSixAxisSensor(); - if (R_FAILED(rc)) return rc; + if (hosversionBefore(10,0,0)) { // No longer used by sdknso on [10.0.0+]. + rc = _hidActivateConsoleSixAxisSensor(); + if (R_FAILED(rc)) return rc; + } g_sevenSixAxisSensorBuffer = (u8*)memalign(0x1000, bufsize); if (g_sevenSixAxisSensorBuffer == NULL) @@ -1225,6 +1228,74 @@ Result hidResetSevenSixAxisSensorTimestamp(void) { return _hidCmdWithNoInput(310); } +Result hidGetSevenSixAxisSensorStates(HidSevenSixAxisSensorState *states, size_t count, size_t *total_out) { + if (g_sevenSixAxisSensorBuffer == NULL) + return MAKERESULT(Module_Libnx, LibnxError_NotInitialized); + + if (states == NULL) + return MAKERESULT(Module_Libnx, LibnxError_BadInput); + + HidSevenSixAxisSensorStates *states_buf = (HidSevenSixAxisSensorStates*)g_sevenSixAxisSensorBuffer; + + s32 total_entries = (s32)atomic_load_explicit(&states_buf->header.total_entries, memory_order_acquire); + if (total_entries < 0) total_entries = 0; + if (total_entries > count) total_entries = count; + s32 latest_entry = (s32)atomic_load_explicit(&states_buf->header.latest_entry, memory_order_acquire); + + for (s32 i=0; ientries[entrypos].timestamp, memory_order_acquire); + memcpy(&states[total_entries-i-1], &states_buf->entries[entrypos].state, sizeof(HidSevenSixAxisSensorState)); + timestamp1 = atomic_load_explicit(&states_buf->entries[entrypos].timestamp, memory_order_acquire); + + if (timestamp0 != timestamp1 || (i>0 && states[total_entries-i-1].timestamp1 - states[total_entries-i].timestamp1 != 1)) { + s32 tmpcount = (s32)atomic_load_explicit(&states_buf->header.total_entries, memory_order_acquire); + tmpcount = total_entries < tmpcount ? tmpcount : total_entries; + total_entries = tmpcount < count ? tmpcount : count; + latest_entry = (s32)atomic_load_explicit(&states_buf->header.latest_entry, memory_order_acquire); + + i=-1; + } + } + + *total_out = total_entries; + + return 0; +} + +Result hidIsSevenSixAxisSensorAtRest(bool *out) { + if (g_sevenSixAxisSensorBuffer == NULL) + return MAKERESULT(Module_Libnx, LibnxError_NotInitialized); + + HidSharedMemory *shared_mem = (HidSharedMemory*)hidGetSharedmemAddr(); + *out = shared_mem->console_six_axis_sensor.is_at_rest & 1; + + return 0; +} + +Result hidGetSensorFusionError(float *out) { + if (g_sevenSixAxisSensorBuffer == NULL) + return MAKERESULT(Module_Libnx, LibnxError_NotInitialized); + + HidSharedMemory *shared_mem = (HidSharedMemory*)hidGetSharedmemAddr(); + *out = shared_mem->console_six_axis_sensor.verticalization_error; + + return 0; +} + +Result hidGetGyroBias(UtilFloat3 *out) { + if (g_sevenSixAxisSensorBuffer == NULL) + return MAKERESULT(Module_Libnx, LibnxError_NotInitialized); + + HidSharedMemory *shared_mem = (HidSharedMemory*)hidGetSharedmemAddr(); + *out = shared_mem->console_six_axis_sensor.gyro_bias; + + return 0; +} + Result hidGetNpadInterfaceType(HidControllerID id, u8 *out) { if (hosversionBefore(4,0,0)) return MAKERESULT(Module_Libnx, LibnxError_IncompatSysVer);