From a7f9729f639302bc713bddf1341484db9e529db3 Mon Sep 17 00:00:00 2001 From: Michael Scire Date: Fri, 16 Jul 2021 02:40:08 -0700 Subject: [PATCH] dmnt: begin working on packet parser --- .../source/dmnt2_gdb_packet_parser.cpp | 421 ++++++++++++++++++ .../source/dmnt2_gdb_packet_parser.hpp | 42 ++ .../dmnt.gen2/source/dmnt2_gdb_server.cpp | 57 ++- 3 files changed, 501 insertions(+), 19 deletions(-) create mode 100644 stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.cpp create mode 100644 stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.hpp diff --git a/stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.cpp b/stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.cpp new file mode 100644 index 000000000..a02519bc6 --- /dev/null +++ b/stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.cpp @@ -0,0 +1,421 @@ +/* + * Copyright (c) 2018-2020 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include +#include "dmnt2_debug_log.hpp" +#include "dmnt2_gdb_packet_parser.hpp" + +namespace ams::dmnt { + + namespace { + + constexpr const char TargetXmlAarch64[] = + "l" + "" + "" + "aarch64" + "GNU/Linux" + "" + "" + ""; + + constexpr const char Aarch64CoreXml[] = + "l\n" + "\n" + "\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\n" + "\t\t\n" + "\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\n" + "\t\n" + ""; + + constexpr const char Aarch64FpuXml[] = + "l\n" + "\n" + "\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\n" + "\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\n" + "\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\n" + "\t\n" + "\t\t\n" + "\t\t\n" + "\t\n" + "\t\n" + "\t\t\n" + "\t\t\n" + "\t\n" + "\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + ""; + + constexpr const char TargetXmlAarch32[] = + "l" + "" + "" + "" + "" + ""; + + constexpr const char ArmCoreXml[] = + "l\n" + "\n" + "\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\n"; + + constexpr const char ArmVfpXml[] = + "l\n" + "\n" + "\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\t\n" + "\n"; + + bool ParsePrefix(char *&packet, const char *prefix) { + const auto len = std::strlen(prefix); + if (std::strncmp(packet, prefix, len) == 0) { + packet += len; + return true; + } else { + return false; + } + } + + void SetReplyOk(char *reply) { + std::strcpy(reply, "OK"); + } + + void SetReplyError(char *reply, const char *err) { + AMS_DMNT2_GDB_LOG_ERROR("Reply Error: %s\n", err); + std::strcpy(reply, err); + } + + void SetReply(char *reply, const char *fmt, ...) __attribute__((format(printf, 2, 3))); + + void SetReply(char *reply, const char *fmt, ...) { + std::va_list vl; + va_start(vl, fmt); + util::VSNPrintf(reply, GdbPacketBufferSize, fmt, vl); + va_end(vl); + } + + void AppendReply(char *reply, const char *fmt, ...) __attribute__((format(printf, 2, 3))); + + void AppendReply(char *reply, const char *fmt, ...) { + const auto len = std::strlen(reply); + + std::va_list vl; + va_start(vl, fmt); + util::VSNPrintf(reply + len, GdbPacketBufferSize - len, fmt, vl); + va_end(vl); + } + + constexpr int DecodeHex(char c) { + if ('a' <= c && c <= 'f') { + return 10 + (c - 'a'); + } else if ('A' <= c && c <= 'F') { + return 10 + (c - 'A'); + } else if ('0' <= c && c <= '9') { + return 0 + (c - '0'); + } else { + return -1; + } + } + + void ParseOffsetLength(const char *packet, u32 &offset, u32 &length) { + /* Default to zero. */ + offset = 0; + length = 0; + + bool parsed_offset = false; + while (*packet) { + const char c = *(packet++); + if (c == ',') { + parsed_offset = true; + } else if (auto hex = DecodeHex(c); hex >= 0) { + if (parsed_offset) { + length <<= 4; + length |= hex; + } else { + offset <<= 4; + offset |= hex; + } + } + } + + AMS_DMNT2_GDB_LOG_DEBUG("Offset/Length %x/%x\n", offset, length); + } + + } + + void GdbPacketParser::Process() { + /* Log the packet we're processing. */ + AMS_DMNT2_GDB_LOG_DEBUG("Receive: %s\n", m_receive_packet); + + /* Clear our reply packet. */ + m_reply_packet[0] = 0; + + /* Handle the received packet. */ + switch (m_receive_packet[0]) { + case 'q': + this->q(); + break; + case '!': + SetReplyOk(m_reply_packet); + break; + default: + AMS_DMNT2_GDB_LOG_DEBUG("Not Implemented: %s\n", m_receive_packet); + break; + } + } + + void GdbPacketParser::q() { + if (ParsePrefix(m_receive_packet, "qSupported:")) { + this->qSupported(); + } else if (ParsePrefix(m_receive_packet, "qXfer:features:read:")) { + this->qXferFeaturesRead(); + } else { + AMS_DMNT2_GDB_LOG_DEBUG("Not Implemented q: %s\n", m_receive_packet); + } + } + + void GdbPacketParser::qSupported() { + /* Current string from devkita64-none-elf-gdb: */ + /* qSupported:multiprocess+;swbreak+;hwbreak+;qRelocInsn+;fork-events+;vfork-events+;exec-events+;vContSupported+;QThreadEvents+;no-resumed+ */ + + SetReply(m_reply_packet, "PacketSize=%lx", GdbPacketBufferSize - 1); + AppendReply(m_reply_packet, ";multiprocess+"); + AppendReply(m_reply_packet, ";qXfer:osdata:read+"); + AppendReply(m_reply_packet, ";qXfer:features:read+"); + AppendReply(m_reply_packet, ";qXfer:libraries:read+"); + AppendReply(m_reply_packet, ";qXfer:threads:read+"); + AppendReply(m_reply_packet, ";swbreak+"); + AppendReply(m_reply_packet, ";hwbreak+"); + } + + void GdbPacketParser::qXferFeaturesRead() { + u32 offset, length; + + if (ParsePrefix(m_receive_packet, "target.xml:")) { + /* Parse offset/length. */ + ParseOffsetLength(m_receive_packet, offset, length); + + /* Send the desired xml. */ + std::strncpy(m_reply_packet, (this->Is64Bit() ? TargetXmlAarch64 : TargetXmlAarch32) + offset, length); + m_reply_packet[length] = 0; + m_reply_packet += std::strlen(m_reply_packet); + } else if (ParsePrefix(m_receive_packet, "aarch64-core.xml:")) { + /* Parse offset/length. */ + ParseOffsetLength(m_receive_packet, offset, length); + + /* Send the desired xml. */ + std::strncpy(m_reply_packet, Aarch64CoreXml + offset, length); + m_reply_packet[length] = 0; + m_reply_packet += std::strlen(m_reply_packet); + } else if (ParsePrefix(m_receive_packet, "aarch64-fpu.xml:")) { + /* Parse offset/length. */ + ParseOffsetLength(m_receive_packet, offset, length); + + /* Send the desired xml. */ + std::strncpy(m_reply_packet, Aarch64FpuXml + offset, length); + m_reply_packet[length] = 0; + m_reply_packet += std::strlen(m_reply_packet); + } else if (ParsePrefix(m_receive_packet, "arm-core.xml:")) { + /* Parse offset/length. */ + ParseOffsetLength(m_receive_packet, offset, length); + + /* Send the desired xml. */ + std::strncpy(m_reply_packet, ArmCoreXml + offset, length); + m_reply_packet[length] = 0; + m_reply_packet += std::strlen(m_reply_packet); + } else if (ParsePrefix(m_receive_packet, "arm-vfp.xml:")) { + /* Parse offset/length. */ + ParseOffsetLength(m_receive_packet, offset, length); + + /* Send the desired xml. */ + std::strncpy(m_reply_packet, ArmVfpXml + offset, length); + m_reply_packet[length] = 0; + m_reply_packet += std::strlen(m_reply_packet); + } else { + AMS_DMNT2_GDB_LOG_DEBUG("Not Implemented qxfer:features:read: %s\n", m_receive_packet); + } + } + +} \ No newline at end of file diff --git a/stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.hpp b/stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.hpp new file mode 100644 index 000000000..6bd1a6d2a --- /dev/null +++ b/stratosphere/dmnt.gen2/source/dmnt2_gdb_packet_parser.hpp @@ -0,0 +1,42 @@ +/* + * Copyright (c) 2018-2020 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#include +#include "dmnt2_gdb_packet_io.hpp" + +namespace ams::dmnt { + + class GdbPacketParser { + private: + char *m_receive_packet; + char *m_reply_packet; + char m_buffer[GdbPacketBufferSize / 2]; + bool m_64_bit; + bool m_no_ack; + public: + GdbPacketParser(char *recv, char *reply, bool is_64_bit) : m_receive_packet(recv), m_reply_packet(reply), m_64_bit(is_64_bit), m_no_ack(false) { /* ... */ } + + void Process(); + + bool Is64Bit() const { return m_64_bit; } + private: + void q(); + + void qSupported(); + void qXferFeaturesRead(); + }; + +} \ No newline at end of file diff --git a/stratosphere/dmnt.gen2/source/dmnt2_gdb_server.cpp b/stratosphere/dmnt.gen2/source/dmnt2_gdb_server.cpp index 17b0c3df9..8758d9f54 100644 --- a/stratosphere/dmnt.gen2/source/dmnt2_gdb_server.cpp +++ b/stratosphere/dmnt.gen2/source/dmnt2_gdb_server.cpp @@ -15,6 +15,7 @@ */ #include #include "dmnt2_gdb_packet_io.hpp" +#include "dmnt2_gdb_packet_parser.hpp" #include "dmnt2_gdb_server.hpp" #include "dmnt2_debug_log.hpp" @@ -22,20 +23,19 @@ namespace ams::dmnt { namespace { - constexpr size_t ServerThreadStackSize = util::AlignUp(3 * GdbPacketBufferSize + os::MemoryPageSize, os::ThreadStackAlignment); + constexpr size_t ServerThreadStackSize = util::AlignUp(4 * GdbPacketBufferSize + os::MemoryPageSize, os::ThreadStackAlignment); - alignas(os::ThreadStackAlignment) constinit u8 g_server_thread_stack[ServerThreadStackSize]; - constinit os::ThreadType g_server_thread; + alignas(os::ThreadStackAlignment) constinit u8 g_server_thread32_stack[ServerThreadStackSize]; + alignas(os::ThreadStackAlignment) constinit u8 g_server_thread64_stack[ServerThreadStackSize]; - constinit util::TypedStorage g_session; + constinit os::ThreadType g_server_thread32; + constinit os::ThreadType g_server_thread64; - void OnClientSocketAccepted(int fd) { - /* Create htcs session for the socket. */ - util::ConstructAt(g_session, fd); - ON_SCOPE_EXIT { util::DestroyAt(g_session); }; + constinit util::TypedStorage g_session32; + constinit util::TypedStorage g_session64; - HtcsSession *session = util::GetPointer(g_session); + void ProcessForHtcsSession(HtcsSession *session, bool is_64_bit) { /* Create packet io handler. */ GdbPacketIo packet_io; @@ -47,15 +47,21 @@ namespace ams::dmnt { char *packet = packet_io.ReceivePacket(std::addressof(do_break), recv_buf, sizeof(recv_buf), session); if (!do_break && packet != nullptr) { - AMS_DMNT2_GDB_LOG_DEBUG("Received Packet %s\n", packet); + /* Create a packet parser. */ + char reply_buffer[GdbPacketBufferSize]; + GdbPacketParser parser(packet, reply_buffer, is_64_bit); - /* TODO: Process packets. */ - packet_io.SendPacket(std::addressof(do_break), "OK", session); + /* Process the packet. */ + parser.Process(); + + /* Send packet. */ + packet_io.SendPacket(std::addressof(do_break), reply_buffer, session); } } } - void GdbServerThreadFunction(void *) { + template + void GdbServerThreadFunction() { /* Loop forever, servicing our gdb server. */ while (true) { /* Get a socket. */ @@ -74,7 +80,7 @@ namespace ams::dmnt { htcs::SockAddrHtcs addr; addr.family = htcs::HTCS_AF_HTCS; addr.peer_name = htcs::GetPeerNameAny(); - std::strcpy(addr.port_name.name, "iywys@$gdb"); + std::strcpy(addr.port_name.name, Is64Bit ? "iywys@$gdb-aarch64" : "iywys@$gdb-aarch32"); /* Bind. */ if (htcs::Bind(fd, std::addressof(addr)) == -1) { @@ -91,8 +97,13 @@ namespace ams::dmnt { break; } - /* Handle the client. */ - OnClientSocketAccepted(client_fd); + /* Create htcs session for the socket. */ + auto &session_storage = Is64Bit ? g_session64 : g_session32; + util::ConstructAt(session_storage, client_fd); + ON_SCOPE_EXIT { util::DestroyAt(session_storage); }; + + /* Process for the session. */ + ProcessForHtcsSession(util::GetPointer(session_storage), Is64Bit); /* Close the client socket. */ htcs::Close(client_fd); @@ -101,14 +112,22 @@ namespace ams::dmnt { } } + void GdbServerThreadFunction64(void *) { + GdbServerThreadFunction(); + } + void GdbServerThreadFunction32(void *) { + GdbServerThreadFunction(); + } } void InitializeGdbServer() { - /* Create and start gdb server thread. */ - R_ABORT_UNLESS(os::CreateThread(std::addressof(g_server_thread), GdbServerThreadFunction, nullptr, g_server_thread_stack, sizeof(g_server_thread_stack), os::HighestThreadPriority - 1)); - os::StartThread(std::addressof(g_server_thread)); + /* Create and start gdb server threads. */ + R_ABORT_UNLESS(os::CreateThread(std::addressof(g_server_thread64), GdbServerThreadFunction64, nullptr, g_server_thread64_stack, sizeof(g_server_thread64_stack), os::HighestThreadPriority - 1)); + R_ABORT_UNLESS(os::CreateThread(std::addressof(g_server_thread32), GdbServerThreadFunction32, nullptr, g_server_thread32_stack, sizeof(g_server_thread32_stack), os::HighestThreadPriority - 1)); + os::StartThread(std::addressof(g_server_thread64)); + os::StartThread(std::addressof(g_server_thread32)); } }