diff --git a/src/input/api/Wiimote/WiimoteControllerProvider.cpp b/src/input/api/Wiimote/WiimoteControllerProvider.cpp index 33b02262..731e6742 100644 --- a/src/input/api/Wiimote/WiimoteControllerProvider.cpp +++ b/src/input/api/Wiimote/WiimoteControllerProvider.cpp @@ -1,9 +1,9 @@ #include "input/api/Wiimote/WiimoteControllerProvider.h" #include "input/api/Wiimote/NativeWiimoteController.h" #include "input/api/Wiimote/WiimoteMessages.h" - +#if HAS_HIDAPI #include "input/api/Wiimote/hidapi/HidapiWiimote.h" - +#endif #if BOOST_OS_LINUX #include "input/api/Wiimote/l2cap/L2CapWiimote.h" #endif @@ -16,6 +16,7 @@ WiimoteControllerProvider::WiimoteControllerProvider() { m_reader_thread = std::thread(&WiimoteControllerProvider::reader_thread, this); m_writer_thread = std::thread(&WiimoteControllerProvider::writer_thread, this); + m_connectionThread = std::jthread(&WiimoteControllerProvider::connectionThread, this); } WiimoteControllerProvider::~WiimoteControllerProvider() @@ -30,48 +31,40 @@ WiimoteControllerProvider::~WiimoteControllerProvider() std::vector> WiimoteControllerProvider::get_controllers() { + m_connectedDeviceMutex.lock(); + auto devices = m_connectedDevices; + m_connectedDeviceMutex.unlock(); + std::scoped_lock lock(m_device_mutex); - std::queue disconnected_wiimote_indices; - for (auto i{0u}; i < m_wiimotes.size(); ++i){ - if (!(m_wiimotes[i].connected = m_wiimotes[i].device->write_data({kStatusRequest, 0x00}))){ - disconnected_wiimote_indices.push(i); - } - } - - const auto valid_new_device = [&](std::shared_ptr & device) { - const auto writeable = device->write_data({kStatusRequest, 0x00}); - const auto not_already_connected = - std::none_of(m_wiimotes.cbegin(), m_wiimotes.cend(), - [device](const auto& it) { - return (*it.device == *device) && it.connected; - }); - return writeable && not_already_connected; - }; - - auto devices = HidapiWiimote::get_devices(); -#if BOOST_OS_LINUX - const auto& l2capDevices = L2CapWiimote::get_devices(); - std::ranges::copy(l2capDevices, std::back_inserter(devices)); -#endif for (auto& device : devices) { - if (!valid_new_device(device)) + const auto writeable = device->write_data({kStatusRequest, 0x00}); + if (!writeable) continue; - // Replace disconnected wiimotes - if (!disconnected_wiimote_indices.empty()){ - const auto idx = disconnected_wiimote_indices.front(); - disconnected_wiimote_indices.pop(); - m_wiimotes.replace(idx, std::make_unique(device)); - } - // Otherwise add them - else { - m_wiimotes.push_back(std::make_unique(device)); - } + bool isDuplicate = false; + ssize_t lowestReplaceableIndex = -1; + for (ssize_t i = m_wiimotes.size() - 1; i >= 0; --i) + { + const auto& wiimote = m_wiimotes[i]; + if (wiimote.device && *wiimote.device == *device) + { + isDuplicate = true; + break; + } + lowestReplaceableIndex = i; + } + if (isDuplicate) + continue; + if (lowestReplaceableIndex != -1) + m_wiimotes.replace(lowestReplaceableIndex, std::make_unique(device)); + else + m_wiimotes.push_back(std::make_unique(device)); } std::vector> result; + result.reserve(m_wiimotes.size()); for (size_t i = 0; i < m_wiimotes.size(); ++i) { result.emplace_back(std::make_shared(i)); @@ -83,7 +76,7 @@ std::vector> WiimoteControllerProvider::get_cont bool WiimoteControllerProvider::is_connected(size_t index) { std::shared_lock lock(m_device_mutex); - return index < m_wiimotes.size() && m_wiimotes[index].connected; + return index < m_wiimotes.size() && m_wiimotes[index].device; } bool WiimoteControllerProvider::is_registered_device(size_t index) @@ -150,6 +143,30 @@ WiimoteControllerProvider::WiimoteState WiimoteControllerProvider::get_state(siz return {}; } +void WiimoteControllerProvider::connectionThread(std::stop_token stopToken) +{ + SetThreadName("Wiimote-connect"); + while (!stopToken.stop_requested()) + { + std::vector devices; +#if HAS_HIDAPI + const auto& hidDevices = HidapiWiimote::get_devices(); + std::ranges::move(hidDevices, std::back_inserter(devices)); +#endif +#if BOOST_OS_LINUX + const auto& l2capDevices = L2CapWiimote::get_devices(); + std::ranges::move(l2capDevices, std::back_inserter(devices)); +#endif + { + std::scoped_lock lock(m_connectedDeviceMutex); + m_connectedDevices.clear(); + std::ranges::move(devices, std::back_inserter(m_connectedDevices)); + } + std::this_thread::sleep_for(std::chrono::seconds(2)); + } +} + + void WiimoteControllerProvider::reader_thread() { SetThreadName("Wiimote-reader"); @@ -157,7 +174,7 @@ void WiimoteControllerProvider::reader_thread() while (m_running.load(std::memory_order_relaxed)) { const auto now = std::chrono::steady_clock::now(); - if (std::chrono::duration_cast(now - lastCheck) > std::chrono::seconds(2)) + if (std::chrono::duration_cast(now - lastCheck) > std::chrono::milliseconds(500)) { // check for new connected wiimotes get_controllers(); @@ -169,11 +186,16 @@ void WiimoteControllerProvider::reader_thread() for (size_t index = 0; index < m_wiimotes.size(); ++index) { auto& wiimote = m_wiimotes[index]; - if (!wiimote.connected) + if (!wiimote.device) continue; const auto read_data = wiimote.device->read_data(); - if (!read_data || read_data->empty()) + if (!read_data) + { + wiimote.device.reset(); + continue; + } + if (read_data->empty()) continue; receivedAnyPacket = true; @@ -930,18 +952,15 @@ void WiimoteControllerProvider::writer_thread() if (index != (size_t)-1 && !data.empty()) { - if (m_wiimotes[index].rumble) + auto& wiimote = m_wiimotes[index]; + if (wiimote.rumble) data[1] |= 1; - - m_wiimotes[index].connected = m_wiimotes[index].device->write_data(data); - if (m_wiimotes[index].connected) - { - m_wiimotes[index].data_ts = std::chrono::high_resolution_clock::now(); - } + if (!wiimote.device->write_data(data)) + wiimote.device.reset(); + if (wiimote.device) + wiimote.data_ts = std::chrono::high_resolution_clock::now(); else - { - m_wiimotes[index].rumble = false; - } + wiimote.rumble = false; } device_lock.unlock(); diff --git a/src/input/api/Wiimote/WiimoteControllerProvider.h b/src/input/api/Wiimote/WiimoteControllerProvider.h index 7629b641..9b191bdb 100644 --- a/src/input/api/Wiimote/WiimoteControllerProvider.h +++ b/src/input/api/Wiimote/WiimoteControllerProvider.h @@ -77,16 +77,17 @@ public: private: std::atomic_bool m_running = false; std::thread m_reader_thread, m_writer_thread; - std::shared_mutex m_device_mutex; + std::jthread m_connectionThread; + std::vector m_connectedDevices; + std::mutex m_connectedDeviceMutex; struct Wiimote { Wiimote(WiimoteDevicePtr device) : device(std::move(device)) {} WiimoteDevicePtr device; - std::atomic_bool connected = true; std::atomic_bool rumble = false; std::shared_mutex mutex; @@ -103,6 +104,7 @@ private: void reader_thread(); void writer_thread(); + void connectionThread(std::stop_token); void calibrate(size_t index); IRMode set_ir_camera(size_t index, bool state); diff --git a/src/input/api/Wiimote/l2cap/L2CapWiimote.cpp b/src/input/api/Wiimote/l2cap/L2CapWiimote.cpp index 8a199236..3a33319f 100644 --- a/src/input/api/Wiimote/l2cap/L2CapWiimote.cpp +++ b/src/input/api/Wiimote/l2cap/L2CapWiimote.cpp @@ -7,8 +7,14 @@ namespace { bool AttemptConnect(int& sockFd, const sockaddr_l2& addr) { + auto res = connect(sockFd, reinterpret_cast(&addr), + sizeof(sockaddr_l2)); + if (res == 0) + return true; + if (res != ECONNREFUSED) + return false; return connect(sockFd, reinterpret_cast(&addr), - sizeof(sockaddr_l2)) == 0; + sizeof(sockaddr_l2)) == 0; } bool AttemptSetNonBlock(int& sockFd) { @@ -105,9 +111,11 @@ std::optional> L2CapWiimote::read_data() uint8 buffer[23]; const auto nBytes = recv(m_sendFd, buffer, 23, 0); + if (nBytes < 0 && errno == EWOULDBLOCK) + return std::vector{}; // All incoming messages must be prefixed with 0xA1 if (nBytes < 2 || buffer[0] != 0xA1) - return {}; + return std::nullopt; return std::vector(buffer + 1, buffer + 1 + nBytes - 1); }