Fixed handling status changes in the FlyDigi controller driver

src/joystick/hidapi/SDL_hidapi_flydigi.c

@@ -136,207 +136,6 @@ static bool HIDAPI_DriverFlydigi_IsSupportedDevice(SDL_HIDAPI_Device *device, co
     return false;
 }
 
-static bool HIDAPI_DriverFlydigi_InitControllerV1(SDL_HIDAPI_Device *device)
-{
-    SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
-
-    // Detecting the Vader 2 can take over 1000 read retries, so be generous here
-    for (int attempt = 0; ctx->deviceID == 0 && attempt < 30; ++attempt) {
-        const Uint8 request[] = { FLYDIGI_V1_CMD_REPORT_ID, FLYDIGI_V1_GET_INFO_COMMAND, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
-        // This write will occasionally return -1, so ignore failure here and try again
-        (void)SDL_hid_write(device->dev, request, sizeof(request));
-
-        // Read the reply
-        for (int i = 0; i < 100; ++i) {
-            SDL_Delay(1);
-
-            Uint8 data[USB_PACKET_LENGTH];
-            int size = SDL_hid_read_timeout(device->dev, data, sizeof(data), 0);
-            if (size < 0) {
-                break;
-            }
-            if (size == 0) {
-                continue;
-            }
-
-#ifdef DEBUG_FLYDIGI_PROTOCOL
-            HIDAPI_DumpPacket("Flydigi packet: size = %d", data, size);
-#endif
-            if (size == 32 && data[15] == 236) {
-                ctx->deviceID = data[3];
-                ctx->firmware_version = LOAD16(data[9], data[10]);
-
-                char serial[9];
-                (void)SDL_snprintf(serial, sizeof(serial), "%.2x%.2x%.2x%.2x", data[5], data[6], data[7], data[8]);
-                HIDAPI_SetDeviceSerial(device, serial);
-
-                // The Vader 2 with firmware 6.0.4.9 doesn't report the connection state
-                if (ctx->firmware_version >= 0x6400) {
-                    switch (data[13]) {
-                    case 0:
-                        // Wireless connection
-                        ctx->wireless = true;
-                        break;
-                    case 1:
-                        // Wired connection
-                        ctx->wireless = false;
-                        break;
-                    default:
-                        break;
-                    }
-                }
-
-                // Done!
-                break;
-            }
-        }
-    }
-    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;
-
-    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 GetReply(SDL_HIDAPI_Device* device, Uint8 command, Uint8* data, size_t length)
-{
-    for (int i = 0; i < 100; ++i) {
-        SDL_Delay(1);
-
-        int size = SDL_hid_read_timeout(device->dev, data, length, 0);
-        if (size < 0) {
-            break;
-        }
-        if (size == 0) {
-            continue;
-        }
-
-#ifdef DEBUG_FLYDIGI_PROTOCOL
-        HIDAPI_DumpPacket("Flydigi packet: size = %d", data, size);
-#endif
-
-        if (size == 32) {
-            if (data[1] == FLYDIGI_V2_MAGIC1 && data[2] == FLYDIGI_V2_MAGIC2) {
-                // Skip the report ID
-                SDL_memmove(&data[0], &data[1], size - 1);
-                data[size - 1] = 0;
-            }
-            if (data[0] == FLYDIGI_V2_MAGIC1 && data[1] == FLYDIGI_V2_MAGIC2 && data[2] == command) {
-                return true;
-            }
-        }
-    }
-    return false;
-}
-
-static bool SDL_HIDAPI_Flydigi_SendAcquireRequest(SDL_HIDAPI_Device *device, bool acquire)
-{
-    const Uint8 acquireControllerCmd[32] = {
-        FLYDIGI_V2_CMD_REPORT_ID,
-        FLYDIGI_V2_MAGIC1,
-        FLYDIGI_V2_MAGIC2,
-        FLYDIGI_V2_ACQUIRE_CONTROLLER_COMMAND,
-        23,
-        acquire ? 1 : 0,
-        '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) {
-        return SDL_SetError("Couldn't send acquire command");
-    }
-    return true;
-}
-
-static void HIDAPI_DriverFlydigi_HandleAcquireResponse(SDL_HIDAPI_Device *device, Uint8 *data, int size)
-{
-    if (data[5] != 1 && data[6] == 0) {
-        // Controller acquiring failed or has been disabled
-        HIDAPI_DriverFlydigi_SetAvailable(device, false);
-    }
-}
-
-static bool HIDAPI_DriverFlydigi_InitControllerV2(SDL_HIDAPI_Device *device)
-{
-    SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
-
-    Uint8 data[USB_PACKET_LENGTH];
-    const Uint8 query_info[] = { FLYDIGI_V2_CMD_REPORT_ID, FLYDIGI_V2_MAGIC1, FLYDIGI_V2_MAGIC2, FLYDIGI_V2_GET_INFO_COMMAND, 2, 0 };
-    if (SDL_hid_write(device->dev, query_info, sizeof(query_info)) < 0) {
-        return SDL_SetError("Couldn't query controller info");
-    }
-    if (!GetReply(device, FLYDIGI_V2_GET_INFO_COMMAND, data, sizeof(data))) {
-        return SDL_SetError("Couldn't get controller info");
-    }
-
-    // Check the firmware version
-    Uint16 min_firmware_version;
-    ctx->firmware_version = LOAD16(data[16], data[15]);
-    switch (device->product_id) {
-    case USB_PRODUCT_FLYDIGI_V2_APEX:
-        // Minimum supported firmware version, Apex 5
-        min_firmware_version = 0x7031;
-        break;
-    case USB_PRODUCT_FLYDIGI_V2_VADER:
-        // Minimum supported firmware version, Vader 5 Pro
-        min_firmware_version = 0x7141;
-        break;
-    default:
-        // Unknown product, presumably this version is okay?
-        min_firmware_version = 0;
-        break;
-    }
-    if (ctx->firmware_version < min_firmware_version) {
-        return SDL_SetError("Unsupported firmware version");
-    }
-
-    switch (data[6]) {
-    case 1:
-        // Wired connection
-        ctx->wireless = false;
-        break;
-    case 2:
-        // Wireless connection
-        ctx->wireless = true;
-        break;
-    default:
-        break;
-    }
-    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;
@@ -478,6 +277,226 @@ static void HIDAPI_DriverFlydigi_UpdateDeviceIdentity(SDL_HIDAPI_Device *device)
     }
 }
 
+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;
+
+    // Detecting the Vader 2 can take over 1000 read retries, so be generous here
+    for (int attempt = 0; ctx->deviceID == 0 && attempt < 30; ++attempt) {
+        const Uint8 request[] = { FLYDIGI_V1_CMD_REPORT_ID, FLYDIGI_V1_GET_INFO_COMMAND, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+        // This write will occasionally return -1, so ignore failure here and try again
+        (void)SDL_hid_write(device->dev, request, sizeof(request));
+
+        // Read the reply
+        for (int i = 0; i < 100; ++i) {
+            SDL_Delay(1);
+
+            Uint8 data[USB_PACKET_LENGTH];
+            int size = SDL_hid_read_timeout(device->dev, data, sizeof(data), 0);
+            if (size < 0) {
+                break;
+            }
+            if (size == 0) {
+                continue;
+            }
+
+#ifdef DEBUG_FLYDIGI_PROTOCOL
+            HIDAPI_DumpPacket("Flydigi packet: size = %d", data, size);
+#endif
+            if (size == 32 && data[15] == 236) {
+                ctx->deviceID = data[3];
+                ctx->firmware_version = LOAD16(data[9], data[10]);
+
+                char serial[9];
+                (void)SDL_snprintf(serial, sizeof(serial), "%.2x%.2x%.2x%.2x", data[5], data[6], data[7], data[8]);
+                HIDAPI_SetDeviceSerial(device, serial);
+
+                // The Vader 2 with firmware 6.0.4.9 doesn't report the connection state
+                if (ctx->firmware_version >= 0x6400) {
+                    switch (data[13]) {
+                    case 0:
+                        // Wireless connection
+                        ctx->wireless = true;
+                        break;
+                    case 1:
+                        // Wired connection
+                        ctx->wireless = false;
+                        break;
+                    default:
+                        break;
+                    }
+                }
+
+                // Done!
+                break;
+            }
+        }
+    }
+
+    HIDAPI_DriverFlydigi_UpdateDeviceIdentity(device);
+
+    HIDAPI_DriverFlydigi_SetAvailable(device, true);
+
+    return true;
+}
+
+static bool GetReply(SDL_HIDAPI_Device* device, Uint8 command, Uint8* data, size_t length)
+{
+    for (int i = 0; i < 100; ++i) {
+        SDL_Delay(1);
+
+        int size = SDL_hid_read_timeout(device->dev, data, length, 0);
+        if (size < 0) {
+            break;
+        }
+        if (size == 0) {
+            continue;
+        }
+
+#ifdef DEBUG_FLYDIGI_PROTOCOL
+        HIDAPI_DumpPacket("Flydigi packet: size = %d", data, size);
+#endif
+
+        if (size == 32) {
+            if (data[1] == FLYDIGI_V2_MAGIC1 && data[2] == FLYDIGI_V2_MAGIC2) {
+                // Skip the report ID
+                SDL_memmove(&data[0], &data[1], size - 1);
+                data[size - 1] = 0;
+            }
+            if (data[0] == FLYDIGI_V2_MAGIC1 && data[1] == FLYDIGI_V2_MAGIC2 && data[2] == command) {
+                return true;
+            }
+        }
+    }
+    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 cmd[32] = {
+        FLYDIGI_V2_CMD_REPORT_ID,
+        FLYDIGI_V2_MAGIC1,
+        FLYDIGI_V2_MAGIC2,
+        FLYDIGI_V2_ACQUIRE_CONTROLLER_COMMAND,
+        23,
+        acquire ? 1 : 0,
+        '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, cmd, sizeof(cmd)) < 0) {
+        return SDL_SetError("Couldn't send acquire command");
+    }
+    return true;
+}
+
+static void HIDAPI_DriverFlydigi_HandleAcquireResponse(SDL_HIDAPI_Device *device, Uint8 *data, int size)
+{
+    if (data[5] != 1 && data[6] == 0) {
+        // Controller acquiring failed or has been disabled
+        HIDAPI_DriverFlydigi_SetAvailable(device, false);
+    }
+}
+
+static bool HIDAPI_DriverFlydigi_InitControllerV2(SDL_HIDAPI_Device *device)
+{
+    SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)device->context;
+
+    Uint8 data[USB_PACKET_LENGTH];
+    const Uint8 query_info[] = { FLYDIGI_V2_CMD_REPORT_ID, FLYDIGI_V2_MAGIC1, FLYDIGI_V2_MAGIC2, FLYDIGI_V2_GET_INFO_COMMAND, 2, 0 };
+    if (SDL_hid_write(device->dev, query_info, sizeof(query_info)) < 0) {
+        return SDL_SetError("Couldn't query controller info");
+    }
+    if (!GetReply(device, FLYDIGI_V2_GET_INFO_COMMAND, data, sizeof(data))) {
+        return SDL_SetError("Couldn't get controller info");
+    }
+
+    // Check the firmware version
+    Uint16 min_firmware_version;
+    ctx->firmware_version = LOAD16(data[16], data[15]);
+    switch (device->product_id) {
+    case USB_PRODUCT_FLYDIGI_V2_APEX:
+        // Minimum supported firmware version, Apex 5
+        min_firmware_version = 0x7031;
+        break;
+    case USB_PRODUCT_FLYDIGI_V2_VADER:
+        // Minimum supported firmware version, Vader 5 Pro
+        min_firmware_version = 0x7141;
+        break;
+    default:
+        // Unknown product, presumably this version is okay?
+        min_firmware_version = 0;
+        break;
+    }
+    if (ctx->firmware_version < min_firmware_version) {
+        return SDL_SetError("Unsupported firmware version");
+    }
+
+    switch (data[6]) {
+    case 1:
+        // Wired connection
+        ctx->wireless = false;
+        break;
+    case 2:
+        // Wireless connection
+        ctx->wireless = true;
+        break;
+    default:
+        break;
+    }
+    ctx->deviceID = data[5];
+
+    HIDAPI_DriverFlydigi_UpdateDeviceIdentity(device);
+
+    // See whether we can acquire the controller
+    SDL_HIDAPI_Flydigi_SendStatusRequest(device);
+
+    return true;
+}
+
 static bool HIDAPI_DriverFlydigi_InitDevice(SDL_HIDAPI_Device *device)
 {
     SDL_DriverFlydigi_Context *ctx = (SDL_DriverFlydigi_Context *)SDL_calloc(1, sizeof(*ctx));
@@ -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);
+        return HIDAPI_DriverFlydigi_InitControllerV1(device);
     } else {
-        initialized = HIDAPI_DriverFlydigi_InitControllerV2(device);
+        return HIDAPI_DriverFlydigi_InitControllerV2(device);
-    }
-    if (!initialized) {
-        return false;
-    }
-
-    HIDAPI_DriverFlydigi_UpdateDeviceIdentity(device);
-
-    if (ctx->available) {
-        return HIDAPI_JoystickConnected(device, NULL);
-    } else {
-        // We'll connect it once it becomes available
-        return true;
     }
 }
 
@@ -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) {
+    // The status changed, see if we can acquire the controller now
-        // We can now acquire the controller
+    SDL_HIDAPI_Flydigi_SendStatusRequest(device);
-        HIDAPI_DriverFlydigi_SetAvailable(device, true);
-    } else {
-        // We can no longer acquire the controller
-        HIDAPI_DriverFlydigi_SetAvailable(device, false);
-    }
 }
 
 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;