From 3a1152c29056241ed98bef82814f8c1eaed7fb42 Mon Sep 17 00:00:00 2001
From: Huaqi Fang <578567190@qq.com>
Date: Wed, 4 Sep 2024 11:55:06 +0800
Subject: [PATCH] src/target/riscv: add mbatch command for live watch usage

It can be used for reading/writing multiple non-contiguous
chunks of memory with minimum latency using sba method

based on https://github.com/sysprogs/openocd/commit/a0a1aed80f22576e393b8cbab3feddf79a1d0191

Change-Id: If52bf14875f36229e315970643d11983ae2e539d
Signed-off-by: Huaqi Fang <578567190@qq.com>
---
 src/target/target.c | 94 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 94 insertions(+)

diff --git a/src/target/target.c b/src/target/target.c
index 62620bbfd..ea7f12d1a 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -3573,6 +3573,93 @@ COMMAND_HANDLER(handle_mw_command)
 	return target_fill_mem(target, address, fn, wordsize, value, count);
 }
 
+#define ERROR_BAD_ARGUMENTS             (8)
+#define ERROR_INSUFFICIENT_BUFFER       (9)
+
+COMMAND_HANDLER(handle_mbatch_command)
+{
+	if (CMD_ARGC < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/*General syntax:
+		<Unique ID> <Subcommands>
+
+	Sub-command syntax:
+		w:<address>:<access size>:<count>:<hex-encoded data> - write data
+		r:<address>:<access size>:<count> - read data
+
+	Reply syntax for each subcommand:
+		<status>:<optional data>
+	*/
+
+	struct target *target = get_current_target(CMD_CTX);
+	command_print_sameline(cmd, "mbatch(%s):", CMD_ARGV[0]);
+
+	for (int i = 1; i < CMD_ARGC; i++) {
+		const char *pCmd = CMD_ARGV[i];
+		const char *p = strchr(pCmd, ':');
+
+		int status = ERROR_OK;
+		uint8_t *data = NULL;
+		int printData = 0;
+		uint64_t args[3] = { 0, };	/* address, size, count */
+
+		if (!p)
+			status = -ERROR_BAD_ARGUMENTS;
+
+		for (int j = 0; j < sizeof(args) / sizeof(args[0]); j++) {
+			if (p) {
+				p++;
+				char *end = NULL;
+				args[j] = strtoull(p, &end, 0);
+				if (end == p || !end)
+					status = -ERROR_BAD_ARGUMENTS;
+				else if (*end != ':' && *end != 0)
+					status = -ERROR_BAD_ARGUMENTS;
+
+				p = end;
+			}
+		}
+
+		int bufferSize = (int)(args[1] * args[2]);
+
+		if (status == ERROR_OK) {
+			data = (uint8_t *)malloc(bufferSize);
+
+			if (pCmd[0] == 'w') {
+				if (!p)
+					status = -ERROR_BAD_ARGUMENTS;
+				else {
+					int done = unhexify(data, p + 1, args[1] * args[2]);
+					if (done != (args[1] * args[2]))
+						status = -ERROR_INSUFFICIENT_BUFFER;
+					else
+						status = target_write_memory(target, args[0], args[1], args[2], data);
+				}
+			} else if (pCmd[0] == 'r') {
+				status = target_read_memory(target, args[0], args[1], args[2], data);
+				printData = true;
+			} else
+				status = -ERROR_BAD_ARGUMENTS;
+		}
+
+		command_print_sameline(cmd, " %d", status);
+		if (status >= 0 && printData && data) {
+			char *formattedData = malloc(bufferSize * 2 + 2);
+			hexify(formattedData, data, bufferSize, bufferSize * 2 + 2);
+			command_print_sameline(cmd, ":%s", formattedData);
+			free(formattedData);
+		}
+
+		if (data)
+			free(data);
+	}
+
+	command_print(cmd, "");
+	return ERROR_OK;
+}
+
+
 static COMMAND_HELPER(parse_load_image_command, struct image *image,
 		target_addr_t *min_address, target_addr_t *max_address)
 {
@@ -6648,6 +6735,13 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.help = "write memory byte",
 		.usage = "['phys'] address value [count]",
 	},
+	{
+		.name = "mbatch",
+		.handler = handle_mbatch_command,
+		.mode = COMMAND_EXEC,
+		.help = "execute a batch of memory commands",
+		.usage = "[unique ID] [list of memory read/write requests]",
+	},
 	{
 		.name = "bp",
 		.handler = handle_bp_command,