WiimoteReal: Add connected real Wii Remotes to a pool when a slot is …

…not available.

Source/Core/Core/HW/Wiimote.cpp

@@ -44,19 +44,7 @@ void SetSource(unsigned int index, WiimoteSource source)
     return;
   }
 
-  // TODO: Deal with removal of real remotes.
-#if 0
-  {
-    // Kill real wiimote connection (if any) (or swap to different slot)
-    std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
-    if (auto removed_wiimote = std::move(g_wiimotes[index]))
-    {
-      // See if we can use this real Wiimote in another slot.
-      // Otherwise it will be disconnected.
-      TryToConnectWiimote(std::move(removed_wiimote));
-    }
-  }
-#endif
+  WiimoteReal::HandleWiimoteSourceChange(index);
 
   // Reconnect to the emulator.
   Core::RunAsCPUThread([index, previous_source, source] {

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

@@ -47,9 +47,49 @@ static std::mutex s_known_ids_mutex;
 
 std::mutex g_wiimotes_mutex;
 
+// Real wii remotes assigned to a particular slot.
 std::unique_ptr<Wiimote> g_wiimotes[MAX_BBMOTES];
+
+struct WiimotePoolEntry
+{
+  using Clock = std::chrono::steady_clock;
+
+  std::unique_ptr<Wiimote> wiimote;
+  Clock::time_point entry_time = Clock::now();
+
+  bool IsExpired() const
+  {
+    // Keep wii remotes in the pool for a bit before disconnecting them.
+    constexpr auto POOL_TIME = std::chrono::seconds{5};
+
+    return (Clock::now() - entry_time) > POOL_TIME;
+  }
+};
+
+// Connected wii remotes are placed here when no open slot is set to "Real".
+// They are then automatically disconnected after some time.
+std::vector<WiimotePoolEntry> g_wiimote_pool;
+
 WiimoteScanner g_wiimote_scanner;
 
+static void ProcessWiimotePool()
+{
+  std::lock_guard<std::mutex> wm_lk(g_wiimotes_mutex);
+
+  for (auto it = g_wiimote_pool.begin(); it != g_wiimote_pool.end();)
+  {
+    if (it->IsExpired())
+    {
+      NOTICE_LOG(WIIMOTE, "Removing expired wiimote pool entry.");
+      it = g_wiimote_pool.erase(it);
+    }
+    else
+    {
+      ++it;
+    }
+  }
+}
+
 Wiimote::Wiimote() = default;
 
 void Wiimote::Shutdown()
@@ -289,7 +329,8 @@ bool Wiimote::IsBalanceBoard()
 
     switch (InputReportID(buf[1]))
     {
-    case InputReportID::Status: {
+    case InputReportID::Status:
+    {
       const auto* status = reinterpret_cast<InputReportStatus*>(&buf[2]);
       // A Balance Board has a Balance Board extension.
       if (!status->extension)
@@ -306,7 +347,8 @@ bool Wiimote::IsBalanceBoard()
       ret = IOWrite(identify_ext_rpt, sizeof(identify_ext_rpt));
       break;
     }
-    case InputReportID::ReadDataReply: {
+    case InputReportID::ReadDataReply:
+    {
       const auto* reply = reinterpret_cast<InputReportReadDataReply*>(&buf[2]);
       if (Common::swap16(reply->address) != 0x00fe)
       {
@@ -317,7 +359,8 @@ bool Wiimote::IsBalanceBoard()
       // A Balance Board ext can be identified by checking for 0x0402.
       return reply->data[0] == 0x04 && reply->data[1] == 0x02;
     }
-    case InputReportID::Ack: {
+    case InputReportID::Ack:
+    {
       const auto* ack = reinterpret_cast<InputReportAck*>(&buf[2]);
       if (ack->rpt_id == OutputReportID::ReadData && ack->error_code != ErrorCode::Success)
       {
@@ -473,7 +516,7 @@ static unsigned int CalculateWantedWiimotes()
   // Figure out how many real Wiimotes are required
   unsigned int wanted_wiimotes = 0;
   for (unsigned int i = 0; i < MAX_WIIMOTES; ++i)
-    if (WiimoteSource::Real == ::WiimoteCommon::GetSource(i) && !g_wiimotes[i])
+    if (WiimoteSource::Real == WiimoteCommon::GetSource(i) && !g_wiimotes[i])
       ++wanted_wiimotes;
 
   return wanted_wiimotes;
@@ -483,7 +526,7 @@ static unsigned int CalculateWantedBB()
 {
   std::lock_guard<std::mutex> lk(g_wiimotes_mutex);
   unsigned int wanted_bb = 0;
-  if (WiimoteSource::Real == ::WiimoteCommon::GetSource(WIIMOTE_BALANCE_BOARD) &&
+  if (WiimoteSource::Real == WiimoteCommon::GetSource(WIIMOTE_BALANCE_BOARD) &&
       !g_wiimotes[WIIMOTE_BALANCE_BOARD])
     ++wanted_bb;
   return wanted_bb;
@@ -551,6 +594,8 @@ void WiimoteScanner::ThreadFunc()
   {
     m_scan_mode_changed_event.WaitFor(std::chrono::milliseconds(500));
 
+    ProcessWiimotePool();
+
     CheckForDisconnectedWiimotes();
 
     if (m_scan_mode.load() == WiimoteScanMode::DO_NOT_SCAN)
@@ -614,9 +659,15 @@ bool Wiimote::Connect(int index)
 
   if (!m_run_thread.IsSet())
   {
+    m_run_thread.Set();
     StartThread();
     m_thread_ready_event.Wait();
   }
+  else
+  {
+    IOWakeup();
+  }
+
   return IsConnected();
 }
 
@@ -647,7 +698,6 @@ void Wiimote::ThreadFunc()
   }
 
   m_thread_ready_event.Set();
-  m_run_thread.Set();
 
   if (!ok)
   {
@@ -802,7 +852,12 @@ static void TryToConnectWiimote(std::unique_ptr<Wiimote> wm)
       return;
   }
 
-  NOTICE_LOG(WIIMOTE, "No open slot for real wiimote.");
+  NOTICE_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)
@@ -889,4 +944,26 @@ bool IsNewWiimote(const std::string& identifier)
   return s_known_ids.count(identifier) == 0;
 }
 
+void HandleWiimoteSourceChange(unsigned int index)
+{
+  std::lock_guard<std::mutex> wm_lk(g_wiimotes_mutex);
+
+  if (WiimoteCommon::GetSource(index) != WiimoteSource::Real)
+  {
+    if (auto removed_wiimote = std::move(g_wiimotes[index]))
+    {
+      // 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());
+    }
+  }
+}
 };  // namespace WiimoteReal

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

@@ -196,6 +196,8 @@ bool IsValidDeviceName(const std::string& name);
 bool IsBalanceBoardName(const std::string& name);
 bool IsNewWiimote(const std::string& identifier);
 
+void HandleWiimoteSourceChange(unsigned int wiimote_number);
+
 #ifdef ANDROID
 void InitAdapterClass();
 #endif

Source/Core/Core/IOS/USB/Bluetooth/BTEmu.cpp

@@ -56,7 +56,7 @@ BluetoothEmu::BluetoothEmu(Kernel& ios, const std::string& device_name)
     tmp_bd[2] = bt_dinf.active[i].bdaddr[3] = bt_dinf.registered[i].bdaddr[3] = 0x19;
     tmp_bd[1] = bt_dinf.active[i].bdaddr[4] = bt_dinf.registered[i].bdaddr[4] = 2;
     tmp_bd[0] = bt_dinf.active[i].bdaddr[5] = bt_dinf.registered[i].bdaddr[5] = 0x11;
-    
+
     const char* wm_name;
     if (i == WIIMOTE_BALANCE_BOARD)
       wm_name = "Nintendo RVL-WBC-01";
@@ -157,7 +157,8 @@ IPCCommandResult BluetoothEmu::IOCtlV(const IOCtlVRequest& request)
     break;
   }
 
-  case USB::IOCTLV_USBV0_BLKMSG: {
+  case USB::IOCTLV_USBV0_BLKMSG:
+  {
     const USB::V0BulkMessage ctrl{m_ios, request};
     switch (ctrl.endpoint)
     {
@@ -188,7 +189,8 @@ IPCCommandResult BluetoothEmu::IOCtlV(const IOCtlVRequest& request)
     break;
   }
 
-  case USB::IOCTLV_USBV0_INTRMSG: {
+  case USB::IOCTLV_USBV0_INTRMSG:
+  {
     const USB::V0IntrMessage ctrl{m_ios, request};
     if (ctrl.endpoint == HCI_EVENT)  // We are given a HCI buffer to fill
     {