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

This commit is contained in:
Michael Scire 2021-02-09 01:05:43 -08:00 committed by SciresM
parent 0977ee72ca
commit df3d62df84
10 changed files with 197 additions and 15 deletions

View file

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

View file

@ -15,6 +15,7 @@
*/ */
#include <stratosphere.hpp> #include <stratosphere.hpp>
#include "htclow_ctrl_send_buffer.hpp" #include "htclow_ctrl_send_buffer.hpp"
#include "htclow_ctrl_packet_factory.hpp"
namespace ams::htclow::ctrl { namespace ams::htclow::ctrl {
@ -30,6 +31,7 @@ namespace ams::htclow::ctrl {
case HtcctrlPacketType_ResumeFromTarget: case HtcctrlPacketType_ResumeFromTarget:
case HtcctrlPacketType_BeaconResponse: case HtcctrlPacketType_BeaconResponse:
case HtcctrlPacketType_InformationFromTarget: case HtcctrlPacketType_InformationFromTarget:
return true;
default: default:
return false; 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;
}
} }

View file

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

View file

@ -224,6 +224,41 @@ namespace ams::htclow::ctrl {
return htclow::ResultHtcctrlReceiveUnexpectedPacket(); 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) { void HtcctrlService::UpdateServiceChannels(const void *body, size_t body_size) {
/* Copy the packet body to our member. */ /* Copy the packet body to our member. */
std::memcpy(m_service_channels_packet, body, body_size); 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() { Result HtcctrlService::NotifyDriverConnected() {
/* Lock ourselves. */ /* Lock ourselves. */
std::scoped_lock lk(m_mutex); std::scoped_lock lk(m_mutex);

View file

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

View file

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

View file

@ -83,7 +83,8 @@ namespace ams::htclow {
Result Worker::ProcessReceive() { Result Worker::ProcessReceive() {
/* Forever receive packets. */ /* 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) { while (true) {
/* Receive the packet header. */ /* Receive the packet header. */
R_TRY(m_driver->Receive(packet_header_storage, sizeof(packet_header_storage))); R_TRY(m_driver->Receive(packet_header_storage, sizeof(packet_header_storage)));
@ -134,8 +135,47 @@ namespace ams::htclow {
} }
Result Worker::ProcessSend() { 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();
}
}
}
} }
} }

View file

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

View file

@ -20,8 +20,8 @@
namespace ams::htclow::mux { namespace ams::htclow::mux {
Mux::Mux(PacketFactory *pf, ctrl::HtcctrlStateMachine *sm) 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) m_mutex(), m_is_sleeping(false), m_version(ProtocolVersion)
{ {
/* ... */ /* ... */
@ -98,7 +98,7 @@ namespace ams::htclow::mux {
m_is_sleeping = true; m_is_sleeping = true;
} else { } else {
m_is_sleeping = false; m_is_sleeping = false;
m_wake_event.Signal(); m_event.Signal();
} }
} }

View file

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