InputCommon: Add real Wii Remote support to ControllerInterface. Add option to connect additional Wii Remotes.

Source/Core/Core/ConfigManager.cpp

@@ -234,6 +234,7 @@ void SConfig::SaveCoreSettings(IniFile& ini)
   core->Set("WiiKeyboard", m_WiiKeyboard);
   core->Set("WiimoteContinuousScanning", m_WiimoteContinuousScanning);
   core->Set("WiimoteEnableSpeaker", m_WiimoteEnableSpeaker);
+  core->Set("WiimoteControllerInterface", connect_wiimotes_for_ciface);
   core->Set("RunCompareServer", bRunCompareServer);
   core->Set("RunCompareClient", bRunCompareClient);
   core->Set("MMU", bMMU);
@@ -511,6 +512,7 @@ void SConfig::LoadCoreSettings(IniFile& ini)
   core->Get("WiiKeyboard", &m_WiiKeyboard, false);
   core->Get("WiimoteContinuousScanning", &m_WiimoteContinuousScanning, false);
   core->Get("WiimoteEnableSpeaker", &m_WiimoteEnableSpeaker, false);
+  core->Get("WiimoteControllerInterface", &connect_wiimotes_for_ciface, false);
   core->Get("RunCompareServer", &bRunCompareServer, false);
   core->Get("RunCompareClient", &bRunCompareClient, false);
   core->Get("MMU", &bMMU, bMMU);

Source/Core/Core/ConfigManager.h

@@ -72,6 +72,7 @@ struct SConfig
   bool m_WiiKeyboard;
   bool m_WiimoteContinuousScanning;
   bool m_WiimoteEnableSpeaker;
+  bool connect_wiimotes_for_ciface;
 
   // ISO folder
   std::vector<std::string> m_ISOFolder;

Source/Core/Core/HW/WiimoteEmu/Camera.cpp

@@ -60,14 +60,6 @@ void CameraLogic::Update(const Common::Matrix44& transform)
   using Common::Vec3;
   using Common::Vec4;
 
-  constexpr int CAMERA_WIDTH = 1024;
-  constexpr int CAMERA_HEIGHT = 768;
-
-  // Wiibrew claims the camera FOV is about 33 deg by 23 deg.
-  // Unconfirmed but it seems to work well enough.
-  constexpr int CAMERA_FOV_X_DEG = 33;
-  constexpr int CAMERA_FOV_Y_DEG = 23;
-
   constexpr auto CAMERA_FOV_Y = float(CAMERA_FOV_Y_DEG * MathUtil::TAU / 360);
   constexpr auto CAMERA_ASPECT_RATIO = float(CAMERA_FOV_X_DEG) / CAMERA_FOV_Y_DEG;
 
@@ -112,12 +104,12 @@ void CameraLogic::Update(const Common::Matrix44& transform)
       if (point.z > 0)
       {
         // FYI: Casting down vs. rounding seems to produce more symmetrical output.
-        const auto x = s32((1 - point.x / point.w) * CAMERA_WIDTH / 2);
-        const auto y = s32((1 - point.y / point.w) * CAMERA_HEIGHT / 2);
+        const auto x = s32((1 - point.x / point.w) * CAMERA_RES_X / 2);
+        const auto y = s32((1 - point.y / point.w) * CAMERA_RES_Y / 2);
 
         const auto point_size = std::lround(MAX_POINT_SIZE / point.w / 2);
 
-        if (x >= 0 && y >= 0 && x < CAMERA_WIDTH && y < CAMERA_HEIGHT)
+        if (x >= 0 && y >= 0 && x < CAMERA_RES_X && y < CAMERA_RES_Y)
           return CameraPoint{u16(x), u16(y), u8(point_size)};
       }
 
