diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c index 6ab087598..fb5c86aec 100644 --- a/src/jtag/drivers/cmsis_dap.c +++ b/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;