From df3d62df84b924345bf32b0de11beedb8b70ed5a Mon Sep 17 00:00:00 2001 From: Michael Scire Date: Tue, 9 Feb 2021 01:05:43 -0800 Subject: [PATCH] htc: send logic for HtcctrlService, bugfixes (thanks @misson20000) --- .../ctrl/htclow_ctrl_packet_factory.cpp | 10 +-- .../htclow/ctrl/htclow_ctrl_send_buffer.cpp | 48 ++++++++++++ .../htclow/ctrl/htclow_ctrl_send_buffer.hpp | 5 ++ .../htclow/ctrl/htclow_ctrl_service.cpp | 74 +++++++++++++++++++ .../htclow/ctrl/htclow_ctrl_service.hpp | 11 +++ .../source/htclow/htclow_packet.hpp | 2 +- .../source/htclow/htclow_worker.cpp | 46 +++++++++++- .../source/htclow/htclow_worker.hpp | 3 +- .../source/htclow/mux/htclow_mux.cpp | 6 +- .../source/htclow/mux/htclow_mux.hpp | 7 +- 10 files changed, 197 insertions(+), 15 deletions(-) diff --git a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_packet_factory.cpp b/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_packet_factory.cpp index 7c18ed574..d2851378d 100644 --- a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_packet_factory.cpp +++ b/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_packet_factory.cpp @@ -61,14 +61,14 @@ namespace ams::htclow::ctrl { std::unique_ptr 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 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 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 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 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; diff --git a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.cpp b/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.cpp index 77de070c7..ac0ebae26 100644 --- a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.cpp +++ b/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.cpp @@ -15,6 +15,7 @@ */ #include #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(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; + } + } diff --git a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.hpp b/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.hpp index d5c8c4798..a7ecd8af4 100644 --- a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_send_buffer.hpp +++ b/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 ptr); + void RemovePacket(const HtcctrlPacketHeader &header); + + bool QueryNextPacket(HtcctrlPacketHeader *header, HtcctrlPacketBody *body, int *out_body_size); }; } diff --git a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.cpp b/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.cpp index e1fd3b7cc..724a6f734 100644 --- a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.cpp +++ b/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(header.packet_type)); + } + } + Result HtcctrlService::NotifyDriverConnected() { /* Lock ourselves. */ std::scoped_lock lk(m_mutex); diff --git a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.hpp b/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.hpp index b94dc58a8..608d8ada5 100644 --- a/libraries/libstratosphere/source/htclow/ctrl/htclow_ctrl_service.hpp +++ b/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(); }; diff --git a/libraries/libstratosphere/source/htclow/htclow_packet.hpp b/libraries/libstratosphere/source/htclow/htclow_packet.hpp index 369753d1d..925cace68 100644 --- a/libraries/libstratosphere/source/htclow/htclow_packet.hpp +++ b/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 { diff --git a/libraries/libstratosphere/source/htclow/htclow_worker.cpp b/libraries/libstratosphere/source/htclow/htclow_worker.cpp index d5bc43592..f04cd7eaa 100644 --- a/libraries/libstratosphere/source/htclow/htclow_worker.cpp +++ b/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 */ - AMS_ABORT("Worker::ProcessSend"); + /* Forever process packets. */ + 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(m_send_buffer); + auto *packet_body = reinterpret_cast(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(m_send_buffer); + auto *packet_body = reinterpret_cast(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(); + } + } + } + } } diff --git a/libraries/libstratosphere/source/htclow/htclow_worker.hpp b/libraries/libstratosphere/source/htclow/htclow_worker.hpp index 1e9529f1f..6efb0d6c9 100644 --- a/libraries/libstratosphere/source/htclow/htclow_worker.hpp +++ b/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_packet_body[sizeof(PacketBody)]; + u8 m_send_buffer[sizeof(PacketHeader) + sizeof(PacketBody)]; u8 m_receive_packet_body[sizeof(PacketBody)]; mem::StandardAllocator *m_allocator; mux::Mux *m_mux; diff --git a/libraries/libstratosphere/source/htclow/mux/htclow_mux.cpp b/libraries/libstratosphere/source/htclow/mux/htclow_mux.cpp index 9db51a24f..b42be9a7f 100644 --- a/libraries/libstratosphere/source/htclow/mux/htclow_mux.cpp +++ b/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_channel_impl_map(pf, sm, std::addressof(m_task_manager), std::addressof(m_wake_event)), m_global_send_buffer(pf), + : 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_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(); } } diff --git a/libraries/libstratosphere/source/htclow/mux/htclow_mux.hpp b/libraries/libstratosphere/source/htclow/mux/htclow_mux.hpp index 8a41df89e..4538a1455 100644 --- a/libraries/libstratosphere/source/htclow/mux/htclow_mux.hpp +++ b/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: