drivers/cmsis-dap: add multidrop capability

Uses CMSIS-DAP v 1.2 or higher protocol command DAP_SWD_Sequence
to write to DP TARGETSEL register. This write is not acknowledged
by device so we cannot use standard DAP_Transfer

Change-Id: Ib252d09570bcc2282be5f854e0ab9a0dfda06189
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/6117
Tested-by: jenkins
Reviewed-by: Peter Lawrence <majbthrd@gmail.com>

src/jtag/drivers/cmsis_dap.c

@@ -136,6 +136,7 @@ static bool swd_mode;
 
 /* CMSIS-DAP SWD Commands */
 #define CMD_DAP_SWD_CONFIGURE     0x13
+#define CMD_DAP_SWD_SEQUENCE      0x1D
 
 /* CMSIS-DAP JTAG Commands */
 #define CMD_DAP_JTAG_SEQ          0x14
@@ -542,6 +543,45 @@ static int cmsis_dap_cmd_DAP_Delay(uint16_t delay_us)
 }
 #endif
 
+static int cmsis_dap_metacmd_targetsel(uint32_t instance_id)
+{
+	int retval;
+	uint8_t *command = cmsis_dap_handle->command;
+	const uint32_t SEQ_RD = 0x80, SEQ_WR = 0x00;
+
+	/* SWD multi-drop requires a transfer ala CMD_DAP_TFER,
+	but with no expectation of an SWD ACK response.  In
+	CMSIS-DAP v1.20 and v2.00, CMD_DAP_SWD_SEQUENCE was
+	added to allow this special sequence to be generated.
+	The purpose of this operation is to select the target
+	corresponding to the instance_id that is written */
+
+	size_t idx = 0;
+	command[idx++] = CMD_DAP_SWD_SEQUENCE;
+	command[idx++] = 3;	/* sequence count */
+
+	/* sequence 0: packet request for TARGETSEL */
+	command[idx++] = SEQ_WR | 8;
+	command[idx++] = SWD_CMD_START | swd_cmd(false, false, DP_TARGETSEL) | SWD_CMD_STOP | SWD_CMD_PARK;
+
+	/* sequence 1: read Trn ACK Trn, no expectation for target to ACK  */
+	command[idx++] = SEQ_RD | 5;
+
+	/* sequence 2: WDATA plus parity */
+	command[idx++] = SEQ_WR | (32 + 1);
+	h_u32_to_le(command + idx, instance_id);
+	idx += 4;
+	command[idx++] = parity_u32(instance_id);
+
+	retval = cmsis_dap_xfer(cmsis_dap_handle, idx);
+
+	if (retval != ERROR_OK || cmsis_dap_handle->response[1] != DAP_OK) {
+		LOG_ERROR("CMSIS-DAP command SWD_Sequence failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+	return ERROR_OK;
+}
+
 static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 {
 	uint8_t *command = cmsis_dap_handle->command;
@@ -715,7 +755,10 @@ static int cmsis_dap_swd_run_queue(void)
 
 static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 {
-	if (pending_fifo[pending_fifo_put_idx].transfer_count == pending_queue_len) {
+	bool targetsel_cmd = swd_cmd(false, false, DP_TARGETSEL) == cmd;
+
+	if (pending_fifo[pending_fifo_put_idx].transfer_count == pending_queue_len
+			 || targetsel_cmd) {
 		if (pending_fifo_block_count)
 			cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
 
@@ -729,6 +772,11 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 	if (queued_retval != ERROR_OK)
 		return;
 
+	if (targetsel_cmd) {
+		cmsis_dap_metacmd_targetsel(data);
+		return;
+	}
+
 	struct pending_request_block *block = &pending_fifo[pending_fifo_put_idx];
 	struct pending_transfer_result *transfer = &(block->transfers[block->transfer_count]);
 	transfer->data = data;
@@ -846,7 +894,7 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 
 	switch (seq) {
 	case LINE_RESET:
-		LOG_DEBUG("SWD line reset");
+		LOG_DEBUG_IO("SWD line reset");
 		s = swd_seq_line_reset;
 		s_len = swd_seq_line_reset_len;
 		break;
@@ -855,11 +903,26 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 		s = swd_seq_jtag_to_swd;
 		s_len = swd_seq_jtag_to_swd_len;
 		break;
+	case JTAG_TO_DORMANT:
+		LOG_DEBUG("JTAG-to-DORMANT");
+		s = swd_seq_jtag_to_dormant;
+		s_len = swd_seq_jtag_to_dormant_len;
+		break;
 	case SWD_TO_JTAG:
 		LOG_DEBUG("SWD-to-JTAG");
 		s = swd_seq_swd_to_jtag;
 		s_len = swd_seq_swd_to_jtag_len;
 		break;
+	case SWD_TO_DORMANT:
+		LOG_DEBUG("SWD-to-DORMANT");
+		s = swd_seq_swd_to_dormant;
+		s_len = swd_seq_swd_to_dormant_len;
+		break;
+	case DORMANT_TO_SWD:
+		LOG_DEBUG("DORMANT-to-SWD");
+		s = swd_seq_dormant_to_swd;
+		s_len = swd_seq_dormant_to_swd_len;
+		break;
 	default:
 		LOG_ERROR("Sequence %d not supported", seq);
 		return ERROR_FAIL;