diff --git a/src/DebugServer/CMakeLists.txt b/src/DebugServer/CMakeLists.txt index 01b405b4..b3b0c747 100755 --- a/src/DebugServer/CMakeLists.txt +++ b/src/DebugServer/CMakeLists.txt @@ -25,6 +25,7 @@ target_sources( ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/Detach.cpp ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/ListRegistersMonitor.cpp ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/ReadRegistersMonitor.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/WriteRegisterMonitor.cpp # AVR GDB RSP Server ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/AvrGdb/AvrGdbRsp.cpp diff --git a/src/DebugServer/Gdb/CommandPackets/WriteRegisterMonitor.cpp b/src/DebugServer/Gdb/CommandPackets/WriteRegisterMonitor.cpp new file mode 100644 index 00000000..8476903a --- /dev/null +++ b/src/DebugServer/Gdb/CommandPackets/WriteRegisterMonitor.cpp @@ -0,0 +1,145 @@ +#include "WriteRegisterMonitor.hpp" + +#include +#include + +#include "src/DebugServer/Gdb/ResponsePackets/ErrorResponsePacket.hpp" +#include "src/DebugServer/Gdb/ResponsePackets/PartialResponsePacket.hpp" +#include "src/DebugServer/Gdb/ResponsePackets/ResponsePacket.hpp" + +#include "src/Services/StringService.hpp" +#include "src/Logger/Logger.hpp" + +#include "src/Targets/TargetMemory.hpp" +#include "src/Targets/TargetRegisterGroupDescriptor.hpp" + +#include "src/Exceptions/Exception.hpp" + +namespace DebugServer::Gdb::CommandPackets +{ + using Services::TargetControllerService; + using Services::StringService; + + using ResponsePackets::ErrorResponsePacket; + using ResponsePackets::PartialResponsePacket; + using ResponsePackets::ResponsePacket; + + using ::Exceptions::Exception; + + WriteRegisterMonitor::WriteRegisterMonitor(Monitor&& monitorPacket) + : Monitor(std::move(monitorPacket)) + {} + + void WriteRegisterMonitor::handle( + DebugSession& debugSession, + const TargetDescriptor& gdbTargetDescriptor, + const Targets::TargetDescriptor& targetDescriptor, + TargetControllerService& targetControllerService + ) { + Logger::info("Handling WriteRegisterMonitor packet"); + + try { + const auto argCount = this->commandArguments.size(); + if (argCount < 2) { + throw Exception{"Peripheral key required"}; + } + + const auto& peripheralKey = this->commandArguments[1]; + + const auto peripheralDescriptorOpt = targetDescriptor.tryGetPeripheralDescriptor(peripheralKey); + if (!peripheralDescriptorOpt.has_value()) { + throw Exception{"Unknown peripheral key \"" + peripheralKey + "\""}; + } + + const auto& peripheralDescriptor = peripheralDescriptorOpt->get(); + + auto registerGroupDescriptorOpt = std::optional< + std::reference_wrapper + >{}; + + const auto registerGroupKeyProvided = argCount >= 5; + if (registerGroupKeyProvided) { + const auto& registerGroupKey = this->commandArguments[2]; + registerGroupDescriptorOpt = peripheralDescriptor.tryGetRegisterGroupDescriptor(registerGroupKey); + if (!registerGroupDescriptorOpt.has_value()) { + throw Exception{"Unknown absolute register group key \"" + registerGroupKey + "\""}; + } + + } else { + if (peripheralDescriptor.registerGroupDescriptorsByKey.size() != 1) { + throw Exception{"Absolute register group key required"}; + } + + registerGroupDescriptorOpt = peripheralDescriptor.registerGroupDescriptorsByKey.begin()->second; + } + + const auto& registerGroupDescriptor = registerGroupDescriptorOpt->get(); + + if (argCount < (registerGroupKeyProvided ? 4 : 3)) { + throw Exception{"Register key required"}; + } + + const auto& registerKey = registerGroupKeyProvided ? this->commandArguments[3] : this->commandArguments[2]; + const auto registerDescriptorOpt = registerGroupDescriptor.tryGetRegisterDescriptor(registerKey); + if (!registerDescriptorOpt.has_value()) { + throw Exception{"Unknown register key \"" + registerKey + "\""}; + } + + const auto& registerDescriptor = registerDescriptorOpt->get(); + + if (!registerDescriptor.access.writable) { + throw Exception{registerDescriptor.name + " register is not writeable"}; + } + + if (registerDescriptor.size > 8) { + throw Exception{"Unsupported register size"}; + } + + if (argCount < (registerGroupKeyProvided ? 5 : 4)) { + throw Exception{"Register value required"}; + } + + const auto& rawValue = registerGroupKeyProvided ? this->commandArguments[4] : this->commandArguments[3]; + const auto value = StringService::toUint64(rawValue, 16); + + if (value > static_cast(std::pow(2, (registerDescriptor.size * 8)))) { + throw Exception{ + "The given value (0x" + StringService::toHex(value) + ") exceeds the size of the register (" + + std::to_string(registerDescriptor.size) + " byte(s))" + }; + } + + auto buffer = Targets::TargetMemoryBuffer{}; + buffer.reserve(registerDescriptor.size); + + for (auto i = std::uint8_t{0}; i < registerDescriptor.size; ++i) { + buffer.insert(buffer.begin(), static_cast(value >> (registerDescriptor.size * 8 * i))); + } + + debugSession.connection.writePacket(PartialResponsePacket{StringService::toHex( + "Writing value " + StringService::applyTerminalColor( + "0x" + StringService::toHex(value).substr(16 - (registerDescriptor.size * 2)), + StringService::TerminalColor::DARK_YELLOW + ) + " (" + std::to_string(buffer.size()) + " byte(s)" + ") to " + registerDescriptor.name + + " register, at address " + StringService::applyTerminalColor( + "0x" + StringService::toHex(registerDescriptor.startAddress), + StringService::TerminalColor::BLUE + ) + ", via `" + registerDescriptor.addressSpaceKey + "` address space...\n" + )}); + + targetControllerService.writeRegister(registerDescriptor, buffer); + + debugSession.connection.writePacket(ResponsePacket{StringService::toHex("Register written\n")}); + + } catch (const Exception& exception) { + debugSession.connection.writePacket(ResponsePacket{ + StringService::toHex( + StringService::applyTerminalColor( + "Error: " + exception.getMessage() + "\n", + StringService::TerminalColor::DARK_RED + ) + ) + }); + } + } +} diff --git a/src/DebugServer/Gdb/CommandPackets/WriteRegisterMonitor.hpp b/src/DebugServer/Gdb/CommandPackets/WriteRegisterMonitor.hpp new file mode 100644 index 00000000..0207340f --- /dev/null +++ b/src/DebugServer/Gdb/CommandPackets/WriteRegisterMonitor.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include "Monitor.hpp" + +namespace DebugServer::Gdb::CommandPackets +{ + class WriteRegisterMonitor: public Monitor + { + public: + explicit WriteRegisterMonitor(Monitor&& monitorPacket); + + void handle( + DebugSession& debugSession, + const TargetDescriptor& gdbTargetDescriptor, + const Targets::TargetDescriptor& targetDescriptor, + Services::TargetControllerService& targetControllerService + ) override; + }; +} diff --git a/src/DebugServer/Gdb/GdbRspDebugServer.cpp b/src/DebugServer/Gdb/GdbRspDebugServer.cpp index 046362f7..ec67b660 100644 --- a/src/DebugServer/Gdb/GdbRspDebugServer.cpp +++ b/src/DebugServer/Gdb/GdbRspDebugServer.cpp @@ -31,6 +31,7 @@ #include "CommandPackets/Detach.hpp" #include "CommandPackets/ListRegistersMonitor.hpp" #include "CommandPackets/ReadRegistersMonitor.hpp" +#include "CommandPackets/WriteRegisterMonitor.hpp" #ifndef EXCLUDE_INSIGHT #include "CommandPackets/ActivateInsight.hpp" @@ -301,6 +302,10 @@ namespace DebugServer::Gdb return std::make_unique(std::move(*(monitorCommand.release()))); } + if (monitorCommand->command.find("wr") == 0) { + return std::make_unique(std::move(*(monitorCommand.release()))); + } + #ifndef EXCLUDE_INSIGHT if (monitorCommand->command.find("insight") == 0) { return std::make_unique(std::move(*(monitorCommand.release())));