@@ -165,7 +157,7 @@ void CameraLogic::Update(const Common::Matrix44& transform)
       for (std::size_t i = 0; i != camera_points.size(); ++i)
       {
         const auto& p = camera_points[i];
-        if (p.x < CAMERA_WIDTH)
+        if (p.x < CAMERA_RES_X)
         {
           IRExtended irdata = {};
 
@@ -186,7 +178,7 @@ void CameraLogic::Update(const Common::Matrix44& transform)
       for (std::size_t i = 0; i != camera_points.size(); ++i)
       {
         const auto& p = camera_points[i];
-        if (p.x < CAMERA_WIDTH)
+        if (p.x < CAMERA_RES_X)
         {
           IRFull irdata = {};
 
@@ -203,8 +195,8 @@ void CameraLogic::Update(const Common::Matrix44& transform)
 
           irdata.xmin = std::max(p.x - p.size, 0);
           irdata.ymin = std::max(p.y - p.size, 0);
-          irdata.xmax = std::min(p.x + p.size, CAMERA_WIDTH);
-          irdata.ymax = std::min(p.y + p.size, CAMERA_HEIGHT);
+          irdata.xmax = std::min(p.x + p.size, CAMERA_RES_X);
+          irdata.ymax = std::min(p.y + p.size, CAMERA_RES_Y);
 
           // TODO: Is this maybe MSbs of the "intensity" value?
           irdata.zero = 0;

Source/Core/Core/HW/WiimoteEmu/Camera.h

@@ -20,6 +20,8 @@ namespace WiimoteEmu
 // Four bytes for two objects. Filled with 0xFF if empty
 struct IRBasic
 {
+  using IRObject = Common::TVec2<u16>;
+
   u8 x1;
   u8 y1;
   u8 x2hi : 2;
@@ -28,6 +30,9 @@ struct IRBasic
   u8 y1hi : 2;
   u8 x2;
   u8 y2;
+
+  auto GetObject1() const { return IRObject(x1hi << 8 | x1, y1hi << 8 | y1); }
+  auto GetObject2() const { return IRObject(x2hi << 8 | x2, y2hi << 8 | y2); }
 };
 static_assert(sizeof(IRBasic) == 5, "Wrong size");
 
@@ -62,6 +67,14 @@ static_assert(sizeof(IRFull) == 9, "Wrong size");
 class CameraLogic : public I2CSlave
 {
 public:
+  static constexpr int CAMERA_RES_X = 1024;
+  static constexpr int CAMERA_RES_Y = 768;
+
+  // Wiibrew claims the camera FOV is about 33 deg by 23 deg.
+  // Unconfirmed but it seems to work well enough.
+  static constexpr int CAMERA_FOV_X_DEG = 33;
+  static constexpr int CAMERA_FOV_Y_DEG = 23;
+
   enum : u8
   {
     IR_MODE_BASIC = 1,

Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp

@@ -54,11 +54,15 @@ double CalculateStopDistance(double velocity, double max_accel)
   return velocity * velocity / (2 * std::copysign(max_accel, velocity));
 }
 
-// Note that 'gyroscope' is rotation of world around device.
-Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
-                                     const Common::Matrix33& gyroscope, float accel_weight)
+}  // namespace
+
+namespace WiimoteEmu
 {
-  const auto gyro_vec = gyroscope * Common::Vec3{0, 0, 1};
+Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
+                                     const Common::Vec3& accelerometer, float accel_weight,
+                                     const Common::Vec3& accelerometer_normal)
+{
+  const auto gyro_vec = gyroscope * accelerometer_normal;
   const auto normalized_accel = accelerometer.Normalized();
 
   const auto cos_angle = normalized_accel.Dot(gyro_vec);
@@ -76,10 +80,6 @@ Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
   }
 }
 
-}  // namespace
-
-namespace WiimoteEmu
-{
 IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()}
 {
 }
@@ -203,17 +203,17 @@ void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float t
   }
 }
 
-WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g,
-                                                             u16 one_g)
+WiimoteCommon::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g, u16 one_g)
 {
   const auto scaled_accel = accel * (one_g - zero_g) / float(GRAVITY_ACCELERATION);
 
   // 10-bit integers.
   constexpr long MAX_VALUE = (1 << 10) - 1;
 
-  return {u16(std::clamp(std::lround(scaled_accel.x + zero_g), 0l, MAX_VALUE)),
-          u16(std::clamp(std::lround(scaled_accel.y + zero_g), 0l, MAX_VALUE)),
-          u16(std::clamp(std::lround(scaled_accel.z + zero_g), 0l, MAX_VALUE))};
+  return WiimoteCommon::AccelData(
+      {u16(std::clamp(std::lround(scaled_accel.x + zero_g), 0l, MAX_VALUE)),
+       u16(std::clamp(std::lround(scaled_accel.y + zero_g), 0l, MAX_VALUE)),
+       u16(std::clamp(std::lround(scaled_accel.z + zero_g), 0l, MAX_VALUE))});
 }
 
 void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed)
@@ -311,28 +311,24 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
   }
 
   // Apply rotation from gyro data.
-  const auto gyro_rotation = Common::Matrix33::FromQuaternion(ang_vel->x * time_elapsed / -2,
-                                                              ang_vel->y * time_elapsed / -2,
-                                                              ang_vel->z * time_elapsed / -2, 1);
+  const auto gyro_rotation = GetMatrixFromGyroscope(*ang_vel * -1 * time_elapsed);
   state->rotation = gyro_rotation * state->rotation;
 
   // If we have some non-zero accel data use it to adjust gyro drift.
   constexpr auto ACCEL_WEIGHT = 0.02f;
   auto const accel = imu_accelerometer_group->GetState().value_or(Common::Vec3{});
   if (accel.LengthSquared())
-    state->rotation = ComplementaryFilter(accel, state->rotation, ACCEL_WEIGHT);
-
-  const auto inv_rotation = state->rotation.Inverted();
+    state->rotation = ComplementaryFilter(state->rotation, accel, ACCEL_WEIGHT);
 
   // Clamp yaw within configured bounds.
