Implement probe device, more closely mirror OEM flag setting

firmware/libsi/include/si/device/gc_controller.h

@@ -140,4 +140,12 @@ static inline bool si_device_gc_wireless_id_fixed(struct si_device_gc_controller
 static inline void si_device_set_input_valid(struct si_device_gc_controller *device, bool valid)
 {
   device->input_valid = valid;
-}
+}
+
+/**
+ * Update the origin of the controller, based on an origin packet.
+ *
+ * @param device the device to set the wireless origin for
+ * @param origin_data pointer to the analog origin data (6 bytes)
+ */
+void si_device_gc_set_wireless_origin(struct si_device_gc_controller *device, uint8_t *origin_data);

firmware/libsi/include/si/si.h

@@ -73,6 +73,10 @@
 #define SI_CMD_GC_LONG_POLL_LEN     3
 #define SI_CMD_GC_LONG_POLL_RESP    10
 
+#define SI_CMD_GC_PROBE_DEVICE      0x4D
+#define SI_CMD_GC_PROBE_DEVICE_LEN  3
+#define SI_CMD_GC_PROBE_DEVICE_RESP 8
+
 #define SI_CMD_GC_FIX_DEVICE        0x4E
 #define SI_CMD_GC_FIX_DEVICE_LEN    3
 #define SI_CMD_GC_FIX_DEVICE_RESP   3

firmware/libsi/src/device/gc_controller.c

@@ -229,6 +229,24 @@ static int handle_long_poll(const uint8_t *command, si_callback_fn callback, voi
   return SI_CMD_GC_LONG_POLL_RESP;
 }
 
