From 2f7f61f4024d7a063d4a830c675ae19838c36e21 Mon Sep 17 00:00:00 2001
From: wangyanwen <wangyanwen@nucleisys.com>
Date: Mon, 9 Oct 2023 11:46:23 +0800
Subject: [PATCH] target/riscv: add nuclei customized init resethalt command

init resethalt is used to halt the cpu when reset

Change-Id: I5c504599c10da204c0e9f933d33f61fe75ed033c
Signed-off-by: wangyanwen <wangyanwen@nucleisys.com>
---
 src/openocd.c                | 16 +++++++++++++---
 src/target/riscv/riscv-013.c |  7 +++++++
 src/target/target.c          | 15 +++++++++++----
 src/target/target.h          |  1 +
 4 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/src/openocd.c b/src/openocd.c
index 9fd709e32..bb87b5bbf 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -107,10 +107,16 @@ COMMAND_HANDLER(handle_noinit_command)
 /* OpenOCD can't really handle failure of this command. Patches welcome! :-) */
 COMMAND_HANDLER(handle_init_command)
 {
+	bool resethalt = false;
 
-	if (CMD_ARGC != 0)
+	if (CMD_ARGC > 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
+	if (CMD_ARGC == 1) {
+		if (strcmp("resethalt", CMD_ARGV[0]) == 0)
+			resethalt = true;
+	}
+
 	int retval;
 	static int initialized;
 	if (initialized)
@@ -120,7 +126,11 @@ COMMAND_HANDLER(handle_init_command)
 
 	bool save_poll_mask = jtag_poll_mask();
 
-	retval = command_run_line(CMD_CTX, "target init");
+	if (resethalt)
+		retval = command_run_line(CMD_CTX, "target init resethalt");
+	else
+		retval = command_run_line(CMD_CTX, "target init");
+
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
 
@@ -213,7 +223,7 @@ static const struct command_registration openocd_command_handlers[] = {
 			"Changes command mode from CONFIG to EXEC.  "
 			"Unless 'noinit' is called, this command is "
 			"called automatically at the end of startup.",
-		.usage = ""
+		.usage = "[resethalt]"
 	},
 	{
 		.name = "add_script_search_dir",
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index c399de7e4..e49f8dcfe 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1844,6 +1844,13 @@ static int examine_dm(struct target *target)
 
 	dm->current_hartid = HART_INDEX_UNKNOWN;
 
+	if (target->resethalt_during_init) {
+		dmi_write(target, DM_DMCONTROL, DM_DMCONTROL_SETRESETHALTREQ | DM_DMCONTROL_DMACTIVE);
+		dmi_write(target, DM_DMCONTROL, DM_DMCONTROL_NDMRESET | DM_DMCONTROL_DMACTIVE);
+		dmi_write(target, DM_DMCONTROL, DM_DMCONTROL_DMACTIVE);
+		dmi_write(target, DM_DMCONTROL, DM_DMCONTROL_CLRRESETHALTREQ | DM_DMCONTROL_DMACTIVE);
+	}
+
 	result = dm_write(target, DM_DMCONTROL, DM_DMCONTROL_HARTSELLO |
 			DM_DMCONTROL_HARTSELHI | DM_DMCONTROL_DMACTIVE |
 			DM_DMCONTROL_HASEL);
diff --git a/src/target/target.c b/src/target/target.c
index fb038311b..62620bbfd 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1539,7 +1539,7 @@ static int target_init_one(struct command_context *cmd_ctx,
 	return ERROR_OK;
 }
 
-static int target_init(struct command_context *cmd_ctx)
+static int target_init(struct command_context *cmd_ctx, bool resethalt)
 {
 	struct target *target;
 	int retval;
@@ -1548,6 +1548,7 @@ static int target_init(struct command_context *cmd_ctx)
 		retval = target_init_one(cmd_ctx, target);
 		if (retval != ERROR_OK)
 			return retval;
+		target->resethalt_during_init = resethalt;
 	}
 
 	if (!all_targets)
@@ -1568,10 +1569,16 @@ static int target_init(struct command_context *cmd_ctx)
 COMMAND_HANDLER(handle_target_init_command)
 {
 	int retval;
+	bool resethalt = false;
 
-	if (CMD_ARGC != 0)
+	if (CMD_ARGC > 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
+	if (CMD_ARGC == 1) {
+		if (strcmp("resethalt", CMD_ARGV[0]) == 0)
+			resethalt = true;
+	}
+
 	static bool target_initialized;
 	if (target_initialized) {
 		LOG_INFO("'target init' has already been called");
@@ -1592,7 +1599,7 @@ COMMAND_HANDLER(handle_target_init_command)
 		return retval;
 
 	LOG_DEBUG("Initializing targets...");
-	return target_init(CMD_CTX);
+	return target_init(CMD_CTX, resethalt);
 }
 
 int target_register_event_callback(int (*callback)(struct target *target,
@@ -6050,7 +6057,7 @@ static const struct command_registration target_subcommand_handlers[] = {
 		.mode = COMMAND_CONFIG,
 		.handler = handle_target_init_command,
 		.help = "initialize targets",
-		.usage = "",
+		.usage = "[resethalt]",
 	},
 	{
 		.name = "create",
diff --git a/src/target/target.h b/src/target/target.h
index 2f4c1a02f..7f1bc6e4b 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -213,6 +213,7 @@ struct target {
 
 	/* The semihosting information, extracted from the target. */
 	struct semihosting *semihosting;
+	bool resethalt_during_init;
 };
 
 struct target_list {