drivers/cmsis-dap: add multidrop capability

[fw/openocd] / src / jtag / drivers / cmsis_dap.c

diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index 6ab0875982beda7a312537e5ca28b52e1e2d66c2..fb5c86aecccc7706d36dc8f20c0e7f435eba5cec 100644 (file)
--- 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;