From a9f28dafd7a67bf6f0a734f087fc9de283d001f1 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 | 32 ++++++++++++++++++++++++++++++++
 src/target/riscv/riscv.h     |  1 +
 2 files changed, 33 insertions(+)

diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index d256ec4fa..817d65f6c 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -759,6 +759,33 @@ 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;
+	unsigned int count = 1;
+
+	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) {
+			LOG_TARGET_ERROR(target, "Reached the end of DM chain (detected %u DMs in total).", count);
+			break;
+		}
+		/* Safety: Avoid looping forever in case of buggy nextdm values in the hardware. */
+		if (count++ > RISCV_MAX_DMS) {
+			LOG_TARGET_ERROR(target, "Supporting no more than %d DMs on a DMI bus. Aborting", RISCV_MAX_DMS);
+			break;
+		}
+	}
+	return false;
+}
+
 static int dmstatus_read_timeout(struct target *target, uint32_t *dmstatus,
 		bool authenticated, unsigned timeout_sec)
 {
@@ -1792,6 +1819,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 cff0d3688..6afd3c164 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