Linux: Allow connecting Wiimotes via L2CAP (#1353)

src/input/CMakeLists.txt

@@ -73,6 +73,11 @@ if (ENABLE_WIIMOTE)
 			api/Wiimote/hidapi/HidapiWiimote.cpp
 			api/Wiimote/hidapi/HidapiWiimote.h
 	)
+	if (UNIX AND NOT APPLE)
+		target_sources(CemuInput PRIVATE
+			api/Wiimote/l2cap/L2CapWiimote.cpp
+			api/Wiimote/l2cap/L2CapWiimote.h)
+	endif()
 endif ()
 
 
@@ -97,3 +102,8 @@ endif()
 if (ENABLE_WXWIDGETS)
 	target_link_libraries(CemuInput PRIVATE wx::base wx::core)
 endif()
+
+
+if (UNIX AND NOT APPLE)
+	target_link_libraries(CemuInput PRIVATE bluez::bluez)
+endif ()

src/input/api/Wiimote/WiimoteControllerProvider.cpp

@@ -2,7 +2,12 @@
 #include "input/api/Wiimote/NativeWiimoteController.h"
 #include "input/api/Wiimote/WiimoteMessages.h"
 
+#ifdef HAS_HIDAPI
 #include "input/api/Wiimote/hidapi/HidapiWiimote.h"
+#endif
+#ifdef HAS_BLUEZ
+#include "input/api/Wiimote/l2cap/L2CapWiimote.h"
+#endif
 
 #include <numbers>
 #include <queue>
@@ -12,6 +17,7 @@ WiimoteControllerProvider::WiimoteControllerProvider()
 {
 	m_reader_thread = std::thread(&WiimoteControllerProvider::reader_thread, this);
 	m_writer_thread = std::thread(&WiimoteControllerProvider::writer_thread, this);
+	m_connectionThread = std::thread(&WiimoteControllerProvider::connectionThread, this);
 }
 
 WiimoteControllerProvider::~WiimoteControllerProvider()
@@ -21,48 +27,51 @@ WiimoteControllerProvider::~WiimoteControllerProvider()
 		m_running = false;
 		m_writer_thread.join();
 		m_reader_thread.join();
+		m_connectionThread.join();
 	}
 }
 
 std::vector<std::shared_ptr<ControllerBase>> WiimoteControllerProvider::get_controllers()
 {
+	m_connectedDeviceMutex.lock();
+	auto devices = m_connectedDevices;
+	m_connectedDeviceMutex.unlock();
+
 	std::scoped_lock lock(m_device_mutex);
 
-    std::queue<uint32> 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<WiimoteDevice> & 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;
-    };
-
-	for (auto& device : WiimoteDevice_t::get_devices())
+	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<Wiimote>(device));
-        }
-        // Otherwise add them
-        else {
-            m_wiimotes.push_back(std::make_unique<Wiimote>(device));
-        }
+		bool isDuplicate = false;
+		ssize_t lowestReplaceableIndex = -1;
+		for (ssize_t i = m_wiimotes.size() - 1; i >= 0; --i)
+		{
+			const auto& wiimoteDevice = m_wiimotes[i].device;
+			if (wiimoteDevice)
+			{
+				if (*wiimoteDevice == *device)
+				{
+					isDuplicate = true;
+					break;
+				}
+				continue;
+			}
+
+			lowestReplaceableIndex = i;
+		}
+		if (isDuplicate)
+			continue;
+		if (lowestReplaceableIndex != -1)
+			m_wiimotes.replace(lowestReplaceableIndex, std::make_unique<Wiimote>(device));
+		else
+			m_wiimotes.push_back(std::make_unique<Wiimote>(device));
 	}
 
 	std::vector<std::shared_ptr<ControllerBase>> result;
+	result.reserve(m_wiimotes.size());
 	for (size_t i = 0; i < m_wiimotes.size(); ++i)
 	{
 		result.emplace_back(std::make_shared<NativeWiimoteController>(i));
@@ -74,7 +83,7 @@ std::vector<std::shared_ptr<ControllerBase>> 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)
@@ -141,6 +150,30 @@ WiimoteControllerProvider::WiimoteState WiimoteControllerProvider::get_state(siz
 	return {};
 }
 
