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

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

diff --git a/src/jtag/drivers/openjtag.c b/src/jtag/drivers/openjtag.c
index dca27b0a6444f756e9f6c69309d0e3c2759d338e..ea78ca8fd666827ba3c94789c87cd3d1c08fccda 100644 (file)
--- a/src/jtag/drivers/openjtag.c
+++ b/src/jtag/drivers/openjtag.c
@@ -530,9 +530,20 @@ static int openjtag_quit(void)
 static void openjtag_write_tap_buffer(void)
 {
        uint32_t written;
+       uint32_t rx_expected = 0;
+
+       /* calculate expected number of return bytes */
+       for (int tx_offs = 0; tx_offs < usb_tx_buf_offs; tx_offs++) {
+               if ((usb_tx_buf[tx_offs] & 0x0F) == 6) {
+                       rx_expected++;
+                       tx_offs++;
+               } else if ((usb_tx_buf[tx_offs] & 0x0F) == 2) {
+                       rx_expected++;
+               }
+       }
 
        openjtag_buf_write(usb_tx_buf, usb_tx_buf_offs, &written);
-       openjtag_buf_read(usb_rx_buf, usb_tx_buf_offs, &usb_rx_buf_len);
+       openjtag_buf_read(usb_rx_buf, rx_expected, &usb_rx_buf_len);
 
        usb_tx_buf_offs = 0;
 }
@@ -660,14 +671,12 @@ static void openjtag_execute_reset(struct jtag_command *cmd)
 
        uint8_t buf = 0x00;
 
-       if (cmd->cmd.reset->trst) {
-               buf = 0x03;
-       } else {
+       /* Pull SRST low for 5 TCLK cycles */
+       if (cmd->cmd.reset->srst) {
                buf |= 0x04;
                buf |= 0x05 << 4;
+               openjtag_add_byte(buf);
        }
-
-       openjtag_add_byte(buf);
 }
 
 static void openjtag_execute_sleep(struct jtag_command *cmd)
@@ -680,8 +689,14 @@ static void openjtag_set_state(uint8_t openocd_state)
        uint8_t state = openjtag_get_tap_state(openocd_state);
 
        uint8_t buf = 0;
-       buf = 0x01;
-       buf |= state << 4;
+
+       if (state != OPENJTAG_TAP_RESET) {
+               buf = 0x01;
+               buf |= state << 4;
+       } else {
+               /* Force software TLR */
+               buf = 0x03;
+       }
 
        openjtag_add_byte(buf);
 }
@@ -790,9 +805,9 @@ static void openjtag_execute_command(struct jtag_command *cmd)
        }
 }
 
-static int openjtag_execute_queue(void)
+static int openjtag_execute_queue(struct jtag_command *cmd_queue)
 {
-       struct jtag_command *cmd = jtag_command_queue;
+       struct jtag_command *cmd = cmd_queue;
 
        while (cmd) {
                openjtag_execute_command(cmd);
@@ -861,6 +876,17 @@ COMMAND_HANDLER(openjtag_handle_variant_command)
        return ERROR_OK;
 }
 
+COMMAND_HANDLER(openjtag_handle_vid_pid_command)
+{
+       if (CMD_ARGC != 2)
+               return ERROR_COMMAND_SYNTAX_ERROR;
+
+       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], openjtag_vid);
+       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], openjtag_pid);
+
+       return ERROR_OK;
+}
+
 static const struct command_registration openjtag_subcommand_handlers[] = {
        {
                .name = "device_desc",
@@ -876,6 +902,13 @@ static const struct command_registration openjtag_subcommand_handlers[] = {
                .help = "set the OpenJTAG variant",
                .usage = "variant-string",
        },
+       {
+               .name = "vid_pid",
+               .handler = openjtag_handle_vid_pid_command,
+               .mode = COMMAND_CONFIG,
+               .help = "USB VID and PID of the adapter",
+               .usage = "vid pid",
+       },
        COMMAND_REGISTRATION_DONE
 };