Add FTDI SWD driver
[openocd.git] / src / jtag / drivers / ftdi.c
index f427a6b..be02d20 100644 (file)
@@ -72,6 +72,7 @@
 
 /* project specific includes */
 #include <jtag/interface.h>
+#include <jtag/swd.h>
 #include <transport/transport.h>
 #include <helper/time_support.h>
 
 #include "mpsse.h"
 
 #define JTAG_MODE (LSB_FIRST | POS_EDGE_IN | NEG_EDGE_OUT)
+#define SWD_MODE (LSB_FIRST | POS_EDGE_IN | NEG_EDGE_OUT)
 
 static char *ftdi_device_desc;
 static char *ftdi_serial;
 static uint8_t ftdi_channel;
 
+static bool swd_mode;
+
 #define MAX_USB_IDS 8
 /* vid = pid = 0 marks the end of the list */
 static uint16_t ftdi_vid[MAX_USB_IDS + 1] = { 0 };
@@ -108,8 +112,25 @@ struct signal {
 
 static struct signal *signals;
 
+/* FIXME: Where to store per-instance data? We need an SWD context. */
+static struct swd_cmd_queue_entry {
+       uint8_t cmd;
+       uint32_t *dst;
+       uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)];
+} *swd_cmd_queue;
+static size_t swd_cmd_queue_length;
+static size_t swd_cmd_queue_alloced;
+static int queued_retval;
+static int freq;
+
 static uint16_t output;
 static uint16_t direction;
+static uint16_t jtag_output_init;
+static uint16_t jtag_direction_init;
+static uint16_t swd_output_init;
+static uint16_t swd_direction_init;
+
+static int ftdi_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq);
 
 static struct signal *find_signal_by_name(const char *name)
 {
@@ -613,11 +634,22 @@ static int ftdi_initialize(void)
        if (!mpsse_ctx)
                return ERROR_JTAG_INIT_FAILED;
 
+       output = swd_mode ? swd_output_init : jtag_output_init;
+       direction = swd_mode ? swd_direction_init : jtag_direction_init;
+
        mpsse_set_data_bits_low_byte(mpsse_ctx, output & 0xff, direction & 0xff);
        mpsse_set_data_bits_high_byte(mpsse_ctx, output >> 8, direction >> 8);
 
        mpsse_loopback_config(mpsse_ctx, false);
 
+       /* Set a low default */
+       freq = mpsse_set_frequency(mpsse_ctx, 1000);
+
+       if (swd_mode)
+               ftdi_swd_switch_seq(NULL, JTAG_TO_SWD);
+       else
+               ftdi_swd_switch_seq(NULL, SWD_TO_JTAG);
+
        return mpsse_flush(mpsse_ctx);
 }
 
@@ -669,8 +701,19 @@ COMMAND_HANDLER(ftdi_handle_layout_init_command)
        if (CMD_ARGC != 2)
                return ERROR_COMMAND_SYNTAX_ERROR;
 
-       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], output);
-       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], direction);
+       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], jtag_output_init);
+       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], jtag_direction_init);
+
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(ftdi_handle_layout_init_swd_command)
+{
+       if (CMD_ARGC != 2)
+               return ERROR_COMMAND_SYNTAX_ERROR;
+
+       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], swd_output_init);
+       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], swd_direction_init);
 
        return ERROR_OK;
 }
