From c151f1978d16407e080f218bde9405014a6cf2cf Mon Sep 17 00:00:00 2001
From: Rafael Silva <perigoso@riseup.net>
Date: Thu, 23 Feb 2023 01:11:42 +0000
Subject: [PATCH] riscv_debug: provide abstraction for dtm related routines for
 attach and detach

---
 src/target/riscv_debug.c | 32 +++++++++++++++++++++++++++-----
 src/target/riscv_debug.h |  2 ++
 2 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/src/target/riscv_debug.c b/src/target/riscv_debug.c
index 351f4cbd4ba..5580a3f6de6 100644
--- a/src/target/riscv_debug.c
+++ b/src/target/riscv_debug.c
@@ -210,6 +210,8 @@ static void riscv_hart_free(void *priv);
 static uint32_t riscv_shift_dtmcs(const riscv_dtm_s *dtm, uint32_t control);
 static bool riscv_jtag_dmi_read(riscv_dtm_s *dtm, uint32_t address, uint32_t *value);
 static bool riscv_jtag_dmi_write(riscv_dtm_s *dtm, uint32_t address, uint32_t value);
+static void riscv_jtag_attach(target_s *target);
+static void riscv_jtag_detach(target_s *target);
 static void riscv_dm_ref(riscv_dm_s *dbg_module);
 static void riscv_dm_unref(riscv_dm_s *dbg_module);
 static riscv_debug_version_e riscv_dtmcs_version(uint32_t dtmcs);
@@ -251,6 +253,8 @@ void riscv_debug_dtm_handler(const uint8_t dev_index)
 
 	dtm->dmi_read = riscv_jtag_dmi_read;
 	dtm->dmi_write = riscv_jtag_dmi_write;
+	dtm->dtm_attach = riscv_jtag_attach;
+	dtm->dtm_detach = riscv_jtag_detach;
 
 	/* Setup and try to discover the DMI bus */
 	dtm->dev_index = dev_index;
@@ -862,11 +866,27 @@ bool riscv_config_trigger(riscv_hart_s *const hart, const uint32_t trigger, cons
 	return result;
 }
 
-static bool riscv_attach(target_s *const target)
+static void riscv_jtag_attach(target_s *const target)
 {
 	riscv_hart_s *const hart = riscv_hart_struct(target);
-	/* We put the TAP into bypass at the end of the JTAG handler, so put it back into DMI */
+	/* We put the TAP into bypass at the end of the JTAG handler and on detach, so put it back into DMI */
 	jtag_dev_write_ir(hart->dbg_module->dtm->dev_index, IR_DMI);
+}
+
+static void riscv_jtag_detach(target_s *const target)
+{
+	riscv_hart_s *const hart = riscv_hart_struct(target);
+	/* On detaching, stick the TAP back into bypass */
+	jtag_dev_write_ir(hart->dbg_module->dtm->dev_index, IR_BYPASS);
+}
+
+static bool riscv_attach(target_s *const target)
+{
+	riscv_hart_s *const hart = riscv_hart_struct(target);
+	/* Call special behaviour for this DTM on attach if it's defined */
+	if (hart->dbg_module->dtm->dtm_attach)
+		hart->dbg_module->dtm->dtm_attach(target);
+
 	/* We then also need to select the Hart again so we're poking with the right one on the target */
 	if (!riscv_dm_write(hart->dbg_module, RV_DM_CONTROL, hart->hartsel))
 		return false;
@@ -877,11 +897,13 @@ static bool riscv_attach(target_s *const target)
 
 static void riscv_detach(target_s *const target)
 {
-	riscv_hart_s *const hart = riscv_hart_struct(target);
 	/* Once we get done and the user's asked us to detach, we need to resume the hart */
 	riscv_halt_resume(target, false);
-	/* On detaching, stick the TAP back into bypass */
-	jtag_dev_write_ir(hart->dbg_module->dtm->dev_index, IR_BYPASS);
+
+	riscv_hart_s *const hart = riscv_hart_struct(target);
+	/* Call special behaviour for this DTM on attach if it's defined */
+	if (hart->dbg_module->dtm->dtm_detach)
+		hart->dbg_module->dtm->dtm_detach(target);
 }
 
 static bool riscv_check_error(target_s *const target)
diff --git a/src/target/riscv_debug.h b/src/target/riscv_debug.h
index 21a22fb5622..db48d7086ea 100644
--- a/src/target/riscv_debug.h
+++ b/src/target/riscv_debug.h
@@ -95,6 +95,8 @@ struct riscv_dtm {
 
 	bool (*dmi_read)(riscv_dtm_s *dtm, uint32_t address, uint32_t *value);
 	bool (*dmi_write)(riscv_dtm_s *dtm, uint32_t address, uint32_t value);
+	void (*dtm_attach)(target_s *target);
+	void (*dtm_detach)(target_s *target);
 
 	uint8_t dev_index;
 	uint8_t idle_cycles;