SDL: Add support for correlating trigger input

From a23b3c767ffba86785a27a68f85f0af9c73bb11e Mon Sep 17 00:00:00 2001
From: Carl Glave <[EMAIL REDACTED]>
Date: Thu, 20 Jan 2022 17:21:28 -0800
Subject: [PATCH] Add support for correlating trigger input

---
 src/joystick/windows/SDL_rawinputjoystick.c | 108 ++++++++++++++------
 1 file changed, 77 insertions(+), 31 deletions(-)

diff --git a/src/joystick/windows/SDL_rawinputjoystick.c b/src/joystick/windows/SDL_rawinputjoystick.c
index 8178ee624ca..f12661732ad 100644
--- a/src/joystick/windows/SDL_rawinputjoystick.c
+++ b/src/joystick/windows/SDL_rawinputjoystick.c
@@ -67,6 +67,12 @@ typedef struct WindowsGamingInputGamepadState WindowsGamingInputGamepadState;
 #if defined(SDL_JOYSTICK_RAWINPUT_XINPUT) || defined(SDL_JOYSTICK_RAWINPUT_WGI)
 #define SDL_JOYSTICK_RAWINPUT_MATCHING
 #define SDL_JOYSTICK_RAWINPUT_MATCH_AXES
+#define SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+#ifdef SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+#define SDL_JOYSTICK_RAWINPUT_MATCH_COUNT 6 // stick + trigger axes
+#else
+#define SDL_JOYSTICK_RAWINPUT_MATCH_COUNT 4 // stick axes
+#endif
 #endif
 
 /*#define DEBUG_RAWINPUT*/
@@ -128,7 +134,7 @@ struct joystick_hwdata
     USHORT trigger_hack_index;
 
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCHING
-    Uint32 match_state; /* Low 16 bits for button states, high 16 for 4 4bit axes */
+    Uint64 match_state; /* Lowest 16 bits for button states, higher 24 for 6 4bit axes */
     Uint32 last_state_packet;
 #endif
 
@@ -173,7 +179,7 @@ static struct {
 
 typedef struct WindowsMatchState {
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCH_AXES
-    SHORT match_axes[4];
+    SHORT match_axes[SDL_JOYSTICK_RAWINPUT_MATCH_COUNT];
 #endif
 #ifdef SDL_JOYSTICK_RAWINPUT_XINPUT
     WORD xinput_buttons;
@@ -184,13 +190,13 @@ typedef struct WindowsMatchState {
     SDL_bool any_data;
 } WindowsMatchState;
 
-static void RAWINPUT_FillMatchState(WindowsMatchState *state, Uint32 match_state)
+static void RAWINPUT_FillMatchState(WindowsMatchState *state, Uint64 match_state)
 {
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCH_AXES
     int ii;
 #endif
 
-    state->any_data = SDL_FALSE;
+    SDL_bool any_axes_data = SDL_FALSE;
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCH_AXES
     /*  SHORT state->match_axes[4] = {
             (match_state & 0x000F0000) >> 4,
@@ -199,12 +205,18 @@ static void RAWINPUT_FillMatchState(WindowsMatchState *state, Uint32 match_state
             (match_state & 0xF0000000) >> 16,
         }; */
     for (ii = 0; ii < 4; ii++) {
-        state->match_axes[ii] = (match_state & (0x000F0000 << (ii * 4))) >> (4 + ii * 4);
-        if ((Uint32)(state->match_axes[ii] + 0x1000) > 0x2000) { /* match_state bit is not 0xF, 0x1, or 0x2 */
-            state->any_data = SDL_TRUE;
-        }
+        state->match_axes[ii] = (SHORT)((match_state & (0x000F0000ull << (ii * 4))) >> (4 + ii * 4));
+        any_axes_data |= ((Uint32)(state->match_axes[ii] + 0x1000) > 0x2000); /* match_state bit is not 0xF, 0x1, or 0x2 */
     }
 #endif /* SDL_JOYSTICK_RAWINPUT_MATCH_AXES */
+#ifdef SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+    for (; ii < SDL_JOYSTICK_RAWINPUT_MATCH_COUNT; ii++) {
+        state->match_axes[ii] = (SHORT)((match_state & (0x000F0000ull << (ii * 4))) >> (4 + ii * 4));
+        any_axes_data |= (state->match_axes[ii] != SDL_MIN_SINT16);
+    }
+#endif /* SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS */
+
+    state->any_data = any_axes_data;
 
 #ifdef SDL_JOYSTICK_RAWINPUT_XINPUT
     /* Match axes by checking if the distance between the high 4 bits of axis and the 4 bits from match_state is 1 or less */
@@ -220,9 +232,16 @@ static void RAWINPUT_FillMatchState(WindowsMatchState *state, Uint32 match_state
     SDL_abs((Sint8)((gamepad.sThumbRX & 0xF000) >> 8) - ((match_state & 0x0F000000) >> 20)) <= 0x10 && \
     SDL_abs((Sint8)((~gamepad.sThumbRY & 0xF000) >> 8) - ((match_state & 0xF0000000) >> 24)) <= 0x10) */
 
+    /* Can only match trigger values if a single trigger has a value. */
+#define XInputTriggersMatch(gamepad) ( \
+    ((state->match_axes[4] == SDL_MIN_SINT16) && (state->match_axes[5] == SDL_MIN_SINT16)) || \
+    ((gamepad.bLeftTrigger != 0) && (gamepad.bRightTrigger != 0)) || \
+    ((Uint32)((((int)gamepad.bLeftTrigger * 257) - 32768) - state->match_axes[4]) <= 0x2fff) || \
+    ((Uint32)((((int)gamepad.bRightTrigger * 257) - 32768) - state->match_axes[5]) <= 0x2fff))
+
     state->xinput_buttons =
         /* Bitwise map .RLDUWVQTS.KYXBA -> YXBA..WVQTKSRLDU */
-        match_state << 12 | (match_state & 0x0780) >> 1 | (match_state & 0x0010) << 1 | (match_state & 0x0040) >> 2 | (match_state & 0x7800) >> 11;
+        (WORD)(match_state << 12 | (match_state & 0x0780) >> 1 | (match_state & 0x0010) << 1 | (match_state & 0x0040) >> 2 | (match_state & 0x7800) >> 11);
     /*  Explicit
         ((match_state & (1<<SDL_CONTROLLER_BUTTON_A)) ? XINPUT_GAMEPAD_A : 0) |
         ((match_state & (1<<SDL_CONTROLLER_BUTTON_B)) ? XINPUT_GAMEPAD_B : 0) |
@@ -252,6 +271,11 @@ static void RAWINPUT_FillMatchState(WindowsMatchState *state, Uint32 match_state
     (Uint16)(((Sint16)(gamepad.RightThumbstickX * SDL_MAX_SINT16) & 0xF000) - state->match_axes[2] + 0x1000) <= 0x2fff && \
     (Uint16)((~(Sint16)(gamepad.RightThumbstickY * SDL_MAX_SINT16) & 0xF000) - state->match_axes[3] + 0x1000) <= 0x2fff)
 
+#define WindowsGamingInputTriggersMatch(gamepad) ( \
+    ((state->match_axes[4] == SDL_MIN_SINT16) && (state->match_axes[5] == SDL_MIN_SINT16)) || \
+    ((gamepad.LeftTrigger == 0.0f) && (gamepad.RightTrigger == 0.0f)) || \
+    ((Uint16)((((int)(gamepad.LeftTrigger * SDL_MAX_UINT16)) - 32768) - state->match_axes[4]) <= 0x2fff) || \
+    ((Uint16)((((int)(gamepad.RightTrigger * SDL_MAX_UINT16)) - 32768) - state->match_axes[5]) <= 0x2fff))
 
     state->wgi_buttons =
         /* Bitwise map .RLD UWVQ TS.K YXBA -> ..QT WVRL DUYX BAKS */
@@ -354,6 +378,9 @@ RAWINPUT_XInputSlotMatches(const WindowsMatchState *state, Uint8 slot_idx)
         if ((xinput_buttons & ~XINPUT_GAMEPAD_GUIDE) == state->xinput_buttons
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCH_AXES
             && XInputAxesMatch(xinput_state[slot_idx].state.Gamepad)
+#endif
+#ifdef SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+            && XInputTriggersMatch(xinput_state[slot_idx].state.Gamepad)
 #endif
             ) {
             return SDL_TRUE;
@@ -570,12 +597,16 @@ RAWINPUT_InitWindowsGamingInput(RAWINPUT_DeviceContext *ctx)
 }
 
 static SDL_bool
-RAWINPUT_WindowsGamingInputSlotMatches(const WindowsMatchState *state, WindowsGamingInputGamepadState *slot)
+RAWINPUT_WindowsGamingInputSlotMatches(const WindowsMatchState *state, WindowsGamingInputGamepadState *slot, SDL_bool xinput_correlated)
 {
     Uint32 wgi_buttons = slot->state.Buttons;
     if ((wgi_buttons & 0x3FFF) == state->wgi_buttons
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCH_AXES
             && WindowsGamingInputAxesMatch(slot->state)
+#endif
+#ifdef SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+            // Don't try to match WGI triggers if getting values from XInput
+            && (xinput_correlated || WindowsGamingInputTriggersMatch(slot->state))
 #endif
        ) {
         return SDL_TRUE;
@@ -584,14 +615,14 @@ RAWINPUT_WindowsGamingInputSlotMatches(const WindowsMatchState *state, WindowsGa
 }
 
 static SDL_bool
-RAWINPUT_GuessWindowsGamingInputSlot(const WindowsMatchState *state, Uint8 *correlation_id, WindowsGamingInputGamepadState **slot)
+RAWINPUT_GuessWindowsGamingInputSlot(const WindowsMatchState *state, Uint8 *correlation_id, WindowsGamingInputGamepadState **slot, SDL_bool xinput_correlated)
 {
     int match_count, user_index;
 
     match_count = 0;
     for (user_index = 0; user_index < wgi_state.per_gamepad_count; ++user_index) {
         WindowsGamingInputGamepadState *gamepad_state = wgi_state.per_gamepad[user_index];
-        if (RAWINPUT_WindowsGamingInputSlotMatches(state, gamepad_state)) {
+        if (RAWINPUT_WindowsGamingInputSlotMatches(state, gamepad_state, xinput_correlated)) {
             ++match_count;
             *slot = gamepad_state;
             /* Incrementing correlation_id for any match, as negative evidence for others being correlated */
@@ -1383,12 +1414,13 @@ RAWINPUT_HandleStatePacket(SDL_Joystick *joystick, Uint8 *data, int size)
         (1 << SDL_CONTROLLER_BUTTON_DPAD_LEFT),
         (1 << SDL_CONTROLLER_BUTTON_DPAD_UP) | (1 << SDL_CONTROLLER_BUTTON_DPAD_LEFT),
     };
-    Uint32 match_state = ctx->match_state;
+    Uint64 match_state = ctx->match_state;
     /* Update match_state with button bit, then fall through */
-#define SDL_PrivateJoystickButton(joystick, button, state) if (button < SDL_arraysize(button_map)) { if (state) match_state |= 1 << button_map[button]; else match_state &= ~(1 << button_map[button]); } SDL_PrivateJoystickButton(joystick, button, state)
+#define SDL_PrivateJoystickButton(joystick, button, state) if (button < SDL_arraysize(button_map)) { Uint64 button_bit = 1ull << button_map[button]; match_state = (match_state & ~button_bit) | (button_bit * (state)); } SDL_PrivateJoystickButton(joystick, button, state)
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCH_AXES
     /* Grab high 4 bits of value, then fall through */
-#define SDL_PrivateJoystickAxis(joystick, axis, value) if (axis < 4) match_state = (match_state & ~(0xF << (4 * axis + 16))) | ((value) & 0xF000) << (4 * axis + 4); SDL_PrivateJoystickAxis(joystick, axis, value)
+#define AddAxisToMatchState(axis, value) { match_state = (match_state & ~(0xFull << (4 * axis + 16))) | ((value) & 0xF000ull) << (4 * axis + 4); }
+#define SDL_PrivateJoystickAxis(joystick, axis, value) if (axis < 4) AddAxisToMatchState(axis, value); SDL_PrivateJoystickAxis(joystick, axis, value)
 #endif
 #endif /* SDL_JOYSTICK_RAWINPUT_MATCHING */
 
@@ -1453,6 +1485,10 @@ RAWINPUT_HandleStatePacket(SDL_Joystick *joystick, Uint8 *data, int size)
 #undef SDL_PrivateJoystickAxis
 #endif
 
+#ifdef SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+#define AddTriggerToMatchState(axis, value) { int match_axis = axis + SDL_JOYSTICK_RAWINPUT_MATCH_COUNT - joystick->naxes; AddAxisToMatchState(match_axis, value); }
+#endif /* SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS */
+
     if (ctx->trigger_hack) {
         SDL_bool has_trigger_data = SDL_FALSE;
 
@@ -1469,28 +1505,38 @@ RAWINPUT_HandleStatePacket(SDL_Joystick *joystick, Uint8 *data, int size)
         }
 #endif /* SDL_JOYSTICK_RAWINPUT_WGI */
 
-        if (!has_trigger_data) {
+        int left_trigger = joystick->naxes - 2;
+        int right_trigger = joystick->naxes - 1;
+#ifndef SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+        if (!has_trigger_data)
+#endif
+        {
             HIDP_DATA *item = GetData(ctx->trigger_hack_index, ctx->data, data_length);
             if (item) {
-                int left_trigger = joystick->naxes - 2;
-                int right_trigger = joystick->naxes - 1;
                 Sint16 value = (int)(Uint16)item->RawValue - 0x8000;
-                if (value < 0) {
-                    value = -value * 2 - 32769;
-                    SDL_PrivateJoystickAxis(joystick, left_trigger, SDL_MIN_SINT16);
-                    SDL_PrivateJoystickAxis(joystick, right_trigger, value);
-                } else if (value > 0) {
-                    value = value * 2 - 32767;
-                    SDL_PrivateJoystickAxis(joystick, left_trigger, value);
-                    SDL_PrivateJoystickAxis(joystick, right_trigger, SDL_MIN_SINT16);
-                } else {
-                    SDL_PrivateJoystickAxis(joystick, left_trigger, SDL_MIN_SINT16);
-                    SDL_PrivateJoystickAxis(joystick, right_trigger, SDL_MIN_SINT16);
+                Sint16 left_value = (value > 0) ? (value * 2 - 32767) : SDL_MIN_SINT16;
+                Sint16 right_value = (value < 0) ? (-value * 2 - 32769) : SDL_MIN_SINT16;
+        
+#ifdef SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS
+                AddTriggerToMatchState(left_trigger, left_value);
+                AddTriggerToMatchState(right_trigger, right_value);
+                if (!has_trigger_data)
+#endif /* SDL_JOYSTICK_RAWINPUT_MATCH_TRIGGERS */
+                {
+                    SDL_PrivateJoystickAxis(joystick, left_trigger, left_value);
+                    SDL_PrivateJoystickAxis(joystick, right_trigger, right_value);
                 }
             }
         }
     }
 
+#ifdef AddAxisToMatchState
+#undef AddAxisToMatchState
+#endif
+#ifdef AddTriggerToMatchState
+#undef AddTriggerToMatchState
+#endif
+
 #ifdef SDL_JOYSTICK_RAWINPUT_MATCHING
     if (ctx->is_xinput) {
         ctx->match_state = match_state;
@@ -1520,7 +1566,7 @@ RAWINPUT_UpdateOtherAPIs(SDL_Joystick *joystick)
         !joystick->low_frequency_rumble && !joystick->high_frequency_rumble &&
         !joystick->left_trigger_rumble && !joystick->right_trigger_rumble) {
         /* We have been previously correlated, ensure we are still matching, see comments in XINPUT section */
-        if (RAWINPUT_WindowsGamingInputSlotMatches(&match_state_xinput, ctx->wgi_slot)) {
+        if (RAWINPUT_WindowsGamingInputSlotMatches(&match_state_xinput, ctx->wgi_slot, ctx->xinput_correlated)) {
             ctx->wgi_uncorrelate_count = 0;
         } else {
             ++ctx->wgi_uncorrelate_count;
@@ -1549,7 +1595,7 @@ RAWINPUT_UpdateOtherAPIs(SDL_Joystick *joystick)
         if (RAWINPUT_MissingWindowsGamingInputSlot()) {
             Uint8 correlation_id;
             WindowsGamingInputGamepadState *slot_idx = NULL;
-            if (RAWINPUT_GuessWindowsGamingInputSlot(&match_state_xinput, &correlation_id, &slot_idx)) {
+            if (RAWINPUT_GuessWindowsGamingInputSlot(&match_state_xinput, &correlation_id, &slot_idx, ctx->xinput_correlated)) {
                 /* we match exactly one WindowsGamingInput device */
                 /* Probably can do without wgi_correlation_count, just check and clear wgi_slot to NULL, unless we need
                    even more frames to be sure. */