+/**
+ * Handle "probe device" commands.
+ *
+ * I'm currently unclear what this command is used for. The command always seems to return
+ * 8 bytes of zeroes, regardless of the command parameters, so that's what we do here.
+ *
+ * Command:         {0x4D, 0x??, 0x??} - 2nd and 3rd bytes seem to differ every time
+ * Response:        8 bytes of zeroes.
+ */
+static int handle_probe_device(const uint8_t *command, si_callback_fn callback, void *context)
+{
+  // Respond with 8 bytes of zeroes
+  uint8_t response[8] = {0};
+  si_write_bytes(response, SI_CMD_GC_PROBE_DEVICE_RESP, callback);
+
+  return SI_CMD_GC_PROBE_DEVICE_RESP;
+}
+
 /**
  * Handle "fix device" commands, to "fix" the receiver ID to a specific controller ID.
  *
@@ -249,7 +267,7 @@ static int handle_fix_device(const uint8_t *command, si_callback_fn callback, vo
   device->info[2] = wireless_id & 0xFF;
 
   // Update other device info flags
-  device->info[0] |= SI_WIRELESS_STATE;
+  device->info[0] |= SI_GC_STANDARD | SI_WIRELESS_STATE;
   device->info[1] |= SI_WIRELESS_FIX_ID;
 
   // Respond with the new device info
@@ -292,6 +310,7 @@ void si_device_gc_init(struct si_device_gc_controller *device, uint8_t type)
 
   // Register additional commands handled by WaveBird receivers
   if (type & SI_GC_WIRELESS) {
+    si_command_register(SI_CMD_GC_PROBE_DEVICE, SI_CMD_GC_PROBE_DEVICE_LEN, handle_probe_device, device);
     si_command_register(SI_CMD_GC_FIX_DEVICE, SI_CMD_GC_FIX_DEVICE_LEN, handle_fix_device, device);
   }
 }
@@ -307,5 +326,19 @@ void si_device_gc_set_wireless_id(struct si_device_gc_controller *device, uint16
 
   // Update other device info flags
   device->info[0] |= SI_GC_STANDARD | SI_WIRELESS_RECEIVED;
+}
+
+void si_device_gc_set_wireless_origin(struct si_device_gc_controller *device, uint8_t *origin_data)
+{
+  // Check if the origin packet is different from the last known origin
+  if (memcmp(&device->origin.stick_x, origin_data, 6) != 0) {
+    // Update the origin state
+    memcpy(&device->origin.stick_x, origin_data, 6);
+
+    // Set the "need origin" flag to true so the host knows to fetch the new origin
+    device->input.buttons.need_origin = true;
+  }
+
+  // Set the "has wireless origin" flag in the device info
   device->info[1] |= SI_WIRELESS_ORIGIN;
 }

firmware/libsi/test/test_gc_controller.c

@@ -124,7 +124,7 @@ static void test_wavebird_info_after_set_wireless_id()
   simulate_command(&device, info_command);
 
   // Test device info response includes the controller ID
-  uint8_t expected_response[] = {0xE9, 0xA0, 0xB1};
+  uint8_t expected_response[] = {0xE9, 0x80, 0xB1};
   TEST_ASSERT_EQUAL(3, response_len);
   TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3);
 }
@@ -149,33 +149,93 @@ static void test_wavebird_info_after_set_wireless_id_multiple()
   simulate_command(&device, info_command);
 
   // Test device info response includes the most recent controller ID
-  uint8_t expected_response[] = {0xE9, 0xE0, 0x2F};
+  uint8_t expected_response[] = {0xE9, 0xC0, 0x2F};
   TEST_ASSERT_EQUAL(3, response_len);
   TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3);
 }
 
 // Test that the device info response is correct when fixing the wireless ID
 static void test_wavebird_info_after_fix_device()
 {
+  uint8_t info_command[] = {SI_CMD_INFO};
+
   // Initialize as a WaveBird receiver
   struct si_device_gc_controller device;
   si_device_gc_init(&device, SI_TYPE_GC | SI_GC_WIRELESS | SI_GC_NOMOTOR);
+
+  // Send an info command
+  simulate_command(&device, info_command);
+
+  // Test device info response is as expected
+  uint8_t expected_info_response[] = {0xA8, 0x00, 0x00};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response, response_buf, 3);
+
+  // Set the wireless ID (e.g. after packet reception)
   si_device_gc_set_wireless_id(&device, 0x2B1);
 
+  // Send an info command
+  simulate_command(&device, info_command);
+
+  // Test device info response is as expected
+  uint8_t expected_info_response_2[] = {0xE9, 0x80, 0xB1};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response_2, response_buf, 3);
+
   // Send a fix device command
   uint8_t fix_device_command[] = {SI_CMD_GC_FIX_DEVICE, 0x90, 0xB1};
   simulate_command(&device, fix_device_command);
 
+  // Check fix device response is as expected
+  uint8_t expected_fix_response[] = {0xEB, 0x90, 0xB1};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_fix_response, response_buf, 3);
+
   // Send an info command
-  uint8_t info_command[] = {SI_CMD_INFO};
   simulate_command(&device, info_command);
 
   // Test device info response includes the fixed controller ID
-  uint8_t expected_response[] = {0xEB, 0xB0, 0xB1};
+  uint8_t expected_response[] = {0xEB, 0x90, 0xB1};
   TEST_ASSERT_EQUAL(3, response_len);
   TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3);
 }
 
+// Test that the device info is correct when the console fixes the wireless ID,
+// but we have not yet received a packet from the controller
+static void test_wavebird_fix_device_without_wireless_id()
+{
+  uint8_t info_command[] = {SI_CMD_INFO};
+
+  // Initialize as a WaveBird receiver
+  struct si_device_gc_controller device;
+  si_device_gc_init(&device, SI_TYPE_GC | SI_GC_WIRELESS | SI_GC_NOMOTOR);
+
+  // Send an info command
+  simulate_command(&device, info_command);
+
+  // Test device info response is as expected
+  uint8_t expected_info_response[] = {0xA8, 0x00, 0x00};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response, response_buf, 3);
+
+  // Send a fix device command
+  uint8_t fix_device_command[] = {SI_CMD_GC_FIX_DEVICE, 0x90, 0xB1};
+  simulate_command(&device, fix_device_command);
+
+  // Check fix device response is as expected
+  uint8_t expected_fix_response[] = {0xAB, 0x90, 0xB1};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_fix_response, response_buf, 3);
+
+  // Send an info command
+  simulate_command(&device, info_command);
+
+  // Test device info response includes the fixed controller ID
+  uint8_t expected_info_response_2[] = {0xAB, 0x90, 0xB1};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response_2, response_buf, 3);
+}
+
 // Test that setting wireless ID fails when the controller ID has already been fixed
 static void test_set_wireless_id_when_fixed(void)
 {
@@ -193,6 +253,56 @@ static void test_set_wireless_id_when_fixed(void)
   TEST_ASSERT_EQUAL_HEX16(0x2B1, si_device_gc_get_wireless_id(&device));
 }
 
+static void test_set_wireless_origin(void)
+{
+  uint8_t info_command[] = {SI_CMD_INFO};
+
+  // Initialize as a WaveBird receiver
+  struct si_device_gc_controller device;
+  si_device_gc_init(&device, SI_TYPE_GC | SI_GC_WIRELESS | SI_GC_NOMOTOR);
+  si_device_gc_set_wireless_id(&device, 0x2B1);
+
+  // Send an info command
+  simulate_command(&device, info_command);
+
+  // Test device info response is as expected
+  uint8_t expected_info_response[] = {0xE9, 0x80, 0xB1};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response, response_buf, 3);
+
+  // Send a fix device command
+  uint8_t fix_device_command[] = {SI_CMD_GC_FIX_DEVICE, 0x90, 0xB1};
+  simulate_command(&device, fix_device_command);
+
+  // Send an info command
+  simulate_command(&device, info_command);
+
+  // Test device info is as expected
+  uint8_t expected_info_response_2[] = {0xEB, 0x90, 0xB1};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response_2, response_buf, 3);
+
+  // Set the wireless origin
+  uint8_t origin_data[] = {0x85, 0x86, 0x87, 0x88, 0x11, 0x12};
+  si_device_gc_set_wireless_origin(&device, origin_data);
+
+  // Check the origin state is set correctly
+  TEST_ASSERT_EQUAL_UINT8(0x85, device.origin.stick_x);
+  TEST_ASSERT_EQUAL_UINT8(0x86, device.origin.stick_y);
+  TEST_ASSERT_EQUAL_UINT8(0x87, device.origin.substick_x);
+  TEST_ASSERT_EQUAL_UINT8(0x88, device.origin.substick_y);
+  TEST_ASSERT_EQUAL_UINT8(0x11, device.origin.trigger_left);
+  TEST_ASSERT_EQUAL_UINT8(0x12, device.origin.trigger_right);
+
+  // Send an info command
+  simulate_command(&device, info_command);
+
+  // Test device info response includes the origin flag
+  uint8_t expected_info_response_3[] = {0xEB, 0xB0, 0xB1};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_info_response_3, response_buf, 3);
+}
+
 void test_gc_controller(void)
 {
   Unity.TestFile = __FILE_NAME__;
@@ -205,4 +315,6 @@ void test_gc_controller(void)
   RUN_TEST(test_wavebird_info_after_set_wireless_id_multiple);
   RUN_TEST(test_wavebird_info_after_fix_device);
   RUN_TEST(test_set_wireless_id_when_fixed);
+  RUN_TEST(test_wavebird_fix_device_without_wireless_id);
+  RUN_TEST(test_set_wireless_origin);
 }

firmware/receiver/src/main.c

@@ -225,14 +225,8 @@ static void handle_wavebird_packet(const uint8_t *packet)
         wavebird_origin_get_trigger_left(message), wavebird_origin_get_trigger_right(message),
     };
 
-    // Check if the origin packet is different from the last known origin
-    if (memcmp(&si_device.origin.stick_x, new_origin, 6) != 0) {
-      // Update the origin state
-      memcpy(&si_device.origin.stick_x, new_origin, 6);
-
-      // Set the "need origin" flag to true so the host knows to fetch the new origin
-      si_device.input.buttons.need_origin = true;
-    }
+    // Update the origin state in the SI device
+    si_device_gc_set_wireless_origin(&si_device, new_origin);
   }
 }