htc: send logic for HtcctrlService, bugfixes (thanks @misson20000)

libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_packet_factory.cpp

@@ -61,14 +61,14 @@ namespace ams::htclow::ctrl {
     std::unique_ptr<HtcctrlPacket, HtcctrlPacketDeleter> HtcctrlPacketFactory::MakeResumePacket() {
         auto packet = this->MakeSendPacketCommon(0);
         if (packet && packet->IsAllocationSucceeded()) {
-            packet->GetHeader()->packet_type = HtcctrlPacketType_SuspendFromTarget;
+            packet->GetHeader()->packet_type = HtcctrlPacketType_ResumeFromTarget;
         }
 
         return packet;
     }
 
     std::unique_ptr<HtcctrlPacket, HtcctrlPacketDeleter> HtcctrlPacketFactory::MakeReadyPacket(const void *body, int body_size) {
-        auto packet = this->MakeSendPacketCommon(0);
+        auto packet = this->MakeSendPacketCommon(body_size);
         if (packet && packet->IsAllocationSucceeded()) {
             packet->GetHeader()->packet_type = HtcctrlPacketType_ReadyFromTarget;
 
@@ -79,7 +79,7 @@ namespace ams::htclow::ctrl {
     }
 
     std::unique_ptr<HtcctrlPacket, HtcctrlPacketDeleter> HtcctrlPacketFactory::MakeInformationPacket(const void *body, int body_size) {
-        auto packet = this->MakeSendPacketCommon(0);
+        auto packet = this->MakeSendPacketCommon(body_size);
         if (packet && packet->IsAllocationSucceeded()) {
             packet->GetHeader()->packet_type = HtcctrlPacketType_InformationFromTarget;
 
@@ -99,7 +99,7 @@ namespace ams::htclow::ctrl {
     }
 
     std::unique_ptr<HtcctrlPacket, HtcctrlPacketDeleter> HtcctrlPacketFactory::MakeConnectPacket(const void *body, int body_size) {
-        auto packet = this->MakeSendPacketCommon(0);
+        auto packet = this->MakeSendPacketCommon(body_size);
         if (packet && packet->IsAllocationSucceeded()) {
             packet->GetHeader()->packet_type = HtcctrlPacketType_ConnectFromTarget;
 
@@ -110,7 +110,7 @@ namespace ams::htclow::ctrl {
     }
 
     std::unique_ptr<HtcctrlPacket, HtcctrlPacketDeleter> HtcctrlPacketFactory::MakeBeaconResponsePacket(const void *body, int body_size) {
-        auto packet = this->MakeSendPacketCommon(0);
+        auto packet = this->MakeSendPacketCommon(body_size);
         if (packet && packet->IsAllocationSucceeded()) {
             packet->GetHeader()->packet_type = HtcctrlPacketType_BeaconResponse;
 

libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.cpp

@@ -15,6 +15,7 @@
  */
 #include <stratosphere.hpp>
 #include "htclow_ctrl_send_buffer.hpp"
+#include "htclow_ctrl_packet_factory.hpp"
 
 namespace ams::htclow::ctrl {
 
@@ -30,6 +31,7 @@ namespace ams::htclow::ctrl {
             case HtcctrlPacketType_ResumeFromTarget:
             case HtcctrlPacketType_BeaconResponse:
             case HtcctrlPacketType_InformationFromTarget:
+                return true;
             default:
                 return false;
         }
@@ -51,4 +53,50 @@ namespace ams::htclow::ctrl {
         }
     }
 
+    void HtcctrlSendBuffer::RemovePacket(const HtcctrlPacketHeader &header) {
+        /* Get the packet type. */
+        const auto packet_type = header.packet_type;
+
+        /* Remove the front from the appropriate list. */
+        HtcctrlPacket *packet;
+        if (this->IsPriorPacket(packet_type)) {
+            packet = std::addressof(m_prior_packet_list.front());
+            m_prior_packet_list.pop_front();
+        } else {
+            AMS_ABORT_UNLESS(this->IsPosteriorPacket(packet_type));
+            packet = std::addressof(m_posterior_packet_list.front());
+            m_posterior_packet_list.pop_front();
+        }
+
+        /* Delete the packet. */
+        m_packet_factory->Delete(packet);
+    }
+
+    bool HtcctrlSendBuffer::QueryNextPacket(HtcctrlPacketHeader *header, HtcctrlPacketBody *body, int *out_body_size) {
+        if (!m_prior_packet_list.empty()) {
+            this->CopyPacket(header, body, out_body_size, m_prior_packet_list.front());
+            return true;
+        } else if (!m_posterior_packet_list.empty()) {
+            this->CopyPacket(header, body, out_body_size, m_posterior_packet_list.front());
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    void HtcctrlSendBuffer::CopyPacket(HtcctrlPacketHeader *header, HtcctrlPacketBody *body, int *out_body_size, const HtcctrlPacket &packet) {
+        /* Get the body size. */
+        const int body_size = packet.GetBodySize();
+        AMS_ASSERT(0 <= body_size && body_size <= static_cast<int>(sizeof(*body)));
+
+        /* Copy the header. */
+        std::memcpy(header, packet.GetHeader(), sizeof(*header));
+
+        /* Copy the body. */
+        std::memcpy(body, packet.GetBody(), body_size);
+
+        /* Set the output body size. */
+        *out_body_size = body_size;
+    }
+
 }

libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.hpp

@@ -31,10 +31,15 @@ namespace ams::htclow::ctrl {
         private:
             bool IsPriorPacket(HtcctrlPacketType packet_type) const;
             bool IsPosteriorPacket(HtcctrlPacketType packet_type) const;
+
+            void CopyPacket(HtcctrlPacketHeader *header, HtcctrlPacketBody *body, int *out_body_size, const HtcctrlPacket &packet);
         public:
             HtcctrlSendBuffer(HtcctrlPacketFactory *pf) : m_packet_factory(pf), m_prior_packet_list(), m_posterior_packet_list() { /* ... */ }
 
             void AddPacket(std::unique_ptr<HtcctrlPacket, HtcctrlPacketDeleter> ptr);
+            void RemovePacket(const HtcctrlPacketHeader &header);
+
+            bool QueryNextPacket(HtcctrlPacketHeader *header, HtcctrlPacketBody *body, int *out_body_size);
     };
 
 }

libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.cpp

@@ -224,6 +224,41 @@ namespace ams::htclow::ctrl {
         return htclow::ResultHtcctrlReceiveUnexpectedPacket();
     }
 
+    void HtcctrlService::ProcessSendConnectPacket() {
+        /* Set our state. */
+        const Result result = this->SetState(HtcctrlState_Connected);
+        R_ASSERT(result);
+    }
+
+    void HtcctrlService::ProcessSendReadyPacket() {
+        /* Set our state. */
+        if (m_state_machine->GetHtcctrlState() == HtcctrlState_SentReadyFromHost) {
+            const Result result = this->SetState(HtcctrlState_Ready);
+            R_ASSERT(result);
+        }
+
+        /* Update channel states. */
+        m_mux->UpdateChannelState();
+    }
+
+    void HtcctrlService::ProcessSendSuspendPacket() {
+        /* Set our state. */
+        const Result result = this->SetState(HtcctrlState_SentSuspendFromTarget);
+        R_ASSERT(result);
+    }
+
+    void HtcctrlService::ProcessSendResumePacket() {
+        /* Set our state. */
+        const Result result = this->SetState(HtcctrlState_SentResumeFromTarget);
+        R_ASSERT(result);
+    }
+
+    void HtcctrlService::ProcessSendDisconnectPacket() {
+        /* Set our state. */
+        const Result result = this->SetState(HtcctrlState_Disconnected);
+        R_ASSERT(result);
+    }
+
     void HtcctrlService::UpdateServiceChannels(const void *body, size_t body_size) {
         /* Copy the packet body to our member. */
         std::memcpy(m_service_channels_packet, body, body_size);
@@ -259,6 +294,45 @@ namespace ams::htclow::ctrl {
         }
     }
 
+    bool HtcctrlService::QuerySendPacket(HtcctrlPacketHeader *header, HtcctrlPacketBody *body, int *out_body_size) {
+        /* Lock ourselves. */
+        std::scoped_lock lk(m_mutex);
+
+        return m_send_buffer.QueryNextPacket(header, body, out_body_size);
+    }
+
+    void HtcctrlService::RemovePacket(const HtcctrlPacketHeader &header) {
+        /* Lock ourselves. */
+        std::scoped_lock lk(m_mutex);
+
+        /* Remove the packet from our buffer. */
+        m_send_buffer.RemovePacket(header);
+
+        /* Switch on the packet type. */
+        switch (header.packet_type) {
+            case HtcctrlPacketType_ConnectFromTarget:
+                this->ProcessSendConnectPacket();
+                break;
+            case HtcctrlPacketType_ReadyFromTarget:
+                this->ProcessSendReadyPacket();
+                break;
+            case HtcctrlPacketType_SuspendFromTarget:
+                this->ProcessSendSuspendPacket();
+                break;
+            case HtcctrlPacketType_ResumeFromTarget:
+                this->ProcessSendResumePacket();
+                break;
+            case HtcctrlPacketType_DisconnectFromTarget:
+                this->ProcessSendDisconnectPacket();
+                break;
+            case HtcctrlPacketType_BeaconResponse:
+            case HtcctrlPacketType_InformationFromTarget:
+                break;
+            default:
+                AMS_ABORT("Send unsupported packet 0x%04x\n", static_cast<u32>(header.packet_type));
+        }
+    }
+
     Result HtcctrlService::NotifyDriverConnected() {
         /* Lock ourselves. */
         std::scoped_lock lk(m_mutex);

libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.hpp

@@ -65,6 +65,12 @@ namespace ams::htclow::ctrl {
             Result ProcessReceiveBeaconQueryPacket();
             Result ProcessReceiveUnexpectedPacket();
 
+            void ProcessSendConnectPacket();
+            void ProcessSendReadyPacket();
+            void ProcessSendSuspendPacket();
+            void ProcessSendResumePacket();
+            void ProcessSendDisconnectPacket();
+
             void UpdateServiceChannels(const void *body, size_t body_size);
             void TryReadyInternal();
 
@@ -77,9 +83,14 @@ namespace ams::htclow::ctrl {
 
             void SetDriverType(impl::DriverType driver_type);
 
+            os::EventType *GetSendPacketEvent() { return m_event.GetBase(); }
+
             Result CheckReceivedHeader(const HtcctrlPacketHeader &header) const;
             Result ProcessReceivePacket(const HtcctrlPacketHeader &header, const void *body, size_t body_size);
 
+            bool QuerySendPacket(HtcctrlPacketHeader *header, HtcctrlPacketBody *body, int *out_body_size);
+            void RemovePacket(const HtcctrlPacketHeader &header);
+
             Result NotifyDriverConnected();
             Result NotifyDriverDisconnected();
     };

libraries/libstratosphere/source/htclow/htclow_packet.hpp

@@ -80,7 +80,7 @@ namespace ams::htclow {
                 return m_packet_size - sizeof(HeaderType);
             }
 
-            u8 *GetBody() {
+            u8 *GetBody() const {
                 if (this->GetBodySize() > 0) {
                     return m_header + sizeof(HeaderType);
                 } else {

libraries/libstratosphere/source/htclow/htclow_worker.cpp

@@ -83,7 +83,8 @@ namespace ams::htclow {
 
     Result Worker::ProcessReceive() {
         /* Forever receive packets. */
-        u8 packet_header_storage[sizeof(m_packet_header)];
+        constexpr size_t MaxPacketHeaderSize = std::max(sizeof(PacketHeader), sizeof(ctrl::HtcctrlPacketHeader));
+        u8 packet_header_storage[MaxPacketHeaderSize];
         while (true) {
             /* Receive the packet header. */
             R_TRY(m_driver->Receive(packet_header_storage, sizeof(packet_header_storage)));
@@ -134,8 +135,47 @@ namespace ams::htclow {
     }
 
     Result Worker::ProcessSend() {
-        /* TODO */
+        /* Forever process packets. */
-        AMS_ABORT("Worker::ProcessSend");
+        while (true) {
+            const auto index = os::WaitAny(m_service->GetSendPacketEvent(), m_mux->GetSendPacketEvent(), m_event.GetBase());
+            if (index == 0) {
+                /* HtcctrlService packet. */
+
+                /* Clear the packet event. */
+                os::ClearEvent(m_service->GetSendPacketEvent());
+
+                /* While we have packets, send them. */
+                auto *packet_header = reinterpret_cast<ctrl::HtcctrlPacketHeader *>(m_send_buffer);
+                auto *packet_body   = reinterpret_cast<ctrl::HtcctrlPacketBody *>(m_send_buffer + sizeof(*packet_header));
+                int body_size;
+                while (m_service->QuerySendPacket(packet_header, packet_body, std::addressof(body_size))) {
+                    m_service->RemovePacket(*packet_header);
+                    R_TRY(m_driver->Send(packet_header, body_size + sizeof(*packet_header)));
+                }
+            } else if (index == 1) {
+                /* Mux packet. */
+
+                /* Clear the packet event. */
+                os::ClearEvent(m_mux->GetSendPacketEvent());
+
+                /* While we have packets, send them. */
+                auto *packet_header = reinterpret_cast<PacketHeader *>(m_send_buffer);
+                auto *packet_body   = reinterpret_cast<PacketBody *>(m_send_buffer + sizeof(*packet_header));
+                int body_size;
+                while (m_mux->QuerySendPacket(packet_header, packet_body, std::addressof(body_size))) {
+                    R_TRY(m_driver->Send(packet_header, body_size + sizeof(*packet_header)));
+                    m_mux->RemovePacket(*packet_header);
+                }
+            } else {
+                /* Our event. */
+
+                /* Check if we're cancelled. */
+                if (m_cancelled) {
+                    return htclow::ResultCancelled();
+                }
+            }
+        }
+
     }
 
 }

libraries/libstratosphere/source/htclow/htclow_worker.hpp

@@ -27,8 +27,7 @@ namespace ams::htclow {
             static_assert(sizeof(ctrl::HtcctrlPacketBody) <= sizeof(PacketBody));
         private:
             u32 m_thread_stack_size;
-            u8 m_packet_header[sizeof(PacketHeader)];
+            u8 m_send_buffer[sizeof(PacketHeader) + sizeof(PacketBody)];
-            u8 m_send_packet_body[sizeof(PacketBody)];
             u8 m_receive_packet_body[sizeof(PacketBody)];
             mem::StandardAllocator *m_allocator;
             mux::Mux *m_mux;

libraries/libstratosphere/source/htclow/mux/htclow_mux.cpp

@@ -20,8 +20,8 @@
 namespace ams::htclow::mux {
 
     Mux::Mux(PacketFactory *pf, ctrl::HtcctrlStateMachine *sm)
-        : m_packet_factory(pf), m_state_machine(sm), m_task_manager(), m_wake_event(os::EventClearMode_ManualClear),
+        : m_packet_factory(pf), m_state_machine(sm), m_task_manager(), m_event(os::EventClearMode_ManualClear),
-          m_channel_impl_map(pf, sm, std::addressof(m_task_manager), std::addressof(m_wake_event)), m_global_send_buffer(pf),
+          m_channel_impl_map(pf, sm, std::addressof(m_task_manager), std::addressof(m_event)), m_global_send_buffer(pf),
           m_mutex(), m_is_sleeping(false), m_version(ProtocolVersion)
     {
         /* ... */
@@ -98,7 +98,7 @@ namespace ams::htclow::mux {
             m_is_sleeping = true;
         } else {
             m_is_sleeping = false;
-            m_wake_event.Signal();
+            m_event.Signal();
         }
     }
 

libraries/libstratosphere/source/htclow/mux/htclow_mux.hpp

@@ -26,7 +26,7 @@ namespace ams::htclow::mux {
             PacketFactory *m_packet_factory;
             ctrl::HtcctrlStateMachine *m_state_machine;
             TaskManager m_task_manager;
-            os::Event m_wake_event;
+            os::Event m_event;
             ChannelImplMap m_channel_impl_map;
             GlobalSendBuffer m_global_send_buffer;
             os::SdkMutex m_mutex;
@@ -37,9 +37,14 @@ namespace ams::htclow::mux {
 
             void SetVersion(u16 version);
 
+            os::EventType *GetSendPacketEvent() { return m_event.GetBase(); }
+
             Result CheckReceivedHeader(const PacketHeader &header) const;
             Result ProcessReceivePacket(const PacketHeader &header, const void *body, size_t body_size);
 
+            bool QuerySendPacket(PacketHeader *header, PacketBody *body, int *out_body_size);
+            void RemovePacket(const PacketHeader &header);
+
             void UpdateChannelState();
             void UpdateMuxState();
         private: