SDL: hidapi: switch: Add user calibration support

From 9fa55d9cabc93532cd5cf44105ec589a4db1d097 Mon Sep 17 00:00:00 2001
From: Narr the Reg <[EMAIL REDACTED]>
Date: Mon, 9 Jan 2023 17:42:17 -0600
Subject: [PATCH] hidapi: switch: Add user calibration support

---
 src/joystick/hidapi/SDL_hidapi_switch.c | 89 ++++++++++++++++++-------
 1 file changed, 66 insertions(+), 23 deletions(-)

diff --git a/src/joystick/hidapi/SDL_hidapi_switch.c b/src/joystick/hidapi/SDL_hidapi_switch.c
index 97bbe58d8472..e3ba8863c007 100644
--- a/src/joystick/hidapi/SDL_hidapi_switch.c
+++ b/src/joystick/hidapi/SDL_hidapi_switch.c
@@ -107,9 +107,13 @@ typedef enum
 #define k_unSwitchBluetoothPacketLength  k_unSwitchOutputPacketDataLength
 #define k_unSwitchUSBPacketLength        k_unSwitchMaxOutputPacketLength
 
-#define k_unSPIStickCalibrationStartOffset 0x603D
-#define k_unSPIStickCalibrationEndOffset   0x604E
-#define k_unSPIStickCalibrationLength      (k_unSPIStickCalibrationEndOffset - k_unSPIStickCalibrationStartOffset + 1)
+#define k_unSPIStickFactoryCalibrationStartOffset 0x603D
+#define k_unSPIStickFactoryCalibrationEndOffset   0x604E
+#define k_unSPIStickFactoryCalibrationLength      (k_unSPIStickFactoryCalibrationEndOffset - k_unSPIStickFactoryCalibrationStartOffset + 1)
+
+#define k_unSPIStickUserCalibrationStartOffset 0x8010
+#define k_unSPIStickUserCalibrationEndOffset   0x8025
+#define k_unSPIStickUserCalibrationLength      (k_unSPIStickUserCalibrationEndOffset - k_unSPIStickUserCalibrationStartOffset + 1)
 
 #define k_unSPIIMUScaleStartOffset 0x6020
 #define k_unSPIIMUScaleEndOffset   0x6037
@@ -195,6 +199,22 @@ typedef struct
             Uint8 ucFiller2;
             Uint8 ucColorLocation;
         } deviceInfo;
+
+        struct
+        {
+            SwitchSPIOpData_t opData;
+            Uint8 rgucLeftCalibration[9];
+            Uint8 rgucRightCalibration[9];
+        } stickFactoryCalibration;
+
+        struct
+        {
+            SwitchSPIOpData_t opData;
+            Uint8 rgucLeftMagic[2];
+            Uint8 rgucLeftCalibration[9];
+            Uint8 rgucRightMagic[2];
+            Uint8 rgucRightCalibration[9];
+        } stickUserCalibration;
     };
 } SwitchSubcommandInputPacket_t;
 
@@ -702,41 +722,64 @@ static SDL_bool SetIMUEnabled(SDL_DriverSwitch_Context *ctx, SDL_bool enabled)
 
 static SDL_bool LoadStickCalibration(SDL_DriverSwitch_Context *ctx, Uint8 input_mode)
 {
-    Uint8 *pStickCal;
+    Uint8 *pLeftStickCal;
+    Uint8 *pRightStickCal;
     size_t stick, axis;
-    SwitchSubcommandInputPacket_t *reply = NULL;
+    SwitchSubcommandInputPacket_t *user_reply = NULL;
+    SwitchSubcommandInputPacket_t *factory_reply = NULL;
+    SwitchSPIOpData_t readUserParams;
+    SwitchSPIOpData_t readFactoryParams;
 
-    /* Read Calibration Info */
-    SwitchSPIOpData_t readParams;
-    readParams.unAddress = k_unSPIStickCalibrationStartOffset;
-    readParams.ucLength = k_unSPIStickCalibrationLength;
+    /* Read User Calibration Info */
+    readUserParams.unAddress = k_unSPIStickUserCalibrationStartOffset;
+    readUserParams.ucLength = k_unSPIStickUserCalibrationLength;
 
-    if (!WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) {
+    if (!WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readUserParams, sizeof(readUserParams), &user_reply)) {
+        return SDL_FALSE;
+    }
+
+    /* Read Factory Calibration Info */
+    readFactoryParams.unAddress = k_unSPIStickFactoryCalibrationStartOffset;
+    readFactoryParams.ucLength = k_unSPIStickFactoryCalibrationLength;
+
+    if (!WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readFactoryParams, sizeof(readFactoryParams), &factory_reply)) {
         return SDL_FALSE;
     }
 
