From 9a9afb0b54324ef28b6ca0a1c4ee5e66b51805ec Mon Sep 17 00:00:00 2001
From: Johann Fischer <johann.fischer@nordicsemi.no>
Date: Thu, 31 Aug 2023 18:01:51 +0200
Subject: [PATCH] dap: implement DAP SWD sequence command

DAP SWD sequence command is a requirement to support CMSIS-DAPv2.
Raise supported version to "2.1.0".

Signed-off-by: Maximilian Deubel <maximilian.deubel@nordicsemi.no>
Signed-off-by: Johann Fischer <johann.fischer@nordicsemi.no>
---
 subsys/dap/cmsis_dap.c | 49 ++++++++++++++++++++++++++++++++++++++++++
 subsys/dap/cmsis_dap.h |  3 ++-
 2 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/subsys/dap/cmsis_dap.c b/subsys/dap/cmsis_dap.c
index 76127c801d4..5bf3f01976f 100644
--- a/subsys/dap/cmsis_dap.c
+++ b/subsys/dap/cmsis_dap.c
@@ -638,6 +638,52 @@ static uint16_t dap_transfer(struct dap_context *const ctx,
 	return retval;
 }
 
+static uint16_t dap_swdp_sequence(struct dap_context *const ctx,
+				  const uint8_t *const request,
+				  uint8_t *const response)
+{
+	const struct swdp_api *api = ctx->swdp_dev->api;
+	const uint8_t *request_data = request + 1;
+	uint8_t *response_data = response + 1;
+	uint8_t count = request[0];
+	uint8_t num_cycles;
+	uint32_t num_bytes;
+	bool input;
+
+	switch (ctx->debug_port) {
+	case DAP_PORT_SWD:
+		response[0] = DAP_OK;
+		break;
+	case DAP_PORT_JTAG:
+	default:
+		LOG_ERR("port unsupported");
+		response[0] = DAP_ERROR;
+		return 1U;
+	}
+
+	for (size_t i = 0; i < count; ++i) {
+		input = *request_data & BIT(7);
+		num_cycles = *request_data & BIT_MASK(7);
+		num_bytes = (num_cycles + 7) >> 3; /* rounded up to full bytes */
+
+		if (num_cycles == 0) {
+			num_cycles = 64;
+		}
+
+		request_data += 1;
+
+		if (input) {
+			api->swdp_input_sequence(ctx->swdp_dev, num_cycles, response_data);
+			response_data += num_bytes;
+		} else {
+			api->swdp_output_sequence(ctx->swdp_dev, num_cycles, request_data);
+			request_data += num_bytes;
+		}
+	}
+
+	return response_data - response;
+}
+
 /*
  * Process SWD DAP_TransferBlock command and prepare response.
  * pyOCD counterpart is _encode_transfer_block_data.
@@ -865,6 +911,9 @@ static uint16_t dap_process_cmd(struct dap_context *const ctx,
 	case ID_DAP_SWDP_CONFIGURE:
 		retval = dap_swdp_configure(ctx, request, response);
 		break;
+	case ID_DAP_SWDP_SEQUENCE:
+		retval = dap_swdp_sequence(ctx, request, response);
+		break;
 	case ID_DAP_JTAG_SEQUENCE:
 		LOG_ERR("JTAG sequence unsupported");
 		retval = 1;
diff --git a/subsys/dap/cmsis_dap.h b/subsys/dap/cmsis_dap.h
index 787dc822b79..d8869e2160a 100644
--- a/subsys/dap/cmsis_dap.h
+++ b/subsys/dap/cmsis_dap.h
@@ -22,7 +22,7 @@
 #include <zephyr/kernel.h>
 
 /* Firmware Version */
-#define DAP_FW_VER				"1.10"
+#define DAP_FW_VER				"2.1.0"
 
 /* DAP Command IDs */
 #define ID_DAP_INFO				0x00U
@@ -42,6 +42,7 @@
 #define ID_DAP_SWJ_SEQUENCE			0x12U
 
 #define ID_DAP_SWDP_CONFIGURE			0x13U
+#define ID_DAP_SWDP_SEQUENCE			0x1DU
 
 #define ID_DAP_JTAG_SEQUENCE			0x14U
 #define ID_DAP_JTAG_CONFIGURE			0x15U