Ensure need_origin flag is set correctly in both wired and wireless modes

Author: loopj

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

@@ -184,9 +184,11 @@ static inline void si_device_gc_input_valid(struct si_device_gc_controller *devi
 }
 
 /**
- * Update the origin of the controller, based on an origin packet.
+ * Update the origin of the controller.
+ *
+ * If the origin data differs from the current origin, the "need origin" flag is set.
  *
  * @param device the device to set the wireless origin for
- * @param origin_data pointer to the analog origin data (6 bytes)
+ * @param new_origin pointer to the new origin data (6 bytes)
  */
-void si_device_gc_set_wireless_origin(struct si_device_gc_controller *device, uint8_t *origin_data);
+void si_device_gc_set_origin(struct si_device_gc_controller *device, struct si_device_gc_input_state *new_origin);

firmware/libsi/src/device/gc_controller.c

@@ -68,6 +68,22 @@ static uint8_t *pack_input_state(struct si_device_gc_input_state *src, uint8_t a
   return packed_state;
 }
 
+// Set or clear the "need origin" flag in the input state and device info
+static void set_need_origin(struct si_device_gc_controller *device, bool need_origin)
+{
+  // Always set the need_origin flag in the input state
+  device->input.buttons.need_origin = need_origin;
+
+  // Also update the device info for non-wireless controllers
+  if (!si_device_gc_is_wireless(device)) {
+    if (need_origin) {
+      device->info[2] |= SI_NEED_ORIGIN;
+    } else {
+      device->info[2] &= ~SI_NEED_ORIGIN;
+    }
+  }
+}
+
 /**
  * Handle "info" commands.
  *
@@ -118,8 +134,7 @@ static int handle_short_poll(const uint8_t *command, si_complete_cb_t callback,
 
   if (!si_device_gc_is_wireless(device)) {
     // Update the origin flags
-    device->input.buttons.need_origin = (device->info[2] & SI_NEED_ORIGIN) != 0;
-    device->input.buttons.use_origin  = true;
+    device->input.buttons.use_origin = true;
 
     // Save the analog mode and motor state
     device->info[2] &= ~(SI_MOTOR_STATE_MASK | SI_ANALOG_MODE_MASK);
@@ -154,13 +169,8 @@ static int handle_read_origin(const uint8_t *command, si_complete_cb_t callback,
 {
   struct si_device_gc_controller *device = (struct si_device_gc_controller *)context;
 
-  // Tell the host it no longer needs to fetch the origin
-  if (!si_device_gc_is_wireless(device)) {
-    device->info[2] &= ~SI_NEED_ORIGIN;
-  }
-
   // Clear the "need origin" flag
-  device->input.buttons.need_origin = false;
+  set_need_origin(device, false);
 
   // Respond with the origin
   si_write_bytes((uint8_t *)(&device->origin), SI_CMD_GC_READ_ORIGIN_RESP, callback);
@@ -186,10 +196,8 @@ static int handle_calibrate(const uint8_t *command, si_complete_cb_t callback, v
   device->origin.trigger_left  = device->input.trigger_left;
   device->origin.trigger_right = device->input.trigger_right;
 
-  // Tell the host it no longer needs to fetch the origin
-  if (!si_device_gc_is_wireless(device)) {
-    device->info[2] &= ~SI_NEED_ORIGIN;
-  }
+  // Clear the "need origin" flag
+  set_need_origin(device, false);
 
   // Respond with the new origin
   si_write_bytes((uint8_t *)(&device->origin), SI_CMD_GC_CALIBRATE_RESP, callback);
@@ -213,18 +221,20 @@ static int handle_long_poll(const uint8_t *command, si_complete_cb_t callback, v
   uint8_t analog_mode = command[1] & 0x07;
   uint8_t motor_state = command[2] & 0x03;
 
-  // Update the origin flags before responding
-  device->input.buttons.need_origin = (device->info[2] & SI_NEED_ORIGIN) != 0;
-  device->input.buttons.use_origin  = true;
-
-  // Save the analog mode and motor state
   if (!si_device_gc_is_wireless(device)) {
+    // Update the origin flags
+    device->input.buttons.use_origin = true;
+
+    // Save the analog mode and motor state
     device->info[2] &= ~(SI_MOTOR_STATE_MASK | SI_ANALOG_MODE_MASK);
     device->info[2] |= motor_state << 3 | analog_mode;
   }
 
+  // If the input state is valid, use that for the response, otherwise use the origin
+  struct si_device_gc_input_state *state = device->input_valid ? &device->input : &device->origin;
+
   // Respond with the current input state
-  si_write_bytes((uint8_t *)&device->input, SI_CMD_GC_LONG_POLL_RESP, callback);
+  si_write_bytes((uint8_t *)state, SI_CMD_GC_LONG_POLL_RESP, callback);
 
   return SI_CMD_GC_LONG_POLL_RESP;
 }
@@ -296,10 +306,6 @@ void si_device_gc_init(struct si_device_gc_controller *device, uint8_t type)
   // Mark the input as valid initially
   device->input_valid = true;
 
-  // Request the origin on non-wireless controllers
-  if (!si_device_gc_is_wireless(device))
-    device->info[2] = SI_NEED_ORIGIN;
-
   // Register the SI commands handled by GameCube controllers
   si_command_register(SI_CMD_INFO, SI_CMD_INFO_LEN, handle_info, device);
   si_command_register(SI_CMD_RESET, SI_CMD_RESET_LEN, handle_reset, device);
@@ -328,15 +334,15 @@ void si_device_gc_set_wireless_id(struct si_device_gc_controller *device, uint16
   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)
+void si_device_gc_set_origin(struct si_device_gc_controller *device, struct si_device_gc_input_state *new_origin)
 {
   // Check if the origin packet is different from the last known origin
-  if (memcmp(&device->origin.stick_x, origin_data, 6) != 0) {
+  if (memcmp(&device->origin.stick_x, &new_origin->stick_x, 6) != 0) {
     // Update the origin state
-    memcpy(&device->origin.stick_x, origin_data, 6);
+    memcpy(&device->origin.stick_x, &new_origin->stick_x, 6);
 
-    // Set the "need origin" flag to true so the host knows to fetch the new origin
-    device->input.buttons.need_origin = true;
+    // Tell the host that new origin data is available
+    set_need_origin(device, true);
   }
 
   // Set the "has wireless origin" flag in the device info

firmware/libsi/test/test_gc_controller.c

@@ -40,7 +40,7 @@ static void test_gcc_info()
   simulate_command(&device, info_command);
 
   // Test device info response is as expected
-  uint8_t expected_response[] = {0x09, 0x00, 0x20};
+  uint8_t expected_response[] = {0x09, 0x00, 0x00};
   TEST_ASSERT_EQUAL(3, response_len);
   TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3);
 }
@@ -52,18 +52,37 @@ static void test_gcc_info_after_read_origin()
   struct si_device_gc_controller device;
   si_device_gc_init(&device, SI_TYPE_GC | SI_GC_STANDARD);
 
+  // Set an origin
+  struct si_device_gc_input_state new_origin = {
+      .stick_x       = 0x81,
+      .stick_y       = 0x82,
+      .substick_x    = 0x83,
+      .substick_y    = 0x84,
+      .trigger_left  = 0x11,
+      .trigger_right = 0x12,
+  };
+  si_device_gc_set_origin(&device, &new_origin);
+
+  // Send an info command
+  uint8_t info_command[] = {SI_CMD_INFO};
+  simulate_command(&device, info_command);
+
+  // Test device info response is as expected
+  uint8_t expected_response[] = {0x09, 0x00, 0x20};
+  TEST_ASSERT_EQUAL(3, response_len);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3);
+
   // Send a read origin command
   uint8_t read_origin_command[] = {SI_CMD_GC_READ_ORIGIN};
   simulate_command(&device, read_origin_command);
 
-  // Send an info command
-  uint8_t info_command[] = {SI_CMD_INFO};
+  // Send another info command
   simulate_command(&device, info_command);
 
   // Verify the "need_origin" flag is not present
-  uint8_t expected_response[] = {0x09, 0x00, 0x00};
+  uint8_t expected_response_2[] = {0x09, 0x00, 0x00};
   TEST_ASSERT_EQUAL(3, response_len);
-  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response, response_buf, 3);
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected_response_2, response_buf, 3);
 }
 
 // Test that "analog mode" and "motor state" are saved after a "poll" command
@@ -283,8 +302,15 @@ static void test_set_wireless_origin(void)
   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);
+  struct si_device_gc_input_state origin = {
+      .stick_x       = 0x85,
+      .stick_y       = 0x86,
+      .substick_x    = 0x87,
+      .substick_y    = 0x88,
+      .trigger_left  = 0x11,
+      .trigger_right = 0x12,
+  };
+  si_device_gc_set_origin(&device, &origin);
 
   // Check the origin state is set correctly
   TEST_ASSERT_EQUAL_UINT8(0x85, device.origin.stick_x);

firmware/receiver/src/main.c

@@ -218,14 +218,17 @@ static void handle_wavebird_packet(const uint8_t *packet)
     //
 
     // Copy the origin values from the packet
-    uint8_t new_origin[] = {
-        wavebird_origin_get_stick_x(message),      wavebird_origin_get_stick_y(message),
-        wavebird_origin_get_substick_x(message),   wavebird_origin_get_substick_y(message),
-        wavebird_origin_get_trigger_left(message), wavebird_origin_get_trigger_right(message),
+    struct si_device_gc_input_state new_origin = {
+        .stick_x       = wavebird_origin_get_stick_x(message),
+        .stick_y       = wavebird_origin_get_stick_y(message),
+        .substick_x    = wavebird_origin_get_substick_x(message),
+        .substick_y    = wavebird_origin_get_substick_y(message),
+        .trigger_left  = wavebird_origin_get_trigger_left(message),
+        .trigger_right = wavebird_origin_get_trigger_right(message),
     };
 
     // Update the origin state in the SI device
-    si_device_gc_set_wireless_origin(&si_device, new_origin);
+    si_device_gc_set_origin(&si_device, &new_origin);
   }
 }