From eda6fe0c7fdb81eae103d191d804b65e01c88097 Mon Sep 17 00:00:00 2001
From: Nav <nav@navnav.me>
Date: Sat, 25 Nov 2023 19:06:23 +0000
Subject: [PATCH] Enable/disable debug module on activation/deactivation of
 RISC-V targets

---
 src/Targets/RiscV/RiscV.cpp | 52 +++++++++++++++++++++++++++++++++++++
 src/Targets/RiscV/RiscV.hpp |  3 +++
 2 files changed, 55 insertions(+)

diff --git a/src/Targets/RiscV/RiscV.cpp b/src/Targets/RiscV/RiscV.cpp
index 58ac0d11..d481322a 100644
--- a/src/Targets/RiscV/RiscV.cpp
+++ b/src/Targets/RiscV/RiscV.cpp
@@ -75,6 +75,13 @@ namespace Targets::RiscV
         this->selectedHartIndex = *(this->hartIndices.begin());
         Logger::info("Selected RISC-V hart index: " + std::to_string(this->selectedHartIndex));
 
+        /*
+         * Disabling the debug module before enabling it will clear any state from a previous debug session that
+         * wasn't terminated properly.
+         */
+        this->disableDebugModule();
+        this->enableDebugModule();
+
         this->stop();
 
         auto debugControlStatusRegister = this->readDebugControlStatusRegister();
@@ -86,6 +93,11 @@ namespace Targets::RiscV
     }
 
     void RiscV::deactivate() {
+        if (this->getState() != TargetState::RUNNING) {
+            this->run();
+        }
+
+        this->disableDebugModule();
         this->riscVDebugInterface->deactivate();
     }
 
@@ -387,6 +399,46 @@ namespace Targets::RiscV
         return DebugControlStatusRegister(this->readRegister(Registers::RegisterNumber::DEBUG_CONTROL_STATUS_REGISTER));
     }
 
+    void RiscV::enableDebugModule() {
+        auto controlRegister = ControlRegister();
+        controlRegister.debugModuleActive = true;
+        controlRegister.selectedHartIndex = this->selectedHartIndex;
+
+        this->writeDebugModuleControlRegister(controlRegister);
+
+        constexpr auto maxAttempts = 10;
+        controlRegister = this->readDebugModuleControlRegister();
+
+        for (auto attempts = 1; !controlRegister.debugModuleActive && attempts <= maxAttempts; ++attempts) {
+            std::this_thread::sleep_for(std::chrono::microseconds(10));
+            controlRegister = this->readDebugModuleControlRegister();
+        }
+
+        if (!controlRegister.debugModuleActive) {
+            throw Exceptions::Exception("Took too long to enable debug module");
+        }
+    }
+
+    void RiscV::disableDebugModule() {
+        auto controlRegister = ControlRegister();
+        controlRegister.debugModuleActive = false;
+        controlRegister.selectedHartIndex = this->selectedHartIndex;
+
+        this->writeDebugModuleControlRegister(controlRegister);
+
+        constexpr auto maxAttempts = 10;
+        controlRegister = this->readDebugModuleControlRegister();
+
+        for (auto attempts = 1; controlRegister.debugModuleActive && attempts <= maxAttempts; ++attempts) {
+            std::this_thread::sleep_for(std::chrono::microseconds(10));
+            controlRegister = this->readDebugModuleControlRegister();
+        }
+
+        if (controlRegister.debugModuleActive) {
+            throw Exceptions::Exception("Took too long to disable debug module");
+        }
+    }
+
     RegisterValue RiscV::readRegister(RegisterNumber number) {
         using DebugModule::Registers::RegisterAccessControlField;
 
diff --git a/src/Targets/RiscV/RiscV.hpp b/src/Targets/RiscV/RiscV.hpp
index 70df14ed..863d1174 100644
--- a/src/Targets/RiscV/RiscV.hpp
+++ b/src/Targets/RiscV/RiscV.hpp
@@ -118,6 +118,9 @@ namespace Targets::RiscV
 
         Registers::DebugControlStatusRegister readDebugControlStatusRegister();
 
+        void enableDebugModule();
+        void disableDebugModule();
+
         RegisterValue readRegister(RegisterNumber number);
         RegisterValue readRegister(Registers::RegisterNumber number);
         void writeRegister(RegisterNumber number, RegisterValue value);