+    /* Automatically select the user calibration if magic bytes are set */
+    if (user_reply->stickUserCalibration.rgucLeftMagic[0] == 0xB2 && user_reply->stickUserCalibration.rgucLeftMagic[1] == 0xA1) {
+        pLeftStickCal = user_reply->stickUserCalibration.rgucLeftCalibration;
+    } else {
+        pLeftStickCal = factory_reply->stickFactoryCalibration.rgucLeftCalibration;
+    }
+
+    if (user_reply->stickUserCalibration.rgucRightMagic[0] == 0xB2 && user_reply->stickUserCalibration.rgucRightMagic[1] == 0xA1) {
+        pRightStickCal = user_reply->stickUserCalibration.rgucRightCalibration;
+    } else {
+        pRightStickCal = factory_reply->stickFactoryCalibration.rgucRightCalibration;
+    }
+
     /* Stick calibration values are 12-bits each and are packed by bit
      * For whatever reason the fields are in a different order for each stick
      * Left:  X-Max, Y-Max, X-Center, Y-Center, X-Min, Y-Min
      * Right: X-Center, Y-Center, X-Min, Y-Min, X-Max, Y-Max
      */
-    pStickCal = reply->spiReadData.rgucReadData;
 
     /* Left stick */
-    ctx->m_StickCalData[0].axis[0].sMax = ((pStickCal[1] << 8) & 0xF00) | pStickCal[0];    /* X Axis max above center */
-    ctx->m_StickCalData[0].axis[1].sMax = (pStickCal[2] << 4) | (pStickCal[1] >> 4);       /* Y Axis max above center */
-    ctx->m_StickCalData[0].axis[0].sCenter = ((pStickCal[4] << 8) & 0xF00) | pStickCal[3]; /* X Axis center */
-    ctx->m_StickCalData[0].axis[1].sCenter = (pStickCal[5] << 4) | (pStickCal[4] >> 4);    /* Y Axis center */
-    ctx->m_StickCalData[0].axis[0].sMin = ((pStickCal[7] << 8) & 0xF00) | pStickCal[6];    /* X Axis min below center */
-    ctx->m_StickCalData[0].axis[1].sMin = (pStickCal[8] << 4) | (pStickCal[7] >> 4);       /* Y Axis min below center */
+    ctx->m_StickCalData[0].axis[0].sMax = ((pLeftStickCal[1] << 8) & 0xF00) | pLeftStickCal[0];    /* X Axis max above center */
+    ctx->m_StickCalData[0].axis[1].sMax = (pLeftStickCal[2] << 4) | (pLeftStickCal[1] >> 4);       /* Y Axis max above center */
+    ctx->m_StickCalData[0].axis[0].sCenter = ((pLeftStickCal[4] << 8) & 0xF00) | pLeftStickCal[3]; /* X Axis center */
+    ctx->m_StickCalData[0].axis[1].sCenter = (pLeftStickCal[5] << 4) | (pLeftStickCal[4] >> 4);    /* Y Axis center */
+    ctx->m_StickCalData[0].axis[0].sMin = ((pLeftStickCal[7] << 8) & 0xF00) | pLeftStickCal[6];    /* X Axis min below center */
+    ctx->m_StickCalData[0].axis[1].sMin = (pLeftStickCal[8] << 4) | (pLeftStickCal[7] >> 4);       /* Y Axis min below center */
 
     /* Right stick */
-    ctx->m_StickCalData[1].axis[0].sCenter = ((pStickCal[10] << 8) & 0xF00) | pStickCal[9]; /* X Axis center */
-    ctx->m_StickCalData[1].axis[1].sCenter = (pStickCal[11] << 4) | (pStickCal[10] >> 4);   /* Y Axis center */
-    ctx->m_StickCalData[1].axis[0].sMin = ((pStickCal[13] << 8) & 0xF00) | pStickCal[12];   /* X Axis min below center */
-    ctx->m_StickCalData[1].axis[1].sMin = (pStickCal[14] << 4) | (pStickCal[13] >> 4);      /* Y Axis min below center */
-    ctx->m_StickCalData[1].axis[0].sMax = ((pStickCal[16] << 8) & 0xF00) | pStickCal[15];   /* X Axis max above center */
-    ctx->m_StickCalData[1].axis[1].sMax = (pStickCal[17] << 4) | (pStickCal[16] >> 4);      /* Y Axis max above center */
+    ctx->m_StickCalData[1].axis[0].sCenter = ((pRightStickCal[1] << 8) & 0xF00) | pRightStickCal[0]; /* X Axis center */
+    ctx->m_StickCalData[1].axis[1].sCenter = (pRightStickCal[2] << 4) | (pRightStickCal[1] >> 4);    /* Y Axis center */
+    ctx->m_StickCalData[1].axis[0].sMin = ((pRightStickCal[4] << 8) & 0xF00) | pRightStickCal[3];    /* X Axis min below center */
+    ctx->m_StickCalData[1].axis[1].sMin = (pRightStickCal[5] << 4) | (pRightStickCal[4] >> 4);       /* Y Axis min below center */
+    ctx->m_StickCalData[1].axis[0].sMax = ((pRightStickCal[7] << 8) & 0xF00) | pRightStickCal[6];    /* X Axis max above center */
+    ctx->m_StickCalData[1].axis[1].sMax = (pRightStickCal[8] << 4) | (pRightStickCal[7] >> 4);       /* Y Axis max above center */
 
     /* Filter out any values that were uninitialized (0xFFF) in the SPI read */
     for (stick = 0; stick < 2; ++stick) {