From be34025af1889cb20c4290c4e07fc7d083331dd9 Mon Sep 17 00:00:00 2001
From: Nav <nav@navnav.me>
Date: Mon, 4 Oct 2021 21:31:31 +0100
Subject: [PATCH] Implemented driver-side masked memory reading in
 EdbgAvr8Interface driver

---
 .../EDBG/AVR/EdbgAvr8Interface.cpp            | 42 +++++++++++++++++++
 .../EDBG/AVR/EdbgAvr8Interface.hpp            | 14 +++++++
 2 files changed, 56 insertions(+)

diff --git a/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.cpp b/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.cpp
index 4db23cbe..b43138ef 100644
--- a/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.cpp
+++ b/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.cpp
@@ -963,6 +963,48 @@ TargetMemoryBuffer EdbgAvr8Interface::readMemory(
     std::uint32_t bytes,
     std::set<std::uint32_t> excludedAddresses
 ) {
+    if (!excludedAddresses.empty() && this->avoidMaskedMemoryRead) {
+        /*
+         * Driver-side masked memory read.
+         *
+         * Split the read into numerous reads, whenever we encounter an excluded address.
+         *
+         * All values for bytes located at excluded addresses will be returned as 0x00 - this mirrors the behaviour
+         * of the masked read memory command.
+         */
+        auto output = TargetMemoryBuffer();
+        auto segmentStartAddress = address;
+
+        for (std::uint32_t i = 0; i < bytes; i++) {
+            const auto byteAddress = address + i;
+
+            if (excludedAddresses.contains(byteAddress)) {
+                auto segmentBuffer = this->readMemory(
+                    type,
+                    segmentStartAddress,
+                    (byteAddress - segmentStartAddress)
+                );
+
+                output.insert(output.end(), segmentBuffer.begin(), segmentBuffer.end());
+                output.emplace_back(0x00);
+
+                segmentStartAddress = byteAddress + 1;
+
+            } else if (i == (bytes - 1)) {
+                // Read final segment
+                auto segmentBuffer = this->readMemory(
+                    type,
+                    segmentStartAddress,
+                    (byteAddress - segmentStartAddress + 1)
+                );
+
+                output.insert(output.end(), segmentBuffer.begin(), segmentBuffer.end());
+            }
+        }
+
+        return output;
+    }
+
     if (type == Avr8MemoryType::FLASH_PAGE) {
         if (this->targetParameters.flashPageSize.value_or(0) < 1) {
             throw Exception("Missing/invalid flash page size parameter");
diff --git a/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.hpp b/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.hpp
index 647ab7aa..bb55b4d8 100644
--- a/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.hpp
+++ b/src/DebugToolDrivers/Protocols/CMSIS-DAP/VendorSpecific/EDBG/AVR/EdbgAvr8Interface.hpp
@@ -72,6 +72,16 @@ namespace Bloom::DebugToolDrivers::Protocols::CmsisDap::Edbg::Avr
          */
         Targets::Microchip::Avr::Avr8Bit::TargetParameters targetParameters;
 
+        /**
+         * Some EDBG devices don't seem to operate correctly when actioning the masked memory read command. The data
+         * returned in response to the command appears to be completely incorrect. This appears to only occur with
+         * the MPLAB Snap device.
+         *
+         * Setting this flag to true will mean we implement our own masked memory read, in this driver, as opposed to
+         * employing the masked memory read command.
+         */
+        bool avoidMaskedMemoryRead = false;
+
         /**
          * We keep record of the current target state for caching purposes. We'll only refresh the target state if the
          * target is running. If it has already stopped, then we assume it cannot transition to a running state without
@@ -451,6 +461,10 @@ namespace Bloom::DebugToolDrivers::Protocols::CmsisDap::Edbg::Avr
         explicit EdbgAvr8Interface(EdbgInterface& edbgInterface)
         : edbgInterface(edbgInterface) {};
 
+        void setAvoidMaskedMemoryRead(bool avoidMaskedMemoryRead) {
+            this->avoidMaskedMemoryRead = avoidMaskedMemoryRead;
+        }
+
         /*
          * The public methods below implement the interface defined by the Avr8Interface class.
          * See the comments in that class for more info on the expected behaviour of each method.