Fixed handling status changes in the FlyDigi controller driver

(cherry picked from commit ce6a6d1b392d2f271e9a40a3c41aaad71ce194ab)

src/joystick/hidapi/SDL_hidapi_flydigi.c

@@ -136,6 +136,167 @@ static bool HIDAPI_DriverFlydigi_IsSupportedDevice(SDL_HIDAPI_Device *device, co
     return false;
 }
 
+static void HIDAPI_DriverFlydigi_UpdateDeviceIdentity(SDL_HIDAPI_Device *device)
+{
+    SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
+
+    Uint8 controller_type = SDL_FLYDIGI_UNKNOWN;
+    switch (ctx->deviceID) {
+    case 19:
+        controller_type = SDL_FLYDIGI_APEX2;
+        break;
+    case 24:
+    case 26:
+    case 29:
+        controller_type = SDL_FLYDIGI_APEX3;
+        break;
+    case 84:
+        controller_type = SDL_FLYDIGI_APEX4;
+        break;
+    case 20:
+    case 21:
+    case 23:
+        controller_type = SDL_FLYDIGI_VADER2;
+        break;
+    case 22:
+        controller_type = SDL_FLYDIGI_VADER2_PRO;
+        break;
+    case 28:
+        controller_type = SDL_FLYDIGI_VADER3;
+        break;
+    case 80:
+    case 81:
+        controller_type = SDL_FLYDIGI_VADER3_PRO;
+        break;
+    case 85:
+    case 91:
+    case 105:
+        controller_type = SDL_FLYDIGI_VADER4_PRO;
+        break;
+    case 128:
+    case 129:
+        controller_type = SDL_FLYDIGI_APEX5;
+        break;
+    case 130:
+        controller_type = SDL_FLYDIGI_VADER5_PRO;
+        break;
+    case 133:
+    case 134:
+        controller_type = SDL_FLYDIGI_APEX5;
+        break;
+    default:
+        // Try to guess from the name of the controller
+        if (SDL_strcasestr(device->name, "VADER") != NULL) {
+            if (SDL_strstr(device->name, "VADER2") != NULL) {
+                controller_type = SDL_FLYDIGI_VADER2;
+            } else if (SDL_strstr(device->name, "VADER3") != NULL) {
+                controller_type = SDL_FLYDIGI_VADER3;
+            } else if (SDL_strstr(device->name, "VADER4") != NULL) {
+                controller_type = SDL_FLYDIGI_VADER4_PRO;
+            } else if (SDL_strstr(device->name, "Vader 5") != NULL) {
+                controller_type = SDL_FLYDIGI_VADER5_PRO;
+            }
+        } else if (SDL_strstr(device->name, "APEX") != NULL) {
+            if (SDL_strstr(device->name, "APEX2") != NULL) {
+                controller_type = SDL_FLYDIGI_APEX2;
+            } else if (SDL_strstr(device->name, "APEX3") != NULL) {
+                controller_type = SDL_FLYDIGI_APEX3;
+            } else if (SDL_strstr(device->name, "APEX4") != NULL) {
+                controller_type = SDL_FLYDIGI_APEX4;
+            } else if (SDL_strstr(device->name, "APEX5") != NULL) {
+                controller_type = SDL_FLYDIGI_APEX5;
+            }
+        }
+        break;
+    }
+    device->guid.data[15] = controller_type;
+
+    // This is the previous sensor default of 125hz.
+    // Override this in the switch statement below based on observed sensor packet rate.
+    ctx->sensor_timestamp_step_ns = SDL_NS_PER_SECOND / 125;
+
+    switch (controller_type) {
+    case SDL_FLYDIGI_APEX2:
+        HIDAPI_SetDeviceName(device, "Flydigi Apex 2");
+        break;
+    case SDL_FLYDIGI_APEX3:
+        HIDAPI_SetDeviceName(device, "Flydigi Apex 3");
+        break;
+    case SDL_FLYDIGI_APEX4:
+        // The Apex 4 controller has sensors, but they're only reported when gyro mouse is enabled
+        HIDAPI_SetDeviceName(device, "Flydigi Apex 4");
+        break;
+    case SDL_FLYDIGI_APEX5:
+        HIDAPI_SetDeviceName(device, "Flydigi Apex 5");
+        ctx->has_lmrm = true;
+        ctx->sensors_supported = true;
+        ctx->accelScale = SDL_STANDARD_GRAVITY / 4096.0f;
+        ctx->gyroScale = DEG2RAD(2000.0f);
+        ctx->sensor_timestamp_step_ns = ctx->wireless ? SENSOR_INTERVAL_APEX5_DONGLE_NS : SENSOR_INTERVAL_APEX5_WIRED_NS;
+        break;
+    case SDL_FLYDIGI_VADER2:
+        // The Vader 2 controller has sensors, but they're only reported when gyro mouse is enabled
+        HIDAPI_SetDeviceName(device, "Flydigi Vader 2");
+        ctx->has_cz = true;
+        break;
+    case SDL_FLYDIGI_VADER2_PRO:
+        HIDAPI_SetDeviceName(device, "Flydigi Vader 2 Pro");
+        ctx->has_cz = true;
+        break;
+    case SDL_FLYDIGI_VADER3:
+        HIDAPI_SetDeviceName(device, "Flydigi Vader 3");
+        ctx->has_cz = true;
+        break;
+    case SDL_FLYDIGI_VADER3_PRO:
+        HIDAPI_SetDeviceName(device, "Flydigi Vader 3 Pro");
+        ctx->has_cz = true;
+        ctx->sensors_supported = true;
+        ctx->accelScale = SDL_STANDARD_GRAVITY / 256.0f;
+        ctx->sensor_timestamp_step_ns = ctx->wireless ? SENSOR_INTERVAL_VADER4_PRO_DONGLE_NS : SENSOR_INTERVAL_VADER4_PRO_WIRED_NS;
+        break;
+    case SDL_FLYDIGI_VADER4_PRO:
+        HIDAPI_SetDeviceName(device, "Flydigi Vader 4 Pro");
+        ctx->has_cz = true;
+        ctx->sensors_supported = true;
+        ctx->accelScale = SDL_STANDARD_GRAVITY / 256.0f;
+        ctx->sensor_timestamp_step_ns = ctx->wireless ? SENSOR_INTERVAL_VADER4_PRO_DONGLE_NS : SENSOR_INTERVAL_VADER4_PRO_WIRED_NS;
+        break;
+    case SDL_FLYDIGI_VADER5_PRO:
+        HIDAPI_SetDeviceName(device, "Flydigi Vader 5 Pro");
+        ctx->has_cz = true;
+        ctx->has_lmrm = true;
+        ctx->has_circle = true;
+        ctx->sensors_supported = true;
+        ctx->accelScale = SDL_STANDARD_GRAVITY / 4096.0f;
+        ctx->gyroScale = DEG2RAD(2000.0f);
+        ctx->sensor_timestamp_step_ns = SENSOR_INTERVAL_VADER5_PRO_NS;
+        break;
+    default:
+        SDL_LogDebug(SDL_LOG_CATEGORY_INPUT, "Unknown FlyDigi controller with ID %d, name '%s'", ctx->deviceID, device->name);
+        break;
+    }
+}
+
+static void HIDAPI_DriverFlydigi_SetAvailable(SDL_HIDAPI_Device *device, bool available)
+{
+    SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
+
+    if (available == ctx->available) {
+        return;
+    }
+
+    if (available) {
+        if (device->num_joysticks == 0) {
+            HIDAPI_JoystickConnected(device, NULL);
+        }
+    } else {
+        if (device->num_joysticks > 0) {
+            HIDAPI_JoystickDisconnected(device, device->joysticks[0]);
+        }
+    }
+    ctx->available = available;
+}
+
 static bool HIDAPI_DriverFlydigi_InitControllerV1(SDL_HIDAPI_Device *device)
 {
     SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
@@ -191,29 +352,12 @@ static bool HIDAPI_DriverFlydigi_InitControllerV1(SDL_HIDAPI_Device *device)
             }
         }
     }