-  const auto yaw = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).x);
+  const auto yaw = GetYaw(state->rotation);
   const auto max_yaw = float(imu_ir_group->GetTotalYaw() / 2);
   auto target_yaw = std::clamp(yaw, -max_yaw, max_yaw);
 
   // Handle the "Recenter" button being pressed.
   if (imu_ir_group->controls[0]->GetState<bool>())
   {
-    state->recentered_pitch = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).z);
+    state->recentered_pitch = GetPitch(state->rotation);
     target_yaw = 0;
   }
 
@@ -390,10 +386,33 @@ Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel)
                                   axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
 }
 
+Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro)
+{
+  return Common::Matrix33::FromQuaternion(gyro.x / 2, gyro.y / 2, gyro.z / 2, 1);
+}
+
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle)
 {
   return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) *
          Common::Matrix33::RotateX(angle.x);
 }
 
+float GetPitch(const Common::Matrix33& world_rotation)
+{
+  const auto vec = world_rotation * Common::Vec3{0, 0, 1};
+  return std::atan2(vec.y, Common::Vec2(vec.x, vec.z).Length());
+}
+
+float GetRoll(const Common::Matrix33& world_rotation)
+{
+  const auto vec = world_rotation * Common::Vec3{0, 0, 1};
+  return std::atan2(vec.x, vec.z);
+}
+
+float GetYaw(const Common::Matrix33& world_rotation)
+{
+  const auto vec = world_rotation.Inverted() * Common::Vec3{0, 1, 0};
+  return std::atan2(vec.x, vec.y);
+}
+
 }  // namespace WiimoteEmu

Source/Core/Core/HW/WiimoteEmu/Dynamics.h

@@ -54,12 +54,26 @@ struct MotionState : PositionalState, RotationalState
 {
 };
 
+// Note that 'gyroscope' is rotation of world around device.
+// Alternative accelerometer_normal can be supplied to correct from non-accelerometer data.
+// e.g. Used for yaw/pitch correction with IR data.
+Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
+                                     const Common::Vec3& accelerometer, float accel_weight,
+                                     const Common::Vec3& accelerometer_normal = {0, 0, 1});
+
 // Estimate orientation from accelerometer data.
 Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel);
 
+// Get a rotation matrix from current gyro data.
+Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro);
+
 // Build a rotational matrix from euler angles.
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle);
 
+float GetPitch(const Common::Matrix33& world_rotation);
+float GetRoll(const Common::Matrix33& world_rotation);
+float GetYaw(const Common::Matrix33& world_rotation);
+
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& target,
                               const Common::Vec3& max_jerk, float time_elapsed);
 
@@ -75,7 +89,6 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
                       ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);
 
 // Convert m/s/s acceleration data to the format used by Wiimote/Nunchuk (10-bit unsigned integers).
-WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g,
-                                                             u16 one_g);
+WiimoteCommon::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g, u16 one_g);
 
 }  // namespace WiimoteEmu

Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp

@@ -236,10 +236,6 @@ void Wiimote::HandleRequestStatus(const OutputReportRequestStatus&)
   // Update status struct
   m_status.extension = m_extension_port.IsDeviceConnected();
 
-  // Based on testing, old WiiLi.org docs, and WiiUse library:
-  // Max battery level seems to be 0xc8 (decimal 200)
-  constexpr u8 MAX_BATTERY_LEVEL = 0xc8;
-
   m_status.battery = u8(std::lround(m_battery_setting.GetValue() / 100 * MAX_BATTERY_LEVEL));
 
   if (Core::WantsDeterminism())

Source/Core/Core/HW/WiimoteReal/IOdarwin.mm

@@ -257,12 +257,13 @@ void WiimoteDarwin::DisablePowerAssertionInternal()
 @implementation SearchBT
 - (void)deviceInquiryComplete:(IOBluetoothDeviceInquiry*)sender
                         error:(IOReturn)error
