Reuse threads between connections. Either Switch or libnx doesn't seem to release allocated memory for spawned threads after they have been cleaned up, joined, and exited.

Author: Koi-3088

include/commandHandler.h

@@ -75,7 +75,11 @@ namespace CommandHandler {
 #pragma endregion Command registration.
 		};
 
-		~Handler() override {}
+		~Handler() override {
+            m_cmd.clear();
+            cqNotifyAll();
+			cqJoinThread();
+        };
 
 	public:
 		std::vector<char> HandleCommand(const std::string& cmd, const std::vector<std::string>& params);

include/connection.h

@@ -12,6 +12,8 @@ namespace Connection {
 
 	public:
 		virtual Result initialize(Result& res) = 0;
+        virtual void initializeThreads() = 0;
+        virtual void stopThreads() = 0;
 		virtual bool connect() = 0;
 		virtual void run() = 0;
 		virtual void disconnect() = 0;

include/controllerCommands.h

@@ -16,7 +16,6 @@ namespace ControllerCommands {
 	class Controller : protected virtual ModuleBase::BaseCommands {
 	public:
         Controller() : BaseCommands(), m_ccQueue() {
-           m_workMem = (u8*)aligned_alloc(0x1000, m_workMem_size);
            m_controllerHandle = { 0 };
            m_controllerDevice = { 0 };
 		   m_hiddbgHdlsState = { 0 };
@@ -29,12 +28,17 @@ namespace ControllerCommands {
         };
 
 		~Controller() override {
-            std::lock_guard<std::mutex> lock(m_ccMutex);
-            m_isEnabledPA = false;
+			m_isEnabledPA = false;
 			m_ccThreadRunning = false;
-			detachController();
 			m_ccCv.notify_all();
 			if (m_ccThread.joinable()) m_ccThread.join();
+			m_ccQueue.clear();
+			detachController();
+			hiddbgExit();
+			if (m_workMem) {
+				aligned_free(m_workMem);
+				m_workMem = nullptr;
+			}
 		};
 
 	public:
@@ -94,7 +98,7 @@ namespace ControllerCommands {
 		static int parseStringToButton(const std::string& arg);
 		static int parseStringToStick(const std::string& arg);
 
-        void startControllerThread(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& error);
+        void startControllerThread(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& stop, std::atomic_bool& error);
 		void cqEnqueueCommand(const ControllerCommand& cmd);
 		void cqReplaceOnNext();
 		void cqCancel();
@@ -116,7 +120,7 @@ namespace ControllerCommands {
 		void setControllerType(const std::vector<std::string>& params);
 
 	private:
-		void commandLoopPA(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& error);
+		void commandLoopPA(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& stop, std::atomic_bool& error);
 		void cqControllerState(const ControllerCommand& cmd);
 
 		inline void* aligned_alloc(size_t alignment, size_t size) {

include/lockFreeQueue.h

@@ -4,7 +4,7 @@
 #include <atomic>
 
 namespace LocklessQueue {
-    template<typename T, size_t Capacity = 128>
+    template<typename T, size_t Capacity = 256>
     class LockFreeQueue {
         static_assert(Capacity > 0, "Capacity must be greater than 0.");
 
@@ -95,5 +95,11 @@ namespace LocklessQueue {
             size_t seq = cell.sequence.load(std::memory_order_acquire);
             return ((intptr_t)seq - (intptr_t)pos) < 0;
         }
+
+        size_t size() const {
+            size_t enq = m_enqueuePos.load(std::memory_order_acquire);
+            size_t deq = m_dequeuePos.load(std::memory_order_acquire);
+            return enq - deq;
+        }
     };
 }

include/moduleBase.h

@@ -89,7 +89,7 @@ namespace ModuleBase {
 
 	private:
 		static std::string getCurrentSbbVersion() {
-            return !g_enableBackwardsCompat ? "3.0\r\n" : "3.1\r\n";
+            return !g_enableBackwardsCompat ? "3.01\r\n" : "3.11\r\n";
         }
 
         void setButtonClickSleepTime(const std::vector<std::string>& params);

include/socketConnection.h

@@ -12,25 +12,19 @@ namespace SocketConnection {
 	public:
 		SocketConnection() : ConnectionHandler(), m_tcp(), m_senderQueue(), m_commandQueue() {
 			m_error = false;
+            m_stop = false;
 			m_handler = std::make_unique<CommandHandler::Handler>();
 		};
 
 		~SocketConnection() override {
-			m_error = true;
-			notifyAll();
-
-			m_persistentBuffer.clear();
-			m_senderQueue.clear();
-			m_commandQueue.clear();
-
-			if (m_senderThread.joinable()) m_senderThread.join();
-			if (m_commandThread.joinable()) m_commandThread.join();
-			if (m_handler) m_handler->cqJoinThread();
-			if (m_handler) m_handler.reset();
+            disconnect();
+            stopThreads();
 		};
 
 	public:
 		Result initialize(Result& res) override;
+		void initializeThreads() override;
+		void stopThreads() override;
 		bool connect() override;
 		void run() override;
 		void disconnect() override;
@@ -47,13 +41,21 @@ namespace SocketConnection {
 		TcpConnection m_tcp;
 
 		int setupServerSocket();
+		void closeSocket();
 		void notifyAll() {
 			m_commandCv.notify_all();
             m_senderCv.notify_all();
 			if (m_handler) m_handler->cqNotifyAll();
 		}
 
+		bool getThreadsInitialized() const {
+			return m_senderInitialized.load(std::memory_order_relaxed)
+				   && m_commandInitialized.load(std::memory_order_relaxed);
+        }
+
 		std::string m_persistentBuffer;
+		std::atomic_bool m_senderInitialized { false };
+		std::atomic_bool m_commandInitialized { false };
 
 		std::thread m_senderThread;
 		LocklessQueue::LockFreeQueue<std::vector<char>> m_senderQueue;
@@ -66,6 +68,7 @@ namespace SocketConnection {
 		std::condition_variable m_commandCv;
 
 		std::atomic_bool m_error { false };
+		std::atomic_bool m_stop { false };
 		std::unique_ptr<CommandHandler::Handler> m_handler;
 	};
 }

include/usbConnection.h

@@ -12,6 +12,7 @@ namespace UsbConnection {
 	public:
 		UsbConnection() : ConnectionHandler() {
 			m_error = false;
+			m_stop = false;
 			m_handler = std::make_unique<CommandHandler::Handler>();
 		};
 
@@ -30,6 +31,8 @@ namespace UsbConnection {
 
 	public:
 		Result initialize(Result& res) override;
+        void initializeThreads() override;
+        void stopThreads() override;
 		bool connect() override;
 		void run() override;
 		void disconnect() override;
@@ -43,7 +46,14 @@ namespace UsbConnection {
 			if (m_handler) m_handler->cqNotifyAll();
 		}
 
+		bool getThreadsInitialized() const {
+			return m_senderInitialized.load(std::memory_order_relaxed)
+				&& m_commandInitialized.load(std::memory_order_relaxed);
+		}
+
 		std::string m_persistentBuffer;
+		std::atomic_bool m_senderInitialized { false };
+		std::atomic_bool m_commandInitialized { false };
 
 		std::thread m_senderThread;
 		LocklessQueue::LockFreeQueue<std::vector<char>> m_senderQueue;
@@ -56,6 +66,7 @@ namespace UsbConnection {
 		std::condition_variable m_commandCv;
 
 		std::atomic_bool m_error { false };
+		std::atomic_bool m_stop{ false };
 		std::unique_ptr<CommandHandler::Handler> m_handler;
 
 		struct USBResponse {

source/commandHandler.cpp

@@ -38,11 +38,7 @@ namespace CommandHandler {
         Logger::instance().log(log);
 		u64 pid = 0;
 		Result rc = pmdmntGetApplicationProcessId(&pid);
-		if (R_FAILED(rc)) {
-			Logger::instance().log("initMetaData() pmdmntGetApplicationProcessId() failed: pid=" + std::to_string(pid), std::to_string(R_DESCRIPTION(rc)));
-		}
-
-		if (m_metaData.pid == 0 || m_metaData.pid != pid) {
+		if (R_SUCCEEDED(rc) && (m_metaData.pid == 0 || m_metaData.pid != pid)) {
 			m_metaData.pid = pid;
 			initMetaData();
         }

source/controllerCommands.cpp

@@ -25,9 +25,16 @@ namespace ControllerCommands {
         }
 
         if (!m_workMem) {
-            m_workMem = (u8*)aligned_alloc(0x1000, m_workMem_size);
-            if (!m_workMem) {
-                Logger::instance().log("Failed to initialize virtual controller.", "initController() aligned_alloc() failed.");
+            try {
+                m_workMem = (u8*)aligned_alloc(0x1000, m_workMem_size);
+                if (!m_workMem) {
+                    Logger::instance().log("Failed to initialize virtual controller.", "initController() aligned_alloc() failed.");
+                    hiddbgExit();
+                    return;
+                }
+            } catch (...) {
+                Logger::instance().log("Exception during m_workMem allocation.");
+                hiddbgExit();
                 return;
             }
         }
@@ -225,14 +232,30 @@ namespace ControllerCommands {
      * @param Condition variable for the sender queue.
      * @param Atomic boolean for error handling, passed from the command thread.
      */
-    void Controller::startControllerThread(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& error) {
+    void Controller::startControllerThread(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& stop, std::atomic_bool& error) {
         if (m_ccThreadRunning) {
             Logger::instance().log("Controller thread already running.");
             return;
         }
 
         Logger::instance().log("Starting commandLoopPA thread.");
-        m_ccThread = std::thread(&Controller::commandLoopPA, this, std::ref(senderQueue), std::ref(senderCv), std::ref(error));
+        try {
+            m_ccThread = std::thread(&Controller::commandLoopPA, this, std::ref(senderQueue), std::ref(senderCv), std::ref(stop), std::ref(error));
+            m_ccThreadRunning = true;
+            Logger::instance().log("commandLoopPA thread created successfully.");
+        } catch (const std::exception& e) {
+            Logger::instance().log("Failed to create commandLoopPA thread: ", e.what());
+            m_ccThreadRunning = false;
+            stop = true;
+            error = true;
+            throw;
+        } catch (...) {
+            Logger::instance().log("Unknown exception creating commandLoopPA thread.");
+            m_ccThreadRunning = false;
+            stop = true;
+            error = true;
+            throw;
+        }
     }
 
     /**
@@ -241,14 +264,13 @@ namespace ControllerCommands {
      * @param Condition variable for the sender queue.
      * @param Atomic boolean for error handling, passed from the command thread.
      */
-    void Controller::commandLoopPA(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& error) {
+    void Controller::commandLoopPA(LockFreeQueue<std::vector<char>>& senderQueue, std::condition_variable& senderCv, std::atomic_bool& stop, std::atomic_bool& error) {
         const std::chrono::microseconds earlyWake(1000);
         m_nextStateChange = WallClock::max();
-        m_ccThreadRunning = true;
         Logger::instance().log("commandLoopPA() started.");
 
         std::unique_lock<std::mutex> lock(m_ccMutex);
-        while (!error) {
+        while (!stop) {
             WallClock now = std::chrono::steady_clock::now();
             ControllerCommand cmd;
             if (now >= m_nextStateChange) {
@@ -270,17 +292,29 @@ namespace ControllerCommands {
             if (m_ccCurrentCommand.seqnum != 0){
                 Logger::instance().log("cqSendState() command finished with seqnum: " + std::to_string(m_ccCurrentCommand.seqnum));
                 std::string res = "cqCommandFinished " + std::to_string(m_ccCurrentCommand.seqnum) + "\r\n";
-                senderQueue.push(std::vector<char>(res.begin(), res.end()));
-                senderCv.notify_one();
+                if (!senderQueue.full()) {
+                    senderQueue.push(std::vector<char>(res.begin(), res.end()));
+                    senderCv.notify_one();
+                } else {
+                    Logger::instance().log("Sender queue full, dropping command finished message.");
+                }
             }
 
             m_ccCurrentCommand = cmd;
-            m_ccCv.wait_until(lock, m_nextStateChange - earlyWake, [&] { return error || now + earlyWake >= m_nextStateChange; });
+            m_ccCv.wait_until(lock, m_nextStateChange - earlyWake, [&] { return stop || error || now + earlyWake >= m_nextStateChange; });
+            if (error) {
+                m_ccQueue.clear();
+                cqControllerState(ControllerCommand{});
+                m_nextStateChange = WallClock::max();
+            }
         }
 
         m_ccQueue.clear();
         cqControllerState(ControllerCommand{});
         detachController();
+        m_ccThreadRunning = false;
+        m_isEnabledPA = false;
+        stop = true;
         Logger::instance().log("commandLoopPA() exiting thread...");
     }
 
@@ -290,7 +324,15 @@ namespace ControllerCommands {
      */
     void Controller::cqControllerState(const ControllerCommand& cmd) {
         Logger::instance().log("cqControllerState() called with seqnum: " + std::to_string(cmd.seqnum));
-        initController();
+        try {
+            initController();
+        } catch (const std::exception& e) {
+            Logger::instance().log("cqControllerState() initController() failed: ", e.what());
+            return;
+        } catch (...) {
+            Logger::instance().log("cqControllerState() initController() unknown exception.");
+            return;
+        }
 
         m_hiddbgHdlsState.buttons = cmd.state.buttons;
         m_hiddbgHdlsState.analog_stick_l.x = cmd.state.left_joystick_x;
@@ -362,11 +404,8 @@ namespace ControllerCommands {
      * @brief Join the PA controller thread if it is running.
      */
     void Controller::cqJoinThread() {
-        if (m_ccThreadRunning && m_ccThread.joinable()) {
-            m_ccThread.join();
-            m_ccThreadRunning = false;
-            Logger::instance().log("commandLoopPA thread finished.");
-        }
+        m_ccCv.notify_all();
+        if (m_ccThread.joinable()) m_ccThread.join();
     }
 
     /**