+void WiimoteControllerProvider::connectionThread()
+{
+	SetThreadName("Wiimote-connect");
+	while (m_running.load(std::memory_order_relaxed))
+	{
+		std::vector<WiimoteDevicePtr> devices;
+#ifdef HAS_HIDAPI
+		const auto& hidDevices = HidapiWiimote::get_devices();
+		std::ranges::move(hidDevices, std::back_inserter(devices));
+#endif
+#ifdef HAS_BLUEZ
+		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");
@@ -148,7 +181,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<std::chrono::seconds>(now - lastCheck) > std::chrono::seconds(2))
+		if (std::chrono::duration_cast<std::chrono::seconds>(now - lastCheck) > std::chrono::milliseconds(500))
 		{
 			// check for new connected wiimotes
 			get_controllers();
@@ -160,11 +193,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;
 
@@ -921,18 +959,18 @@ void WiimoteControllerProvider::writer_thread()
 
 		if (index != (size_t)-1 && !data.empty())
 		{
-			if (m_wiimotes[index].rumble)
+			auto& wiimote = m_wiimotes[index];
+			if (!wiimote.device)
+				continue;
+			if (wiimote.rumble)
 				data[1] |= 1;
-
-			m_wiimotes[index].connected = m_wiimotes[index].device->write_data(data);
-			if (m_wiimotes[index].connected)
+			if (!wiimote.device->write_data(data))
 			{
-				m_wiimotes[index].data_ts = std::chrono::high_resolution_clock::now();
+				wiimote.device.reset();
+				wiimote.rumble = false;
 			}
 			else
-			{
-				m_wiimotes[index].rumble = false;
-			}
+				wiimote.data_ts = std::chrono::high_resolution_clock::now();
 		}
 		device_lock.unlock();
 

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::thread m_connectionThread;
+	std::vector<WiimoteDevicePtr> 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();
 
 	void calibrate(size_t index);
 	IRMode set_ir_camera(size_t index, bool state);

src/input/api/Wiimote/WiimoteDevice.h

@@ -9,8 +9,7 @@ public:
 	virtual bool write_data(const std::vector<uint8>& data) = 0;
 	virtual std::optional<std::vector<uint8_t>> read_data() = 0;
 
-	virtual bool operator==(WiimoteDevice& o) const = 0;
-	bool operator!=(WiimoteDevice& o) const { return *this == o; }
+	virtual bool operator==(const WiimoteDevice& o) const = 0;
 };
 
 using WiimoteDevicePtr = std::shared_ptr<WiimoteDevice>;

src/input/api/Wiimote/hidapi/HidapiWiimote.cpp

@@ -47,8 +47,11 @@ std::vector<WiimoteDevicePtr> HidapiWiimote::get_devices() {
     return wiimote_devices;
 }
 
-bool HidapiWiimote::operator==(WiimoteDevice& o) const  {
-    return static_cast<HidapiWiimote const&>(o).m_path == m_path;
+bool HidapiWiimote::operator==(const WiimoteDevice& rhs) const  {
+    auto other = dynamic_cast<const HidapiWiimote*>(&rhs);
+	if (!other)
+		return false;
+	return m_path == other->m_path;
 }
 
 HidapiWiimote::~HidapiWiimote() {

src/input/api/Wiimote/hidapi/HidapiWiimote.h

@@ -10,7 +10,7 @@ public:
 
     bool write_data(const std::vector<uint8> &data) override;
     std::optional<std::vector<uint8>> read_data() override;
-    bool operator==(WiimoteDevice& o) const override;
+    bool operator==(const WiimoteDevice& o) const override;
 
     static std::vector<WiimoteDevicePtr> get_devices();
 
@@ -19,5 +19,3 @@ private:
     const std::string m_path;
 
 };
-
-using WiimoteDevice_t = HidapiWiimote;

src/input/api/Wiimote/l2cap/L2CapWiimote.cpp

@@ -0,0 +1,148 @@
+#include "L2CapWiimote.h"
+#include <bluetooth/l2cap.h>
+
+constexpr auto comparator = [](const bdaddr_t& a, const bdaddr_t& b) {
+	return bacmp(&a, &b);
+};
+
+static auto s_addresses = std::map<bdaddr_t, bool, decltype(comparator)>(comparator);
+static std::mutex s_addressMutex;
+
+static bool AttemptConnect(int sockFd, const sockaddr_l2& addr)
+{
+	auto res = connect(sockFd, reinterpret_cast<const sockaddr*>(&addr),
+					   sizeof(sockaddr_l2));
+	if (res == 0)
+		return true;
+	return connect(sockFd, reinterpret_cast<const sockaddr*>(&addr),
+				   sizeof(sockaddr_l2)) == 0;
+}
+
+static bool AttemptSetNonBlock(int sockFd)
+{
+	return fcntl(sockFd, F_SETFL, fcntl(sockFd, F_GETFL) | O_NONBLOCK) == 0;
+}
+
+L2CapWiimote::L2CapWiimote(int recvFd, int sendFd, bdaddr_t addr)
+	: m_recvFd(recvFd), m_sendFd(sendFd), m_addr(addr)
+{
+}
+
+L2CapWiimote::~L2CapWiimote()
+{
+	close(m_recvFd);
+	close(m_sendFd);
+	const auto& b = m_addr.b;
+	cemuLog_logDebug(LogType::Force, "Wiimote at {:02x}:{:02x}:{:02x}:{:02x}:{:02x}:{:02x} disconnected", b[5], b[4], b[3], b[2], b[1], b[0]);
+
+	// Re-add to candidate vec
+	s_addressMutex.lock();
+	s_addresses[m_addr] = false;
+	s_addressMutex.unlock();
+}
+
+void L2CapWiimote::AddCandidateAddress(bdaddr_t addr)
+{
+	std::scoped_lock lock(s_addressMutex);
+	s_addresses.try_emplace(addr, false);
+}
+
+std::vector<WiimoteDevicePtr> L2CapWiimote::get_devices()
+{
+	s_addressMutex.lock();
+	std::vector<bdaddr_t> unconnected;
+	for (const auto& [addr, connected] : s_addresses)
+	{
+		if (!connected)
+			unconnected.push_back(addr);
+	}
+	s_addressMutex.unlock();
+
+	std::vector<WiimoteDevicePtr> outDevices;
+	for (const auto& addr : unconnected)
+	{
+		// Socket for sending data to controller, PSM 0x11
+		auto sendFd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+		if (sendFd < 0)
+		{
+			cemuLog_logDebug(LogType::Force, "Failed to open send socket: {}", strerror(errno));
+			continue;
+		}
+
+		sockaddr_l2 sendAddr{};
+		sendAddr.l2_family = AF_BLUETOOTH;
+		sendAddr.l2_psm = htobs(0x11);
+		sendAddr.l2_bdaddr = addr;
+
+		if (!AttemptConnect(sendFd, sendAddr) || !AttemptSetNonBlock(sendFd))
+		{
+			const auto& b = addr.b;
+			cemuLog_logDebug(LogType::Force, "Failed to connect send socket to '{:02x}:{:02x}:{:02x}:{:02x}:{:02x}:{:02x}': {}",
+							 b[5], b[4], b[3], b[2], b[1], b[0], strerror(errno));
+			close(sendFd);
+			continue;
+		}
+
+		// Socket for receiving data from controller, PSM 0x13
+		auto recvFd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+		if (recvFd < 0)
+		{
+			cemuLog_logDebug(LogType::Force, "Failed to open recv socket: {}", strerror(errno));
+			close(sendFd);
+			continue;
+		}
+		sockaddr_l2 recvAddr{};
+		recvAddr.l2_family = AF_BLUETOOTH;
+		recvAddr.l2_psm = htobs(0x13);
+		recvAddr.l2_bdaddr = addr;
+
+		if (!AttemptConnect(recvFd, recvAddr) || !AttemptSetNonBlock(recvFd))
+		{
+			const auto& b = addr.b;
+			cemuLog_logDebug(LogType::Force, "Failed to connect recv socket to '{:02x}:{:02x}:{:02x}:{:02x}:{:02x}:{:02x}': {}",
+							 b[5], b[4], b[3], b[2], b[1], b[0], strerror(errno));
+			close(sendFd);
+			close(recvFd);
+			continue;
+		}
+		outDevices.emplace_back(std::make_shared<L2CapWiimote>(sendFd, recvFd, addr));
+
+		s_addressMutex.lock();
+		s_addresses[addr] = true;
+		s_addressMutex.unlock();
+	}
+	return outDevices;
+}
+
+bool L2CapWiimote::write_data(const std::vector<uint8>& data)
+{
+	const auto size = data.size();
+	cemu_assert_debug(size < 23);
+	uint8 buffer[23];
+	// All outgoing messages must be prefixed with 0xA2
+	buffer[0] = 0xA2;
+	std::memcpy(buffer + 1, data.data(), size);
+	const auto outSize = size + 1;
+	return send(m_sendFd, buffer, outSize, 0) == outSize;
+}
+
+std::optional<std::vector<uint8>> L2CapWiimote::read_data()
+{
+	uint8 buffer[23];
+	const auto nBytes = recv(m_sendFd, buffer, 23, 0);
+
+	if (nBytes < 0 && errno == EWOULDBLOCK)
+		return std::vector<uint8>{};
+	// All incoming messages must be prefixed with 0xA1
+	if (nBytes < 2 || buffer[0] != 0xA1)
+		return std::nullopt;
+	return std::vector(buffer + 1, buffer + 1 + nBytes - 1);
+}
+
+bool L2CapWiimote::operator==(const WiimoteDevice& rhs) const
+{
+	auto mote = dynamic_cast<const L2CapWiimote*>(&rhs);
+	if (!mote)
+		return false;
+	return bacmp(&m_addr, &mote->m_addr) == 0;
+}

src/input/api/Wiimote/l2cap/L2CapWiimote.h

@@ -0,0 +1,22 @@
+#pragma once
+#include <input/api/Wiimote/WiimoteDevice.h>
+#include <bluetooth/bluetooth.h>
+
+class L2CapWiimote : public WiimoteDevice
+{
+  public:
+	L2CapWiimote(int recvFd, int sendFd, bdaddr_t addr);
+	~L2CapWiimote() override;
+
+	bool write_data(const std::vector<uint8>& data) override;
+	std::optional<std::vector<uint8>> read_data() override;
+	bool operator==(const WiimoteDevice& o) const override;
+
+	static void AddCandidateAddress(bdaddr_t addr);
+	static std::vector<WiimoteDevicePtr> get_devices();
+  private:
+	int m_recvFd;
+	int m_sendFd;
+	bdaddr_t m_addr;
+};
+