-                      aborted:(BOOL)aborted {
+                      aborted:(BOOL)aborted
+{
   done = true;
 }
 
-- (void)deviceInquiryDeviceFound:(IOBluetoothDeviceInquiry*)sender
-                          device:(IOBluetoothDevice*)device {
+- (void)deviceInquiryDeviceFound:(IOBluetoothDeviceInquiry*)sender device:(IOBluetoothDevice*)device
+{
   NOTICE_LOG(WIIMOTE, "Discovered Bluetooth device at %s: %s", [[device addressString] UTF8String],
              [[device name] UTF8String]);
 
@@ -274,11 +275,12 @@ void WiimoteDarwin::DisablePowerAssertionInternal()
 @implementation ConnectBT
 - (void)l2capChannelData:(IOBluetoothL2CAPChannel*)l2capChannel
                     data:(unsigned char*)data
-                  length:(NSUInteger)length {
+                  length:(NSUInteger)length
+{
   IOBluetoothDevice* device = [l2capChannel device];
   WiimoteReal::WiimoteDarwin* wm = nullptr;
 
-  std::lock_guard<std::mutex> lk(WiimoteReal::g_wiimotes_mutex);
+  std::lock_guard lk(WiimoteReal::g_wiimotes_mutex);
 
   for (int i = 0; i < MAX_WIIMOTES; i++)
   {
@@ -314,11 +316,12 @@ void WiimoteDarwin::DisablePowerAssertionInternal()
   CFRunLoopStop(CFRunLoopGetCurrent());
 }
 
-- (void)l2capChannelClosed:(IOBluetoothL2CAPChannel*)l2capChannel {
+- (void)l2capChannelClosed:(IOBluetoothL2CAPChannel*)l2capChannel
+{
   IOBluetoothDevice* device = [l2capChannel device];
   WiimoteReal::WiimoteDarwin* wm = nullptr;
 
-  std::lock_guard<std::mutex> lk(WiimoteReal::g_wiimotes_mutex);
+  std::lock_guard lk(WiimoteReal::g_wiimotes_mutex);
 
   for (int i = 0; i < MAX_WIIMOTES; i++)
   {

Source/Core/Core/HW/WiimoteReal/WiimoteReal.cpp

@@ -26,6 +26,7 @@
 #include "Core/HW/WiimoteReal/IOWin.h"
 #include "Core/HW/WiimoteReal/IOdarwin.h"
 #include "Core/HW/WiimoteReal/IOhidapi.h"
+#include "InputCommon/ControllerInterface/Wiimote/Wiimote.h"
 #include "InputCommon/InputConfig.h"
 
 #include "SFML/Network.hpp"
@@ -35,7 +36,7 @@ namespace WiimoteReal
 using namespace WiimoteCommon;
 
 static void TryToConnectBalanceBoard(std::unique_ptr<Wiimote>);
-static void TryToConnectWiimote(std::unique_ptr<Wiimote>);
+static bool TryToConnectWiimoteToSlot(std::unique_ptr<Wiimote>&, unsigned int);
 static void HandleWiimoteDisconnect(int index);
 
 static bool g_real_wiimotes_initialized = false;
@@ -45,7 +46,7 @@ static bool g_real_wiimotes_initialized = false;
 static std::unordered_set<std::string> s_known_ids;
 static std::mutex s_known_ids_mutex;
 
-std::mutex g_wiimotes_mutex;
+std::recursive_mutex g_wiimotes_mutex;
 
 // Real wii remotes assigned to a particular slot.
 std::unique_ptr<Wiimote> g_wiimotes[MAX_BBMOTES];
@@ -72,22 +73,64 @@ std::vector<WiimotePoolEntry> g_wiimote_pool;
 
 WiimoteScanner g_wiimote_scanner;
 
-static void ProcessWiimotePool()
+// Attempt to fill a real wiimote slot from the pool or by stealing from ControllerInterface.
+static void TryToFillWiimoteSlot(u32 index)
 {
-  std::lock_guard<std::mutex> wm_lk(g_wiimotes_mutex);
+  std::lock_guard lk(g_wiimotes_mutex);
 
-  for (auto it = g_wiimote_pool.begin(); it != g_wiimote_pool.end();)
+  if (g_wiimotes[index] || WiimoteCommon::GetSource(index) != WiimoteSource::Real)
+    return;
+
+  // If the pool is empty, attempt to steal from ControllerInterface.
+  if (g_wiimote_pool.empty())
   {
-    if (it->IsExpired())
-    {
-      INFO_LOG(WIIMOTE, "Removing expired wiimote pool entry.");
-      it = g_wiimote_pool.erase(it);
-    }
-    else
-    {
-      ++it;
-    }
+    ciface::Wiimote::ReleaseDevices(1);
+
+    // Still empty?
+    if (g_wiimote_pool.empty())
+      return;
   }
+
+  if (TryToConnectWiimoteToSlot(g_wiimote_pool.front().wiimote, index))
+    g_wiimote_pool.erase(g_wiimote_pool.begin());
+}
+
+// Attempts to fill enabled real wiimote slots.
+// Push/pull wiimotes to/from ControllerInterface as needed.
+void ProcessWiimotePool()
+{
+  std::lock_guard lk(g_wiimotes_mutex);
+
+  for (u32 index = 0; index != MAX_WIIMOTES; ++index)
+    TryToFillWiimoteSlot(index);
+
+  if (SConfig::GetInstance().connect_wiimotes_for_ciface)
+  {
+    for (auto& entry : g_wiimote_pool)
+      ciface::Wiimote::AddDevice(std::move(entry.wiimote));
+
+    g_wiimote_pool.clear();
+  }
+  else
+  {
+    ciface::Wiimote::ReleaseDevices();
+  }
+}
+
+void AddWiimoteToPool(std::unique_ptr<Wiimote> wiimote)
+{
+  // Our real wiimote class requires an index.
+  // Within the pool it's only going to be used for logging purposes.
+  static constexpr int POOL_WIIMOTE_INDEX = 99;
+
+  if (!wiimote->Connect(POOL_WIIMOTE_INDEX))
+  {
+    ERROR_LOG(WIIMOTE, "Failed to connect real wiimote.");
+    return;
+  }
+
+  std::lock_guard lk(g_wiimotes_mutex);
+  g_wiimote_pool.emplace_back(WiimotePoolEntry{std::move(wiimote)});
 }
 
 Wiimote::Wiimote() = default;
@@ -165,7 +208,7 @@ void Wiimote::ResetDataReporting()
   OutputReportMode rpt = {};
   rpt.mode = InputReportID::ReportCore;
   rpt.continuous = 0;
-  QueueReport(OutputReportID::ReportMode, &rpt, sizeof(rpt));
+  QueueReport(rpt);
 }
 
 void Wiimote::ClearReadQueue()
@@ -241,11 +284,11 @@ void Wiimote::InterruptChannel(const u16 channel, const void* const data, const
   else if (rpt[1] == u8(OutputReportID::SpeakerData) &&
            (!SConfig::GetInstance().m_WiimoteEnableSpeaker || !m_speaker_enable || m_speaker_mute))
   {
+    rpt.resize(3);
     // Translate undesired speaker data reports into rumble reports.
     rpt[1] = u8(OutputReportID::Rumble);
     // Keep only the rumble bit.
     rpt[2] &= 0x1;
-    rpt.resize(3);
   }
 
   WriteReport(std::move(rpt));
@@ -380,11 +423,16 @@ static bool IsDataReport(const Report& rpt)
   return rpt.size() >= 2 && rpt[1] >= u8(InputReportID::ReportCore);
 }
 
+bool Wiimote::GetNextReport(Report* report)
+{
+  return m_read_reports.Pop(*report);
+}
+
 // Returns the next report that should be sent
 Report& Wiimote::ProcessReadQueue()
 {
   // Pop through the queued reports
-  while (m_read_reports.Pop(m_last_input_report))
+  while (GetNextReport(&m_last_input_report))
   {
     if (!IsDataReport(m_last_input_report))
     {
@@ -452,26 +500,16 @@ void Wiimote::Prepare()
 
 bool Wiimote::PrepareOnThread()
 {
-  // core buttons, no continuous reporting
-  // TODO: use the structs..
-  u8 static const mode_report[] = {WR_SET_REPORT | BT_OUTPUT, u8(OutputReportID::ReportMode), 0,
+  // Set reporting mode to non-continuous core buttons and turn on rumble.
+  u8 static const mode_report[] = {WR_SET_REPORT | BT_OUTPUT, u8(OutputReportID::ReportMode), 1,
                                    u8(InputReportID::ReportCore)};
 
-  // Set the active LEDs and turn on rumble.
-  u8 static led_report[] = {WR_SET_REPORT | BT_OUTPUT, u8(OutputReportID::LED), 0};
-  led_report[2] = u8(u8(LED::LED_1) << (m_index % WIIMOTE_BALANCE_BOARD) | 0x1);
-
-  // Turn off rumble
-  u8 static const rumble_report[] = {WR_SET_REPORT | BT_OUTPUT, u8(OutputReportID::Rumble), 0};
-
-  // Request status report
+  // Request status and turn off rumble.
   u8 static const req_status_report[] = {WR_SET_REPORT | BT_OUTPUT,
                                          u8(OutputReportID::RequestStatus), 0};
-  // TODO: check for sane response?
 
-  return (IOWrite(mode_report, sizeof(mode_report)) && IOWrite(led_report, sizeof(led_report)) &&
-          (Common::SleepCurrentThread(200), IOWrite(rumble_report, sizeof(rumble_report))) &&
-          IOWrite(req_status_report, sizeof(req_status_report)));
+  return IOWrite(mode_report, sizeof(mode_report)) &&
+         (Common::SleepCurrentThread(200), IOWrite(req_status_report, sizeof(req_status_report)));
 }
 
 void Wiimote::EmuStart()
@@ -499,32 +537,20 @@ void Wiimote::EmuPause()
   DisablePowerAssertionInternal();
 }
 
-static unsigned int CalculateConnectedWiimotes()
-{
-  std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
-  unsigned int connected_wiimotes = 0;
-  for (unsigned int i = 0; i < MAX_WIIMOTES; ++i)
-    if (g_wiimotes[i])
-      ++connected_wiimotes;
-
-  return connected_wiimotes;
-}
-
 static unsigned int CalculateWantedWiimotes()
 {
-  std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
+  std::lock_guard lk(g_wiimotes_mutex);
   // Figure out how many real Wiimotes are required
   unsigned int wanted_wiimotes = 0;
   for (unsigned int i = 0; i < MAX_WIIMOTES; ++i)
     if (WiimoteCommon::GetSource(i) == WiimoteSource::Real && !g_wiimotes[i])
       ++wanted_wiimotes;
-
   return wanted_wiimotes;
 }
 
 static unsigned int CalculateWantedBB()
 {
-  std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
+  std::lock_guard lk(g_wiimotes_mutex);
   unsigned int wanted_bb = 0;
   if (WiimoteCommon::GetSource(WIIMOTE_BALANCE_BOARD) == WiimoteSource::Real &&
       !g_wiimotes[WIIMOTE_BALANCE_BOARD])
@@ -564,14 +590,63 @@ bool WiimoteScanner::IsReady() const
 
 static void CheckForDisconnectedWiimotes()
 {
-  std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
+  std::lock_guard lk(g_wiimotes_mutex);
   for (unsigned int i = 0; i < MAX_BBMOTES; ++i)
     if (g_wiimotes[i] && !g_wiimotes[i]->IsConnected())
       HandleWiimoteDisconnect(i);
 }
 
+void WiimoteScanner::PoolThreadFunc()
+{
+  Common::SetCurrentThreadName("Wiimote Pool Thread");
+
+  // Toggle between 1010 and 0101.
+  u8 led_value = 0b1010;
+
+  auto next_time = std::chrono::steady_clock::now();
+
+  while (m_scan_thread_running.IsSet())
+  {
+    std::this_thread::sleep_until(next_time);
+    next_time += std::chrono::milliseconds(250);
+
+    std::lock_guard lk(g_wiimotes_mutex);
+
+    // Remove stale pool entries.
+    for (auto it = g_wiimote_pool.begin(); it != g_wiimote_pool.end();)
+    {
+      if (!it->wiimote->IsConnected())
+      {
+        INFO_LOG(WIIMOTE, "Removing disconnected wiimote pool entry.");
+        it = g_wiimote_pool.erase(it);
+      }
+      else if (it->IsExpired())
+      {
+        INFO_LOG(WIIMOTE, "Removing expired wiimote pool entry.");
+        it = g_wiimote_pool.erase(it);
+      }
+      else
+      {
+        ++it;
+      }
+    }
+
+    // Make wiimote pool LEDs dance.
+    for (auto& wiimote : g_wiimote_pool)
+    {
+      OutputReportLeds leds = {};
+      leds.leds = led_value;
+      wiimote.wiimote->QueueReport(leds);
+    }
+
+    led_value ^= 0b1111;
+  }
+}
+
 void WiimoteScanner::ThreadFunc()
 {
+  std::thread pool_thread(&WiimoteScanner::PoolThreadFunc, this);
+
   Common::SetCurrentThreadName("Wiimote Scanning Thread");
 
   NOTICE_LOG(WIIMOTE, "Wiimote scanning thread has started.");
@@ -594,22 +669,28 @@ void WiimoteScanner::ThreadFunc()
   {
     m_scan_mode_changed_event.WaitFor(std::chrono::milliseconds(500));
 
-    ProcessWiimotePool();
+    // Does stuff needed to detect disconnects on Windows
+    for (const auto& backend : m_backends)
+      backend->Update();
 
     CheckForDisconnectedWiimotes();
 
     if (m_scan_mode.load() == WiimoteScanMode::DO_NOT_SCAN)
       continue;
 
-    if (!g_real_wiimotes_initialized)
-      continue;
+    // If we don't want Wiimotes in ControllerInterface, we may not need them at all.
+    if (!SConfig::GetInstance().connect_wiimotes_for_ciface)
+    {
+      // We don't want any remotes in passthrough mode or running in GC mode.
+      const bool core_running = Core::GetState() != Core::State::Uninitialized;
+      if (SConfig::GetInstance().m_bt_passthrough_enabled ||
+          (core_running && !SConfig::GetInstance().bWii))
+        continue;
 
-    // Does stuff needed to detect disconnects on Windows
-    for (const auto& backend : m_backends)
-      backend->Update();
-
-    if (0 == CalculateWantedWiimotes() && 0 == CalculateWantedBB())
-      continue;
+      // We don't want any remotes if we already connected everything we need.
+      if (0 == CalculateWantedWiimotes() && 0 == CalculateWantedBB())
+        continue;
+    }
 
     for (const auto& backend : m_backends)
     {
@@ -617,7 +698,7 @@ void WiimoteScanner::ThreadFunc()
       Wiimote* found_board = nullptr;
       backend->FindWiimotes(found_wiimotes, found_board);
       {
-        std::lock_guard<std::mutex> wm_lk(g_wiimotes_mutex);
+        std::unique_lock wm_lk(g_wiimotes_mutex);
 
         for (auto* wiimote : found_wiimotes)
         {
@@ -626,7 +707,8 @@ void WiimoteScanner::ThreadFunc()
             s_known_ids.insert(wiimote->GetId());
           }
 
-          TryToConnectWiimote(std::unique_ptr<Wiimote>(wiimote));
+          AddWiimoteToPool(std::unique_ptr<Wiimote>(wiimote));
+          ProcessWiimotePool();
         }
 
         if (found_board)
@@ -641,32 +723,32 @@ void WiimoteScanner::ThreadFunc()
       }
     }
 
-    if (m_scan_mode.load() == WiimoteScanMode::SCAN_ONCE)
-      m_scan_mode.store(WiimoteScanMode::DO_NOT_SCAN);
+    // Stop scanning if not in continous mode.
+    auto scan_mode = WiimoteScanMode::SCAN_ONCE;
+    m_scan_mode.compare_exchange_strong(scan_mode, WiimoteScanMode::DO_NOT_SCAN);
   }
 
   {
     std::lock_guard<std::mutex> lg(m_backends_mutex);
     m_backends.clear();
   }
+
+  pool_thread.join();
+
   NOTICE_LOG(WIIMOTE, "Wiimote scanning thread has stopped.");
 }
 
 bool Wiimote::Connect(int index)
 {
   m_index = index;
-  m_need_prepare.Set();
 
   if (!m_run_thread.IsSet())
   {
+    m_need_prepare.Set();
     m_run_thread.Set();
     StartThread();
     m_thread_ready_event.Wait();
   }
-  else
-  {
-    IOWakeup();
-  }
 
   return IsConnected();
 }
@@ -729,6 +811,11 @@ int Wiimote::GetIndex() const
   return m_index;
 }
 
+void Wiimote::SetChannel(u16 channel)
+{
+  m_channel = channel;
+}
+
 void LoadSettings()
 {
   std::string ini_filename = File::GetUserPath(D_CONFIG_IDX) + WIIMOTE_INI_NAME ".ini";
@@ -763,8 +850,7 @@ void Initialize(::Wiimote::InitializeMode init_mode)
     g_wiimote_scanner.StartThread();
   }
 
-  if (SConfig::GetInstance().m_WiimoteContinuousScanning &&
-      !SConfig::GetInstance().m_bt_passthrough_enabled)
+  if (SConfig::GetInstance().m_WiimoteContinuousScanning)
     g_wiimote_scanner.SetScanMode(WiimoteScanMode::CONTINUOUSLY_SCAN);
   else
     g_wiimote_scanner.SetScanMode(WiimoteScanMode::DO_NOT_SCAN);
@@ -774,7 +860,7 @@ void Initialize(::Wiimote::InitializeMode init_mode)
   {
     int timeout = 100;
     g_wiimote_scanner.SetScanMode(WiimoteScanMode::SCAN_ONCE);
-    while (CalculateWantedWiimotes() > CalculateConnectedWiimotes() && timeout)
+    while (CalculateWantedWiimotes() && timeout)
     {
       Common::SleepCurrentThread(100);
       timeout--;
@@ -805,9 +891,13 @@ void Shutdown()
 
   NOTICE_LOG(WIIMOTE, "WiimoteReal::Shutdown");
 
-  std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
+  std::lock_guard lk(g_wiimotes_mutex);
   for (unsigned int i = 0; i < MAX_BBMOTES; ++i)
     HandleWiimoteDisconnect(i);
+
+  // Release remotes from ControllerInterface and empty the pool.
+  ciface::Wiimote::ReleaseDevices();
+  g_wiimote_pool.clear();
 }
 
 void Resume()
@@ -836,6 +926,13 @@ static bool TryToConnectWiimoteToSlot(std::unique_ptr<Wiimote>& wm, unsigned int
     return false;
   }
 
+  wm->Prepare();
+
+  // Set LEDs.
+  OutputReportLeds led_report = {};
+  led_report.leds = u8(1 << (i % WIIMOTE_BALANCE_BOARD));
+  wm->QueueReport(led_report);
+
   g_wiimotes[i] = std::move(wm);
   Core::RunAsCPUThread([i] { ::Wiimote::Connect(i, true); });
 
@@ -844,22 +941,6 @@ static bool TryToConnectWiimoteToSlot(std::unique_ptr<Wiimote>& wm, unsigned int
   return true;
 }
 
-static void TryToConnectWiimote(std::unique_ptr<Wiimote> wm)
-{
-  for (unsigned int i = 0; i < MAX_WIIMOTES; ++i)
-  {
-    if (TryToConnectWiimoteToSlot(wm, i))
-      return;
-  }
-
-  INFO_LOG(WIIMOTE, "No open slot for real wiimote, adding it to the pool.");
-  wm->Connect(0);
-  // Turn on LED 1 and 4 to make it apparant this remote is in the pool.
-  const u8 led_value = u8(LED::LED_1) | u8(LED::LED_4);
-  wm->QueueReport(OutputReportID::LED, &led_value, 1);
-  g_wiimote_pool.emplace_back(WiimotePoolEntry{std::move(wm)});
-}
-
 static void TryToConnectBalanceBoard(std::unique_ptr<Wiimote> wm)
 {
   if (TryToConnectWiimoteToSlot(wm, WIIMOTE_BALANCE_BOARD))
@@ -882,14 +963,14 @@ void Refresh()
 
 void InterruptChannel(int wiimote_number, u16 channel_id, const void* data, u32 size)
 {
-  std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
+  std::lock_guard lk(g_wiimotes_mutex);
   if (g_wiimotes[wiimote_number])
     g_wiimotes[wiimote_number]->InterruptChannel(channel_id, data, size);
 }
 
 void ControlChannel(int wiimote_number, u16 channel_id, const void* data, u32 size)
 {
-  std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
+  std::lock_guard lk(g_wiimotes_mutex);
   if (g_wiimotes[wiimote_number])
     g_wiimotes[wiimote_number]->ControlChannel(channel_id, data, size);
 }
@@ -946,25 +1027,17 @@ bool IsNewWiimote(const std::string& identifier)
 
 void HandleWiimoteSourceChange(unsigned int index)
 {
-  std::lock_guard<std::mutex> wm_lk(g_wiimotes_mutex);
+  std::lock_guard wm_lk(g_wiimotes_mutex);
 
-  if (WiimoteCommon::GetSource(index) != WiimoteSource::Real)
-  {
-    if (auto removed_wiimote = std::move(g_wiimotes[index]))
-    {
-      removed_wiimote->EmuStop();
-      // Try to use this removed wiimote in another slot.
-      TryToConnectWiimote(std::move(removed_wiimote));
-    }
-  }
-  else if (WiimoteCommon::GetSource(index) == WiimoteSource::Real)
-  {
-    // Try to fill this slot from the pool.
-    if (!g_wiimote_pool.empty())
-    {
-      if (TryToConnectWiimoteToSlot(g_wiimote_pool.front().wiimote, index))
-        g_wiimote_pool.erase(g_wiimote_pool.begin());
-    }
-  }
+  if (auto removed_wiimote = std::move(g_wiimotes[index]))
+    AddWiimoteToPool(std::move(removed_wiimote));
+
+  ProcessWiimotePool();
 }
-};  // namespace WiimoteReal
+
+void HandleWiimotesInControllerInterfaceSettingChange()
+{
+  ProcessWiimotePool();
+}
+
+}  // namespace WiimoteReal

Source/Core/Core/HW/WiimoteReal/WiimoteReal.h

@@ -65,6 +65,7 @@ public:
   void Update();
   bool CheckForButtonPress();
 
+  bool GetNextReport(Report* report);
   Report& ProcessReadQueue();
 
   void Read();
@@ -101,8 +102,16 @@ public:
 
   void QueueReport(WiimoteCommon::OutputReportID rpt_id, const void* data, unsigned int size);
 
+  template <typename T>
+  void QueueReport(const T& report)
+  {
+    QueueReport(report.REPORT_ID, &report, sizeof(report));
+  }
+
   int GetIndex() const;
 
+  void SetChannel(u16 channel);
+
 protected:
   Wiimote();
 
@@ -173,6 +182,7 @@ public:
 
 private:
   void ThreadFunc();
+  void PoolThreadFunc();
 
   std::vector<std::unique_ptr<WiimoteScannerBackend>> m_backends;
   mutable std::mutex m_backends_mutex;
@@ -183,10 +193,13 @@ private:
   std::atomic<WiimoteScanMode> m_scan_mode{WiimoteScanMode::DO_NOT_SCAN};
 };
 
-extern std::mutex g_wiimotes_mutex;
+// Mutex is recursive as ControllerInterface may call AddWiimoteToPool within ProcessWiimotePool.
+extern std::recursive_mutex g_wiimotes_mutex;
 extern WiimoteScanner g_wiimote_scanner;
 extern std::unique_ptr<Wiimote> g_wiimotes[MAX_BBMOTES];
 
+void AddWiimoteToPool(std::unique_ptr<Wiimote>);
+
 void InterruptChannel(int wiimote_number, u16 channel_id, const void* data, u32 size);
 void ControlChannel(int wiimote_number, u16 channel_id, const void* data, u32 size);
 void Update(int wiimote_number);
@@ -202,4 +215,7 @@ void HandleWiimoteSourceChange(unsigned int wiimote_number);
 void InitAdapterClass();
 #endif
 
+void HandleWiimotesInControllerInterfaceSettingChange();
+void ProcessWiimotePool();
+
 }  // namespace WiimoteReal