-    ctx->available = true;
-
-    return true;
-}
 
-static void HIDAPI_DriverFlydigi_SetAvailable(SDL_HIDAPI_Device* device, bool available)
-{
-    SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
+    HIDAPI_DriverFlydigi_UpdateDeviceIdentity(device);
 
-    if (available == ctx->available) {
-        return;
-    }
+    HIDAPI_DriverFlydigi_SetAvailable(device, true);
 
-    if (available) {
-        if (device->num_joysticks == 0) {
-            HIDAPI_JoystickConnected(device, NULL);
-        }
-    } else {
-        if (device->num_joysticks > 0) {
-            HIDAPI_JoystickDisconnected(device, device->joysticks[0]);
-        }
-    }
-    ctx->available = available;
+    return true;
 }
 
 static bool GetReply(SDL_HIDAPI_Device* device, Uint8 command, Uint8* data, size_t length)
@@ -247,9 +391,33 @@ static bool GetReply(SDL_HIDAPI_Device* device, Uint8 command, Uint8* data, size
     return false;
 }
 
+static bool SDL_HIDAPI_Flydigi_SendStatusRequest(SDL_HIDAPI_Device *device)
+{
+    const Uint8 cmd[] = {
+        FLYDIGI_V2_CMD_REPORT_ID,
+        FLYDIGI_V2_MAGIC1,
+        FLYDIGI_V2_MAGIC2,
+        FLYDIGI_V2_GET_STATUS_COMMAND
+    };
+    if (SDL_hid_write(device->dev, cmd, sizeof(cmd)) < 0) {
+        return SDL_SetError("Couldn't query controller status");
+    }
+    return true;
+}
+
+static void HIDAPI_DriverFlydigi_HandleStatusResponse(SDL_HIDAPI_Device *device, Uint8 *data, int size)
+{
+    if (data[9] == 1) {
+        HIDAPI_DriverFlydigi_SetAvailable(device, true);
+    } else {
+        // Click "Allow third-party apps to take over mappings" in the FlyDigi Space Station app
+        HIDAPI_DriverFlydigi_SetAvailable(device, false);
+    }
+}
+
 static bool SDL_HIDAPI_Flydigi_SendAcquireRequest(SDL_HIDAPI_Device *device, bool acquire)
 {
-    const Uint8 acquireControllerCmd[32] = {
+    const Uint8 cmd[32] = {
         FLYDIGI_V2_CMD_REPORT_ID,
         FLYDIGI_V2_MAGIC1,
         FLYDIGI_V2_MAGIC2,
@@ -259,7 +427,7 @@ static bool SDL_HIDAPI_Flydigi_SendAcquireRequest(SDL_HIDAPI_Device *device, boo
         'S', 'D', 'L', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
     };
 
-    if (SDL_hid_write(device->dev, acquireControllerCmd, sizeof(acquireControllerCmd)) < 0) {
+    if (SDL_hid_write(device->dev, cmd, sizeof(cmd)) < 0) {
         return SDL_SetError("Couldn't send acquire command");
     }
     return true;
@@ -321,161 +489,12 @@ static bool HIDAPI_DriverFlydigi_InitControllerV2(SDL_HIDAPI_Device *device)
     }
     ctx->deviceID = data[5];
 
-    // See whether we can acquire the controller
-    const Uint8 query_status[] = { FLYDIGI_V2_CMD_REPORT_ID, FLYDIGI_V2_MAGIC1, FLYDIGI_V2_MAGIC2, FLYDIGI_V2_GET_STATUS_COMMAND };
-    if (SDL_hid_write(device->dev, query_status, sizeof(query_status)) < 0) {
-        return SDL_SetError("Couldn't query controller status");
-    }
-    if (!GetReply(device, FLYDIGI_V2_GET_STATUS_COMMAND, data, sizeof(data))) {
-        return SDL_SetError("Couldn't get controller status");
-    }
-    if (data[9] == 1) {
-        ctx->available = true;
-    } else {
-        // Click "Allow third-party apps to take over mappings" in the FlyDigi Space Station app
-    }
-    return true;
-}
-
-static void HIDAPI_DriverFlydigi_UpdateDeviceIdentity(SDL_HIDAPI_Device *device)
-{
-    SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
-
-    Uint8 controller_type = SDL_FLYDIGI_UNKNOWN;
-    switch (ctx->deviceID) {
-    case 19:
-        controller_type = SDL_FLYDIGI_APEX2;
-        break;
-    case 24:
-    case 26:
-    case 29:
-        controller_type = SDL_FLYDIGI_APEX3;
-        break;
-    case 84:
-        controller_type = SDL_FLYDIGI_APEX4;
-        break;
-    case 20:
-    case 21:
-    case 23:
-        controller_type = SDL_FLYDIGI_VADER2;
-        break;
-    case 22:
-        controller_type = SDL_FLYDIGI_VADER2_PRO;
-        break;
-    case 28:
-        controller_type = SDL_FLYDIGI_VADER3;
-        break;
-    case 80:
-    case 81:
-        controller_type = SDL_FLYDIGI_VADER3_PRO;
-        break;
-    case 85:
-    case 91:
-    case 105:
-        controller_type = SDL_FLYDIGI_VADER4_PRO;
-        break;
-    case 128:
-    case 129:
-        controller_type = SDL_FLYDIGI_APEX5;
-        break;
-    case 130:
-        controller_type = SDL_FLYDIGI_VADER5_PRO;
-        break;
-    case 133:
-    case 134:
-        controller_type = SDL_FLYDIGI_APEX5;
-        break;
-    default:
-        // Try to guess from the name of the controller
-        if (SDL_strcasestr(device->name, "VADER") != NULL) {
-            if (SDL_strstr(device->name, "VADER2") != NULL) {
-                controller_type = SDL_FLYDIGI_VADER2;
-            } else if (SDL_strstr(device->name, "VADER3") != NULL) {
-                controller_type = SDL_FLYDIGI_VADER3;
-            } else if (SDL_strstr(device->name, "VADER4") != NULL) {
-                controller_type = SDL_FLYDIGI_VADER4_PRO;
-            } else if (SDL_strstr(device->name, "Vader 5") != NULL) {
-                controller_type = SDL_FLYDIGI_VADER5_PRO;
-            }
-        } else if (SDL_strstr(device->name, "APEX") != NULL) {
-            if (SDL_strstr(device->name, "APEX2") != NULL) {
-                controller_type = SDL_FLYDIGI_APEX2;
-            } else if (SDL_strstr(device->name, "APEX3") != NULL) {
-                controller_type = SDL_FLYDIGI_APEX3;
-            } else if (SDL_strstr(device->name, "APEX4") != NULL) {
-                controller_type = SDL_FLYDIGI_APEX4;
-            } else if (SDL_strstr(device->name, "APEX5") != NULL) {
-                controller_type = SDL_FLYDIGI_APEX5;
-            }
-        }
-        break;
-    }
-    device->guid.data[15] = controller_type;
+    HIDAPI_DriverFlydigi_UpdateDeviceIdentity(device);
 
-    // This is the previous sensor default of 125hz.
-    // Override this in the switch statement below based on observed sensor packet rate.
-    ctx->sensor_timestamp_step_ns = SDL_NS_PER_SECOND / 125;
+    // See whether we can acquire the controller
+    SDL_HIDAPI_Flydigi_SendStatusRequest(device);
 
-    switch (controller_type) {
-    case SDL_FLYDIGI_APEX2:
-        HIDAPI_SetDeviceName(device, "Flydigi Apex 2");
-        break;
-    case SDL_FLYDIGI_APEX3:
-        HIDAPI_SetDeviceName(device, "Flydigi Apex 3");
-        break;
-    case SDL_FLYDIGI_APEX4:
-        // The Apex 4 controller has sensors, but they're only reported when gyro mouse is enabled
-        HIDAPI_SetDeviceName(device, "Flydigi Apex 4");
-        break;
-    case SDL_FLYDIGI_APEX5:
-        HIDAPI_SetDeviceName(device, "Flydigi Apex 5");
-        ctx->has_lmrm = true;
-        ctx->sensors_supported = true;
-        ctx->accelScale = SDL_STANDARD_GRAVITY / 4096.0f;
-        ctx->gyroScale = DEG2RAD(2000.0f);
-        ctx->sensor_timestamp_step_ns = ctx->wireless ? SENSOR_INTERVAL_APEX5_DONGLE_NS : SENSOR_INTERVAL_APEX5_WIRED_NS;
-        break;
-    case SDL_FLYDIGI_VADER2:
-        // The Vader 2 controller has sensors, but they're only reported when gyro mouse is enabled
-        HIDAPI_SetDeviceName(device, "Flydigi Vader 2");
-        ctx->has_cz = true;
-        break;
-    case SDL_FLYDIGI_VADER2_PRO:
-        HIDAPI_SetDeviceName(device, "Flydigi Vader 2 Pro");
-        ctx->has_cz = true;
-        break;
-    case SDL_FLYDIGI_VADER3:
-        HIDAPI_SetDeviceName(device, "Flydigi Vader 3");
-        ctx->has_cz = true;
-        break;
-    case SDL_FLYDIGI_VADER3_PRO:
-        HIDAPI_SetDeviceName(device, "Flydigi Vader 3 Pro");
-        ctx->has_cz = true;
-        ctx->sensors_supported = true;
-        ctx->accelScale = SDL_STANDARD_GRAVITY / 256.0f;
-        ctx->sensor_timestamp_step_ns = ctx->wireless ? SENSOR_INTERVAL_VADER4_PRO_DONGLE_NS : SENSOR_INTERVAL_VADER4_PRO_WIRED_NS;
-        break;
-    case SDL_FLYDIGI_VADER4_PRO:
-        HIDAPI_SetDeviceName(device, "Flydigi Vader 4 Pro");
-        ctx->has_cz = true;
-        ctx->sensors_supported = true;
-        ctx->accelScale = SDL_STANDARD_GRAVITY / 256.0f;
-        ctx->sensor_timestamp_step_ns = ctx->wireless ? SENSOR_INTERVAL_VADER4_PRO_DONGLE_NS : SENSOR_INTERVAL_VADER4_PRO_WIRED_NS;
-        break;
-    case SDL_FLYDIGI_VADER5_PRO:
-        HIDAPI_SetDeviceName(device, "Flydigi Vader 5 Pro");
-        ctx->has_cz = true;
-        ctx->has_lmrm = true;
-        ctx->has_circle = true;
-        ctx->sensors_supported = true;
-        ctx->accelScale = SDL_STANDARD_GRAVITY / 4096.0f;
-        ctx->gyroScale = DEG2RAD(2000.0f);
-        ctx->sensor_timestamp_step_ns = SENSOR_INTERVAL_VADER5_PRO_NS;
-        break;
-    default:
-        SDL_LogDebug(SDL_LOG_CATEGORY_INPUT, "Unknown FlyDigi controller with ID %d, name '%s'", ctx->deviceID, device->name);
-        break;
-    }
+    return true;
 }
 
 static bool HIDAPI_DriverFlydigi_InitDevice(SDL_HIDAPI_Device *device)
@@ -488,23 +507,10 @@ static bool HIDAPI_DriverFlydigi_InitDevice(SDL_HIDAPI_Device *device)
 
     device->context = ctx;
 
-    bool initialized;
     if (device->vendor_id == USB_VENDOR_FLYDIGI_V1) {
-        initialized = HIDAPI_DriverFlydigi_InitControllerV1(device);
-    } else {
-        initialized = HIDAPI_DriverFlydigi_InitControllerV2(device);
-    }
-    if (!initialized) {
-        return false;
-    }
-
-    HIDAPI_DriverFlydigi_UpdateDeviceIdentity(device);
-
-    if (ctx->available) {
-        return HIDAPI_JoystickConnected(device, NULL);
+        return HIDAPI_DriverFlydigi_InitControllerV1(device);
     } else {
-        // We'll connect it once it becomes available
-        return true;
+        return HIDAPI_DriverFlydigi_InitControllerV2(device);
     }
 }
 
@@ -881,13 +887,8 @@ static void HIDAPI_DriverFlydigi_HandleStatePacketV2(SDL_Joystick *joystick, SDL
 
 static void HIDAPI_DriverFlydigi_HandleStatusUpdate(SDL_HIDAPI_Device *device, Uint8 *data, int size)
 {
-    if (data[9] & 0x01) {
-        // We can now acquire the controller
-        HIDAPI_DriverFlydigi_SetAvailable(device, true);
-    } else {
-        // We can no longer acquire the controller
-        HIDAPI_DriverFlydigi_SetAvailable(device, false);
-    }
+    // The status changed, see if we can acquire the controller now
+    SDL_HIDAPI_Flydigi_SendStatusRequest(device);
 }
 
 static void HIDAPI_DriverFlydigi_HandlePacketV2(SDL_Joystick *joystick, SDL_DriverFlydigi_Context *ctx, Uint8 *data, int size)
@@ -903,6 +904,12 @@ static void HIDAPI_DriverFlydigi_HandlePacketV2(SDL_Joystick *joystick, SDL_Driv
     }
 
     switch (data[2]) {
+    case FLYDIGI_V2_SET_STATUS_COMMAND:
+        HIDAPI_DriverFlydigi_HandleStatusUpdate(ctx->device, data, size);
+        break;
+    case FLYDIGI_V2_GET_STATUS_COMMAND:
+        HIDAPI_DriverFlydigi_HandleStatusResponse(ctx->device, data, size);
+        break;
     case FLYDIGI_V2_ACQUIRE_CONTROLLER_COMMAND:
         HIDAPI_DriverFlydigi_HandleAcquireResponse(ctx->device, data, size);
         break;
@@ -911,9 +918,6 @@ static void HIDAPI_DriverFlydigi_HandlePacketV2(SDL_Joystick *joystick, SDL_Driv
             HIDAPI_DriverFlydigi_HandleStatePacketV2(joystick, ctx, data, size);
         }
         break;
-    case FLYDIGI_V2_SET_STATUS_COMMAND:
-        HIDAPI_DriverFlydigi_HandleStatusUpdate(ctx->device, data, size);
-        break;
     default:
         // We don't recognize this command, ignore it
         break;