+static int vsllink_swd_init(void)
+{
+ LOG_INFO("VSLLink SWD mode enabled");
+ swd_mode = true;
+
+ return ERROR_OK;
+}
+
+static int_least32_t vsllink_swd_frequency(struct adiv5_dap *dap,
+ int_least32_t hz)
+{
+ const int_least32_t delay2hz[] = {
+ 1850000, 235000, 130000, 102000, 85000, 72000
+ };
+
+ if (hz > 0) {
+ uint16_t delay = UINT16_MAX;
+
+ for (uint16_t i = 0; i < ARRAY_SIZE(delay2hz); i++) {
+ if (hz >= delay2hz[i]) {
+ hz = delay2hz[i];
+ delay = i;
+ break;
+ }
+ }
+
+ if (delay == UINT16_MAX)
+ delay = (500000 / hz) - 1;
+
+ /* Calculate retry count after a WAIT response. This will give
+ * a retry timeout at about ~250 ms. 54 is the number of bits
+ * found in a transaction. */
+ uint16_t retry_count = 250 * hz / 1000 / 54;
+
+ LOG_DEBUG("SWD delay: %d, retry count: %d", delay, retry_count);
+
+ versaloon_interface.adaptors.swd.config(0, 2, retry_count, delay);
+ queued_retval = versaloon_interface.adaptors.peripheral_commit();
+ }
+
+ return hz;
+}
+
+static int vsllink_swd_switch_seq(struct adiv5_dap *dap,
+ enum swd_special_seq seq)
+{
+ switch (seq) {
+ case LINE_RESET:
+ LOG_DEBUG("SWD line reset");
+ versaloon_interface.adaptors.swd.seqout(0, swd_seq_line_reset,
+ swd_seq_line_reset_len);
+ break;
+ case JTAG_TO_SWD:
+ LOG_DEBUG("JTAG-to-SWD");
+ versaloon_interface.adaptors.swd.seqout(0, swd_seq_jtag_to_swd,
+ swd_seq_jtag_to_swd_len);
+ break;
+ case SWD_TO_JTAG:
+ LOG_DEBUG("SWD-to-JTAG");
+ versaloon_interface.adaptors.swd.seqout(0, swd_seq_swd_to_jtag,
+ swd_seq_swd_to_jtag_len);
+ break;
+ default:
+ LOG_ERROR("Sequence %d not supported", seq);
+ return ERROR_FAIL;
+ }
+
+ return versaloon_interface.adaptors.peripheral_commit();
+}
+
+static void vsllink_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd,
+ uint32_t *value)
+{
+ if (queued_retval != ERROR_OK)
+ return;
+
+ int retval;
+ uint32_t val = 0;
+ uint8_t ack;
+
+ versaloon_interface.adaptors.swd.transact(0, cmd, &val, &ack);
+ retval = versaloon_interface.adaptors.peripheral_commit();
+
+ if (retval != ERROR_OK) {
+ queued_retval = ERROR_FAIL;
+ return;
+ }
+
+ if (ack != 0x01) {
+ queued_retval = ack;
+ return;
+ }
+
+ if (value)
+ *value = val;
+
+ queued_retval = retval;
+}
+
+static void vsllink_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd,
+ uint32_t value)
+{
+ if (queued_retval != ERROR_OK)
+ return;
+
+ int retval;
+ uint8_t ack;
+
+ versaloon_interface.adaptors.swd.transact(0, cmd, &value, &ack);
+ retval = versaloon_interface.adaptors.peripheral_commit();
+
+ if (retval != ERROR_OK) {
+ queued_retval = ERROR_FAIL;
+ return;
+ }
+
+ if (ack != 0x01) {
+ queued_retval = ack;
+ return;
+ }
+
+ queued_retval = retval;
+}
+
+static int vsllink_swd_run_queue(struct adiv5_dap *dap)
+{
+ int retval = queued_retval;
+ queued_retval = ERROR_OK;
+ return retval;
+}
+