jtag/drivers/bitbang: use LOG_CUSTOM_LEVEL() macro for SWD

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

diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index bc86eb46af6f6714eed20b052c8757c958dcf3e7..d7367d813763f1e7524ac75e3b938745f68da9a6 100644 (file)
--- a/src/jtag/drivers/cmsis_dap.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -365,6 +365,7 @@ static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
                LOG_ERROR("CMSIS-DAP command mismatch. Sent 0x%" PRIx8
                         " received 0x%" PRIx8, current_cmd, resp[0]);
 
+               dap->backend->cancel_all(dap);
                cmsis_dap_flush_read(dap);
                return ERROR_FAIL;
        }
@@ -758,7 +759,6 @@ static int cmsis_dap_cmd_dap_swo_data(
        return ERROR_OK;
 }
 
-
 static void cmsis_dap_swd_discard_all_pending(struct cmsis_dap *dap)
 {
        for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++)
@@ -769,6 +769,13 @@ static void cmsis_dap_swd_discard_all_pending(struct cmsis_dap *dap)
        dap->pending_fifo_block_count = 0;
 }
 
+static void cmsis_dap_swd_cancel_transfers(struct cmsis_dap *dap)
+{
+       dap->backend->cancel_all(dap);
+       cmsis_dap_flush_read(dap);
+       cmsis_dap_swd_discard_all_pending(dap);
+}
+
 static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 {
        uint8_t *command = dap->command;
@@ -854,9 +861,10 @@ static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
                goto skip;
        }
 
-       dap->pending_fifo_put_idx = (dap->pending_fifo_put_idx + 1) % dap->packet_count;
+       unsigned int packet_count = dap->quirk_mode ? 1 : dap->packet_count;
+       dap->pending_fifo_put_idx = (dap->pending_fifo_put_idx + 1) % packet_count;
        dap->pending_fifo_block_count++;
-       if (dap->pending_fifo_block_count > dap->packet_count)
+       if (dap->pending_fifo_block_count > packet_count)
                LOG_ERROR("internal: too much pending writes %u", dap->pending_fifo_block_count);
 
        return;
@@ -913,8 +921,9 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, enum cmsis_dap_blo
        if (resp[0] != block->command) {
                LOG_ERROR("CMSIS-DAP command mismatch. Expected 0x%x received 0x%" PRIx8,
                        block->command, resp[0]);
+               cmsis_dap_swd_cancel_transfers(dap);
                queued_retval = ERROR_FAIL;
-               goto skip;
+               return;
        }
 
        unsigned int transfer_count;
@@ -935,14 +944,18 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, enum cmsis_dap_blo
        if (ack != SWD_ACK_OK) {
                LOG_DEBUG("SWD ack not OK @ %d %s", transfer_count,
                          ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
-               queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+               queued_retval = swd_ack_to_error_code(ack);
                /* TODO: use results of transfers completed before the error occurred? */
                goto skip;
        }
 
-       if (block->transfer_count != transfer_count)
+       if (block->transfer_count != transfer_count) {
                LOG_ERROR("CMSIS-DAP transfer count mismatch: expected %d, got %d",
                          block->transfer_count, transfer_count);
+               cmsis_dap_swd_cancel_transfers(dap);
+               queued_retval = ERROR_FAIL;
+               return;
+       }
 
        LOG_DEBUG_IO("Received results of %d queued transactions FIFO index %u, %s mode",
                                 transfer_count, dap->pending_fifo_get_idx,
@@ -972,7 +985,8 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, enum cmsis_dap_blo
 
 skip:
        block->transfer_count = 0;
-       dap->pending_fifo_get_idx = (dap->pending_fifo_get_idx + 1) % dap->packet_count;
+       if (!dap->quirk_mode && dap->packet_count > 1)
+               dap->pending_fifo_get_idx = (dap->pending_fifo_get_idx + 1) % dap->packet_count;
        dap->pending_fifo_block_count--;
 }
 
@@ -1067,7 +1081,8 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
                /* Not enough room in the queue. Run the queue. */
                cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
 
-               if (cmsis_dap_handle->pending_fifo_block_count >= cmsis_dap_handle->packet_count)
+               unsigned int packet_count = cmsis_dap_handle->quirk_mode ? 1 : cmsis_dap_handle->packet_count;
+               if (cmsis_dap_handle->pending_fifo_block_count >= packet_count)
                        cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_BLOCKING);
        }
 
@@ -1210,7 +1225,7 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
        if (swd_mode)
                queued_retval = cmsis_dap_swd_run_queue();
 
-       if (seq != LINE_RESET &&
+       if (cmsis_dap_handle->quirk_mode && seq != LINE_RESET &&
                        (output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST))
                                == (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
                /* Following workaround deasserts reset on most adapters.
@@ -1939,9 +1954,9 @@ static void cmsis_dap_execute_command(struct jtag_command *cmd)
        }
 }
 
-static int cmsis_dap_execute_queue(void)
+static int cmsis_dap_execute_queue(struct jtag_command *cmd_queue)
 {
-       struct jtag_command *cmd = jtag_command_queue;
+       struct jtag_command *cmd = cmd_queue;
 
        while (cmd) {
                cmsis_dap_execute_command(cmd);
@@ -2208,6 +2223,19 @@ COMMAND_HANDLER(cmsis_dap_handle_backend_command)
        return ERROR_OK;
 }
 
+COMMAND_HANDLER(cmsis_dap_handle_quirk_command)
+{
+       if (CMD_ARGC > 1)
+               return ERROR_COMMAND_SYNTAX_ERROR;
+
+       if (CMD_ARGC == 1)
+               COMMAND_PARSE_ENABLE(CMD_ARGV[0], cmsis_dap_handle->quirk_mode);
+
+       command_print(CMD, "CMSIS-DAP quirk workarounds %s",
+                                 cmsis_dap_handle->quirk_mode ? "enabled" : "disabled");
+       return ERROR_OK;
+}
+
 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
        {
                .name = "info",
@@ -2237,6 +2265,13 @@ static const struct command_registration cmsis_dap_subcommand_handlers[] = {
                .help = "set the communication backend to use (USB bulk or HID).",
                .usage = "(auto | usb_bulk | hid)",
        },
+       {
+               .name = "quirk",
+               .handler = &cmsis_dap_handle_quirk_command,
+               .mode = COMMAND_ANY,
+               .help = "allow expensive workarounds of known adapter quirks.",
+               .usage = "[enable | disable]",
+       },
 #if BUILD_CMSIS_DAP_USB
        {
                .name = "usb",