@@ -807,7 +850,17 @@ static const struct command_registration ftdi_command_handlers[] = {
                .handler = &ftdi_handle_layout_init_command,
                .mode = COMMAND_CONFIG,
                .help = "initialize the FTDI GPIO signals used "
-                       "to control output-enables and reset signals",
+                       "to control output-enables and reset signals"
+                       "when JTAG mode is selected",
+               .usage = "data direction",
+       },
+       {
+               .name = "ftdi_layout_init_swd",
+               .handler = &ftdi_handle_layout_init_swd_command,
+               .mode = COMMAND_CONFIG,
+               .help = "initialize the FTDI GPIO signals used "
+                       "to control output-enables and reset signals"
+                       "when SWD mode is selected",
                .usage = "data direction",
        },
        {
@@ -835,11 +888,209 @@ static const struct command_registration ftdi_command_handlers[] = {
        COMMAND_REGISTRATION_DONE
 };
 
+static int ftdi_swd_init(void)
+{
+       LOG_INFO("FTDI SWD mode enabled");
+       swd_mode = true;
+
+       swd_cmd_queue_alloced = 10;
+       swd_cmd_queue = malloc(swd_cmd_queue_alloced * sizeof(*swd_cmd_queue));
+
+       return swd_cmd_queue != NULL ? ERROR_OK : ERROR_FAIL;
+}
+
+static void ftdi_swd_swdio_en(bool enable)
+{
+       struct signal *oe = find_signal_by_name("SWDIO_OE");
+       if (oe)
+               ftdi_set_signal(oe, enable ? '1' : '0');
+}
+
+/**
+ * Flush the MPSSE queue and process the SWD transaction queue
+ * @param dap
+ * @return
+ */
+static int ftdi_swd_run_queue(struct adiv5_dap *dap)
+{
+       LOG_DEBUG("Executing %zu queued transactions", swd_cmd_queue_length);
+       int retval;
+       struct signal *led = find_signal_by_name("LED");
+
+       if (queued_retval != ERROR_OK) {
+               LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
+               goto skip;
+       }
+
+       /* A transaction must be followed by another transaction or at least 8 idle cycles to
+        * ensure that data is clocked through the AP. */
+       mpsse_clock_data_out(mpsse_ctx, NULL, 0, 8, SWD_MODE);
+
+       /* Terminate the "blink", if the current layout has that feature */
+       if (led)
+               ftdi_set_signal(led, '0');
+
+       queued_retval = mpsse_flush(mpsse_ctx);
+       if (queued_retval != ERROR_OK) {
+               LOG_ERROR("MPSSE failed");
+               goto skip;
+       }
+
+       for (size_t i = 0; i < swd_cmd_queue_length; i++) {
+               int ack = buf_get_u32(&swd_cmd_queue[i].trn_ack_data_parity_trn, 1, 3);
+
+               LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
+                               ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
+                               swd_cmd_queue[i].cmd & SWD_CMD_APnDP ? "AP" : "DP",
+                               swd_cmd_queue[i].cmd & SWD_CMD_RnW ? "read" : "write",
+                               (swd_cmd_queue[i].cmd & SWD_CMD_A32) >> 1,
+                               buf_get_u32(swd_cmd_queue[i].trn_ack_data_parity_trn,
+                                               1 + 3 + (swd_cmd_queue[i].cmd & SWD_CMD_RnW ? 0 : 1), 32));
+
+               if (ack != SWD_ACK_OK) {
+                       queued_retval = ack;
+                       goto skip;
+
+               } else if (swd_cmd_queue[i].cmd & SWD_CMD_RnW) {
+                       uint32_t data = buf_get_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3, 32);
+                       int parity = buf_get_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3 + 32, 1);
+
+                       if (parity != parity_u32(data)) {
+                               LOG_ERROR("SWD Read data parity mismatch");
+                               queued_retval = ERROR_FAIL;
+                               goto skip;
+                       }
+
+                       if (swd_cmd_queue[i].dst != NULL)
+                               *swd_cmd_queue[i].dst = data;
+               }
+       }
+
+skip:
+       swd_cmd_queue_length = 0;
+       retval = queued_retval;
+       queued_retval = ERROR_OK;
+
+       /* Queue a new "blink" */
+       if (led && retval == ERROR_OK)
+               ftdi_set_signal(led, '1');
+
+       return retval;
+}
+
+static void ftdi_swd_queue_cmd(struct adiv5_dap *dap, uint8_t cmd, uint32_t *dst, uint32_t data)
+{
+       if (swd_cmd_queue_length >= swd_cmd_queue_alloced) {
+               /* Not enough room in the queue. Run the queue and increase its size for next time.
+                * Note that it's not possible to avoid running the queue here, because mpsse contains
+                * pointers into the queue which may be invalid after the realloc. */
+               queued_retval = ftdi_swd_run_queue(dap);
+               struct swd_cmd_queue_entry *q = realloc(swd_cmd_queue, swd_cmd_queue_alloced * 2 * sizeof(*swd_cmd_queue));
+               if (q != NULL) {
+                       swd_cmd_queue = q;
+                       swd_cmd_queue_alloced *= 2;
+                       LOG_DEBUG("Increased SWD command queue to %zu elements", swd_cmd_queue_alloced);
+               }
+       }
+
+       if (queued_retval != ERROR_OK)
+               return;
+
+       size_t i = swd_cmd_queue_length++;
+       swd_cmd_queue[i].cmd = cmd | SWD_CMD_START | SWD_CMD_PARK;
+
+       mpsse_clock_data_out(mpsse_ctx, &swd_cmd_queue[i].cmd, 0, 8, SWD_MODE);
+
+       if (swd_cmd_queue[i].cmd & SWD_CMD_RnW) {
+               /* Queue a read transaction */
+               swd_cmd_queue[i].dst = dst;
+
+               ftdi_swd_swdio_en(false);
+               mpsse_clock_data_in(mpsse_ctx, swd_cmd_queue[i].trn_ack_data_parity_trn,
+                               0, 1 + 3 + 32 + 1 + 1, SWD_MODE);
+               ftdi_swd_swdio_en(true);
+       } else {
+               /* Queue a write transaction */
+               ftdi_swd_swdio_en(false);
+
+               mpsse_clock_data_in(mpsse_ctx, swd_cmd_queue[i].trn_ack_data_parity_trn,
+                               0, 1 + 3 + 1, SWD_MODE);
+
+               ftdi_swd_swdio_en(true);
+
+               buf_set_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3 + 1, 32, data);
+               buf_set_u32(swd_cmd_queue[i].trn_ack_data_parity_trn, 1 + 3 + 1 + 32, 1, parity_u32(data));
+
+               mpsse_clock_data_out(mpsse_ctx, swd_cmd_queue[i].trn_ack_data_parity_trn,
+                               1 + 3 + 1, 32 + 1, SWD_MODE);
+       }
+
+       /* Insert idle cycles after AP accesses to avoid WAIT */
+       if (cmd & SWD_CMD_APnDP)
+               mpsse_clock_data_out(mpsse_ctx, NULL, 0, dap->memaccess_tck, SWD_MODE);
+
+}
+
+static void ftdi_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t *value)
+{
+       assert(cmd & SWD_CMD_RnW);
+       ftdi_swd_queue_cmd(dap, cmd, value, 0);
+}
+
+static void ftdi_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd, uint32_t value)
+{
+       assert(!(cmd & SWD_CMD_RnW));
+       ftdi_swd_queue_cmd(dap, cmd, NULL, value);
+}
+
+static int_least32_t ftdi_swd_frequency(struct adiv5_dap *dap, int_least32_t hz)
+{
+       if (hz > 0)
+               freq = mpsse_set_frequency(mpsse_ctx, hz);
+
+       return freq;
+}
+
+static int ftdi_swd_switch_seq(struct adiv5_dap *dap, enum swd_special_seq seq)
+{
+       switch (seq) {
+       case LINE_RESET:
+               LOG_DEBUG("SWD line reset");
+               mpsse_clock_data_out(mpsse_ctx, swd_seq_line_reset, 0, swd_seq_line_reset_len, SWD_MODE);
+               break;
+       case JTAG_TO_SWD:
+               LOG_DEBUG("JTAG-to-SWD");
+               mpsse_clock_data_out(mpsse_ctx, swd_seq_jtag_to_swd, 0, swd_seq_jtag_to_swd_len, SWD_MODE);
+               break;
+       case SWD_TO_JTAG:
+               LOG_DEBUG("SWD-to-JTAG");
+               mpsse_clock_data_out(mpsse_ctx, swd_seq_swd_to_jtag, 0, swd_seq_swd_to_jtag_len, SWD_MODE);
+               break;
+       default:
+               LOG_ERROR("Sequence %d not supported", seq);
+               return ERROR_FAIL;
+       }
+
+       return ERROR_OK;
+}
+
+static const struct swd_driver ftdi_swd = {
+       .init = ftdi_swd_init,
+       .frequency = ftdi_swd_frequency,
+       .switch_seq = ftdi_swd_switch_seq,
+       .read_reg = ftdi_swd_read_reg,
+       .write_reg = ftdi_swd_write_reg,
+       .run = ftdi_swd_run_queue,
+};
+
+static const char * const ftdi_transports[] = { "jtag", "swd", NULL };
+
 struct jtag_interface ftdi_interface = {
        .name = "ftdi",
        .supported = DEBUG_CAP_TMS_SEQ,
        .commands = ftdi_command_handlers,
-       .transports = jtag_only,
+       .transports = ftdi_transports,
+       .swd = &ftdi_swd,
 
        .init = ftdi_initialize,
        .quit = ftdi_quit,