New wrb GDB monitor command, for writing to individual bit fields of target registers

src/DebugServer/CMakeLists.txt

@@ -23,6 +23,7 @@ target_sources(
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/ListRegistersMonitor.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/ReadRegistersMonitor.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/WriteRegisterMonitor.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/WriteRegisterBitFieldMonitor.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/VContContinueExecution.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/VContStepExecution.cpp
 

src/DebugServer/Gdb/CommandPackets/HelpMonitorInfo.cpp

@@ -115,6 +115,31 @@ namespace DebugServer::Gdb::CommandPackets
             + StringService::applyTerminalColor("0x40", PARAM_COLOR) + "\n";
         output += leftPadding + "  Same as above, excluding the register group key.\n\n";
 
+        output += StringService::applyTerminalColor("wrb", CMD_COLOR) + " ["
+            + StringService::applyTerminalColor("PERIPHERAL_KEY", PARAM_COLOR) + "] ["
+            + StringService::applyTerminalColor("ABS_REG_GROUP_KEY", PARAM_COLOR) + "] ["
+            + StringService::applyTerminalColor("REG_KEY", PARAM_COLOR) + "] ["
+            + StringService::applyTerminalColor("BIT_FIELD_KEY", PARAM_COLOR) + "] ["
+            + StringService::applyTerminalColor("VALUE_BINARY", PARAM_COLOR) + "]\n\n";
+        output += leftPadding + "Applies the given bit field value to the current value of the given register. The value\n";
+        output += leftPadding + "must take the form of a binary string. The register group key can be omitted if the\n";
+        output += leftPadding + "peripheral contains a single register group, and the subject register resides directly within\n";
+        output += leftPadding + "that group (not in any subgroup).\n\n";
+        output += leftPadding + "Examples:\n\n";
+        output += leftPadding + "mon " + StringService::applyTerminalColor("wrb", CMD_COLOR) + " "
+            + StringService::applyTerminalColor("rcc", PARAM_COLOR) + " "
+            + StringService::applyTerminalColor("rcc", PARAM_COLOR) + " "
+            + StringService::applyTerminalColor("ctlr", PARAM_COLOR) + " "
+            + StringService::applyTerminalColor("hsion", PARAM_COLOR) + " "
+            + StringService::applyTerminalColor("0b1", PARAM_COLOR) + "\n";
+        output += leftPadding + "  Sets the `hsion` bit field to 0b1, in the `ctlr` register, in the `rcc` register group, in the `rcc` peripheral.\n\n";
+        output += leftPadding + "mon " + StringService::applyTerminalColor("wrb", CMD_COLOR) + " "
+            + StringService::applyTerminalColor("rcc", PARAM_COLOR) + " "
+            + StringService::applyTerminalColor("ctlr", PARAM_COLOR) + " "
+            + StringService::applyTerminalColor("hsion", PARAM_COLOR) + " "
+            + StringService::applyTerminalColor("1", PARAM_COLOR) + "\n";
+        output += leftPadding + "  Same as above, excluding the register group key and the \"0b\" prefix in the given value.\n\n";
+
         if (targetDescriptor.family == Targets::TargetFamily::AVR_8) {
             output += StringService::applyTerminalColor("eeprom fill", CMD_COLOR) + " ["
                 + StringService::applyTerminalColor("FILL_VALUE_HEX", PARAM_COLOR) + "]\n\n";

src/DebugServer/Gdb/CommandPackets/WriteRegisterBitFieldMonitor.cpp

@@ -0,0 +1,214 @@
+#include "WriteRegisterBitFieldMonitor.hpp"
+
+#include <cstdint>
+#include <cmath>
+
+#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/Targets/DynamicRegisterValue.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;
+
+    WriteRegisterBitFieldMonitor::WriteRegisterBitFieldMonitor(Monitor&& monitorPacket)
+        : Monitor(std::move(monitorPacket))
+    {}
+
+    void WriteRegisterBitFieldMonitor::handle(
+        DebugSession& debugSession,
+        const TargetDescriptor& gdbTargetDescriptor,
+        const Targets::TargetDescriptor& targetDescriptor,
+        TargetControllerService& targetControllerService
+    ) {
+        Logger::info("Handling WriteRegisterBitFieldMonitor 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 Targets::TargetRegisterGroupDescriptor>
+            >{};
+
+            const auto registerGroupKeyProvided = argCount >= 6;
+            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{"Bit field key required"};
+            }
+
+            const auto& bitFieldKey = registerGroupKeyProvided ? this->commandArguments[4] : this->commandArguments[3];
+            const auto bitFieldDescriptorOpt = registerDescriptor.tryGetBitFieldDescriptor(bitFieldKey);
+            if (!bitFieldDescriptorOpt.has_value()) {
+                throw Exception{"Unknown bit field key \"" + bitFieldKey + "\""};
+            }
+
+            const auto& bitFieldDescriptor = bitFieldDescriptorOpt->get();
+
+            if (argCount < (registerGroupKeyProvided ? 6 : 5)) {
+                throw Exception{"Bit field value required"};
+            }
+
+            auto rawBitFieldValue = StringService::asciiToLower(
+                registerGroupKeyProvided ? this->commandArguments[5] : this->commandArguments[4]
+            );
+            if (rawBitFieldValue.find("0b") == 0) {
+                rawBitFieldValue = rawBitFieldValue.substr(2);
+            }
+
+            if (!StringService::isBinary(rawBitFieldValue)) {
+                throw Exception{"Invalid bit field value given"};
+            }
+
+            const auto bitFieldWidth = bitFieldDescriptor.width();
+            if (rawBitFieldValue.size() > bitFieldWidth) {
+                throw Exception{
+                    "The width of the given value (" + std::to_string(rawBitFieldValue.size())
+                        + " bit(s)) exceeds that of the given bit field (" + std::to_string(bitFieldWidth) + " bit(s))"
+                };
+            }
+
+            const auto bitFieldValue = StringService::toUint64(rawBitFieldValue, 2);
+
+            auto dynamicValue = Targets::DynamicRegisterValue{
+                registerDescriptor,
+                targetControllerService.readRegister(registerDescriptor)
+            };
+
+            debugSession.connection.writePacket(PartialResponsePacket{StringService::toHex(
+                "Applying bit field value " + StringService::applyTerminalColor(
+                    "0b" + rawBitFieldValue,
+                    StringService::TerminalColor::DARK_YELLOW
+                ) + " to \"" + bitFieldDescriptor.name + "\" bit field, to \"" + registerDescriptor.name
+                + "\" register, via `" + registerDescriptor.addressSpaceKey + "` address space...\n"
+            )});
+
+            const auto initialValueHex = StringService::toHex(
+                dynamicValue.value
+            ).substr(16 - (registerDescriptor.size * 2));
+            Logger::debug("Initial register value: 0x" + initialValueHex);
+            debugSession.connection.writePacket(PartialResponsePacket{StringService::toHex(
+                "Initial register value " + StringService::applyTerminalColor(
+                    "0x" + initialValueHex,
+                    StringService::TerminalColor::DARK_YELLOW
+                ) + "\n"
+            )});
+            dynamicValue.setBitField(bitFieldDescriptor, bitFieldValue);
+
+            const auto newValueHex = StringService::toHex(
+                dynamicValue.value
+            ).substr(16 - (registerDescriptor.size * 2));
+            Logger::debug("New register value: 0x" + newValueHex);
+            debugSession.connection.writePacket(PartialResponsePacket{StringService::toHex(
+                "New register value " + StringService::applyTerminalColor(
+                    "0x" + newValueHex,
+                    StringService::TerminalColor::DARK_YELLOW
+                ) + "\n\n"
+            )});
+
+            if (dynamicValue.value > static_cast<std::uint64_t>(std::pow(2, (registerDescriptor.size * 8)))) {
+                throw Exception{
+                    "The new value (0x" + newValueHex + ") exceeds the size of the register ("
+                        + std::to_string(registerDescriptor.size * 8) + "-bit)"
+                };
+            }
+
+            debugSession.connection.writePacket(PartialResponsePacket{StringService::toHex(
+                "Writing value " + StringService::applyTerminalColor(
+                    "0x" + newValueHex,
+                    StringService::TerminalColor::DARK_YELLOW
+                ) + " (" +  std::to_string(registerDescriptor.size * 8) + "-bit"  + ") 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, dynamicValue.data());
+            debugSession.connection.writePacket(ResponsePacket{StringService::toHex("Register written\n")});
+
+        } catch (const std::invalid_argument& exception) {
+            debugSession.connection.writePacket(ResponsePacket{
+                StringService::toHex(
+                    StringService::applyTerminalColor(
+                        "Error: Invalid bit field value given\n",
+                        StringService::TerminalColor::DARK_RED
+                    )
+                )
+            });
+
+        } catch (const Exception& exception) {
+            debugSession.connection.writePacket(ResponsePacket{
+                StringService::toHex(
+                    StringService::applyTerminalColor(
+                        "Error: " + exception.getMessage() + "\n",
+                        StringService::TerminalColor::DARK_RED
+                    )
+                )
+            });
+        }
+    }
+}

src/DebugServer/Gdb/CommandPackets/WriteRegisterBitFieldMonitor.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "Monitor.hpp"
+
+namespace DebugServer::Gdb::CommandPackets
+{
+    class WriteRegisterBitFieldMonitor: public Monitor
+    {
+    public:
+        explicit WriteRegisterBitFieldMonitor(Monitor&& monitorPacket);
+
+        void handle(
+            DebugSession& debugSession,
+            const TargetDescriptor& gdbTargetDescriptor,
+            const Targets::TargetDescriptor& targetDescriptor,
+            Services::TargetControllerService& targetControllerService
+        ) override;
+    };
+}

src/DebugServer/Gdb/GdbRspDebugServer.hpp

@@ -38,6 +38,7 @@
 #include "CommandPackets/ListRegistersMonitor.hpp"
 #include "CommandPackets/ReadRegistersMonitor.hpp"
 #include "CommandPackets/WriteRegisterMonitor.hpp"
+#include "CommandPackets/WriteRegisterBitFieldMonitor.hpp"
 #include "CommandPackets/VContContinueExecution.hpp"
 #include "CommandPackets/VContStepExecution.hpp"
 
@@ -494,6 +495,10 @@ namespace DebugServer::Gdb
                     return std::make_unique<CommandPackets::ReadRegistersMonitor>(std::move(*(monitorCommand.release())));
                 }
 
+                if (monitorCommand->command.find("wrb") == 0) {
+                    return std::make_unique<CommandPackets::WriteRegisterBitFieldMonitor>(std::move(*(monitorCommand.release())));
+                }
+
                 if (monitorCommand->command.find("wr") == 0) {
                     return std::make_unique<CommandPackets::WriteRegisterMonitor>(std::move(*(monitorCommand.release())));
                 }

src/Services/StringService.cpp

@@ -54,7 +54,13 @@ namespace Services
 
     bool StringService::isNumeric(std::string_view str) {
         return !std::any_of(str.begin(), str.end(), [] (unsigned char character) {
-            return std::isdigit(character);
+            return !std::isdigit(character);
+        });
+    }
+
+    bool StringService::isBinary(std::string_view str) {
+        return !std::any_of(str.begin(), str.end(), [] (unsigned char character) {
+            return character != '0' && character != '1';
         });
     }
 

src/Services/StringService.hpp

@@ -21,6 +21,7 @@ namespace Services
         static std::string replaceUnprintable(std::string_view str);
 
         static bool isNumeric(std::string_view str);
+        static bool isBinary(std::string_view str);
 
         static std::string toHex(std::uint64_t value);
         static std::string toHex(std::uint32_t value);

src/Targets/TargetBitFieldDescriptor.cpp

@@ -1,5 +1,9 @@
 #include "TargetBitFieldDescriptor.hpp"
 
+#include <bitset>
+#include <limits>
+#include <numeric>
+
 namespace Targets
 {
     TargetBitFieldDescriptor::TargetBitFieldDescriptor(
@@ -14,6 +18,21 @@ namespace Targets
         , description(description)
     {}
 
+    std::size_t TargetBitFieldDescriptor::width() const {
+        const auto maskBitset = std::bitset<std::numeric_limits<decltype(TargetBitFieldDescriptor::mask)>::digits>{
+            this->mask
+        };
+
+        auto width = std::size_t{0};
+        for (auto maskIndex = std::size_t{0}; maskIndex < maskBitset.size(); ++maskIndex) {
+            if (maskBitset[maskIndex]) {
+                ++width;
+            }
+        }
+
+        return width;
+    }
+
     TargetBitFieldDescriptor TargetBitFieldDescriptor::clone() const {
         return {*this};
     }

src/Targets/TargetBitFieldDescriptor.hpp

@@ -28,6 +28,8 @@ namespace Targets
         TargetBitFieldDescriptor(TargetBitFieldDescriptor&& other) noexcept = default;
         TargetBitFieldDescriptor& operator = (TargetBitFieldDescriptor&& other) = default;
 
+        std::size_t width() const;
+
         [[nodiscard]] TargetBitFieldDescriptor clone() const;
 
     private: