From 6e970e9c41f0d593244b22d007362d5f805daab6 Mon Sep 17 00:00:00 2001
From: Mark Zhuang <mark.zhuang@spacemit.com>
Date: Wed, 28 Jun 2023 16:18:37 +0800
Subject: [PATCH] target/riscv: support check dbgbase exist

Change-Id: I0d65bf9b33fb6d10c33f4f038045832594579e58
Signed-off-by: Mark Zhuang <mark.zhuang@spacemit.com>
---
 src/target/riscv/riscv-013.c | 28 ++++++++++++++++++++++++++++
 src/target/riscv/riscv.h     |  1 +
 2 files changed, 29 insertions(+)

diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 58f674dde..d0528d5fa 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -759,6 +759,29 @@ static int dm_write_exec(struct target *target, uint32_t address,
 	return dmi_write_exec(target, address + dm->base, value, ensure_success);
 }
 
+static bool check_dbgbase_exists(struct target *target)
+{
+	uint32_t next_dm = 0;
+	uint32_t count = 0;
+
+	LOG_TARGET_DEBUG(target, "Searching for DM with DMI base address (dbgbase) = 0x%x", target->dbgbase);
+	while (1) {
+		uint32_t current_dm = next_dm;
+		if (current_dm == target->dbgbase)
+			return true;
+		if (dmi_read(target, &next_dm, DM_NEXTDM + current_dm) != ERROR_OK)
+			break;
+		LOG_TARGET_DEBUG(target, "dm @ 0x%x --> nextdm=0x%x", current_dm, next_dm);
+		/* Check if it's last one in the chain. */
+		if (next_dm == 0)
+			break;
+		/* Safety: Avoid looping forever in case of buggy nexdm values in the hardware. */
+		if (count++ >= RISCV_MAX_DMS)
+			break;
+	}
+	return false;
+}
+
 static int dmstatus_read_timeout(struct target *target, uint32_t *dmstatus,
 		bool authenticated, unsigned timeout_sec)
 {
@@ -1792,6 +1815,11 @@ static int examine(struct target *target)
 	info->abits = get_field(dtmcontrol, DTM_DTMCS_ABITS);
 	info->dtmcs_idle = get_field(dtmcontrol, DTM_DTMCS_IDLE);
 
+	if (!check_dbgbase_exists(target)) {
+		LOG_TARGET_ERROR(target, "Could not find debug module with DMI base address (dbgbase) = 0x%x", target->dbgbase);
+		return ERROR_FAIL;
+	}
+
 	/* Reset the Debug Module. */
 	dm013_info_t *dm = get_dm(target);
 	if (!dm)
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index eacbe40d9..e164f0999 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -19,6 +19,7 @@ struct riscv_program;
 #define RISCV_MAX_HARTS  ((int)BIT(20))
 #define RISCV_MAX_TRIGGERS 32
 #define RISCV_MAX_HWBPS 16
+#define RISCV_MAX_DMS 100
 
 #define DEFAULT_COMMAND_TIMEOUT_SEC		2
 #define DEFAULT_RESET_TIMEOUT_SEC		30