/*
 * Simple JTAG interface using ESP32S2 SPI3
 *
 * This implements both JTAG and plain SPI functionality. It uses
 * programmed I/O (CPU control) using notifications from an interrupt
 * handler to wake up the active thread; fractional words and
 * TMS-changing transactions are bit banged. This is not particularly
 * fast (especially since this code does not currently do double
 * buffering), but it allows for arbitrary bit shifts and works around
 * some bugs in ESP32/ESP32-S2 silicon, possibly other versions as well.
 *
 * For SPI functionality, the TMS pin represents CS#.
 *
 * Note: this uses direct register access for some things that are
 * normally abstracted in Arduino and ESP-IDF; this greatly reduces
 * overhead in those cases.
 */

#define MODULE "jtag-esp32"
#define DEBUG 0

#include "common.h"
#include "jtag.h"

#include <esp_intr_alloc.h>
#include <esp32-hal-spi.h>
#include <driver/spi_common.h>
#include <driver/spi_common_internal.h> /* spicommon_*() */
#include <soc/soc.h>
#include <soc/spi_struct.h>
#include <soc/spi_reg.h>
#include <soc/periph_defs.h>
#include <soc/gpio_struct.h>
#include <soc/gpio_sig_map.h>

#define JTAG_SPI_HOST HSPI		/* == SPI3 */

typedef struct spi_struct_t {
    volatile spi_dev_t *dev;
    /* Other fields are HAL-specific */
} spi_t;

/* Return a mask of the <bits> least signficant bits */
static inline uint32_t LMASK(unsigned int bits)
{
    return (UINT32_C(1) << bits) - 1;
}

static inline void set_gpio_mask(uint32_t mask, bool val)
{
    volatile uint32_t *reg = val ? &GPIO.out_w1ts : &GPIO.out_w1tc;
    *reg = mask;
}

static inline void set_gpio(unsigned int pin, bool val)
{
    set_gpio_mask(1U << pin, val);
}

static inline uint32_t get_gpio_mask(uint32_t mask)
{
    return GPIO.in & mask;
}

static inline unsigned int get_gpio(unsigned int pin)
{
    if (__builtin_constant_p(pin)) {
	return (GPIO.in >> pin) & 1; /* More efficient on Xtensa */
    } else {
	return !!get_gpio_mask(1U << pin);
    }
}

static struct jtag_state {
    spi_t *spi;
    intr_handle_t intr_handle;
    TaskHandle_t task;

    struct jtag_config config;
    uint32_t pin_tdi_mask;
    uint32_t pin_tdo_mask;
    uint32_t pin_tms_mask;
    uint32_t pin_tck_mask;

    unsigned int hz;		/* Actual frequency */
    unsigned int clkdiv;
    unsigned int bitbang_loops;	/* Delay loop count in bitbang driver */
    uint64_t us_to_clocks_multiplier;
    unsigned int spi_timeout_ticks;

    unsigned int bits;
    unsigned int tdo_bits_read;
    const uint32_t *tdi;
    uint32_t *tdo;
    uint8_t tdi_bitoffs;
    uint8_t tdo_bitoffs;

    enum TAP_STATE tap_state;
    bool configured;
} jtag;

#define NOTIFY_INDEX 0		/* Seems to be the only available... */

static void ARDUINO_ISR_ATTR jtag_isr(void *);

const unsigned int spi_mosi_src = SPI3_D_OUT_IDX;
const unsigned int spi_sclk_src = SPI3_CLK_OUT_MUX_IDX;
const unsigned int gpio_src     = SIG_GPIO_OUT_IDX;

/*
 * Switch the output lines between SPI (true) or GPIO (false).
 */
static inline void jtag_config_tdi(bool attach)
{
    GPIO.func_out_sel_cfg[jtag.config.pin_tdi].val =
	attach ? spi_mosi_src : gpio_src;
}
static inline void jtag_config_tck(bool attach)
{
    GPIO.func_out_sel_cfg[jtag.config.pin_tck].val
	= attach ? spi_sclk_src : gpio_src;
}

/*
 * If hz != 0 then set to the closest supported frequency and return
 * the true frequency.
 *
 * This precalculates a bunch of parameters dependent on the JTAG
 * frequency.
 */
#define APB_CLK_PER_READ 2	/* Pure guess, but probably conservative */

static unsigned int jtag_set_hz(unsigned int hz)
{
    /*
     * If we waited this long poll to see if we lost an interrupt.
     * spi_irq_timeout is the number of times the *maximum* chunk
     * transmission time.
     */
    const unsigned int spi_irq_timeout = 4;

    if (hz) {
	jtag.clkdiv = spiFrequencyToClockDiv(hz);
	jtag.hz = spiClockDivToFrequency(jtag.clkdiv);
	jtag.bitbang_loops =
	    (APB_CLK_FREQ)/(APB_CLK_PER_READ*2*jtag.hz);
	jtag.us_to_clocks_multiplier = ((uint64_t)jtag.hz << 32)/1000000;
	jtag.spi_timeout_ticks =
	    (spi_irq_timeout * (sizeof jtag.spi->dev->data_buf << 3)
	     * configTICK_RATE_HZ) / (jtag.hz+1) + 1;

	MSG("SPI: JTAG frequency set to %u Hz\n", jtag.hz);
    }
    return jtag.hz;
}

/* Set pins to the default output values and let them stabilize */
static void jtag_idle_pins(void)
{
    set_gpio_mask(jtag.pin_tck_mask, 0);
    set_gpio_mask(jtag.pin_tdi_mask|jtag.pin_tms_mask, 1);

    jtag_config_tck(false);
    jtag_config_tdi(false);

    jtag_delay(1000);		/* 1 ms */
}

int jtag_disable(const struct jtag_config *config)
{
    /* Tristate all JTAG pins and stop SPI engine */
    const char *act = "initialized";

    if (jtag.spi) {
	jtag.spi->dev->slave.val = 0; /* Disable all interrupt sources */

	jtag_idle_pins();
	spiDetachMISO(jtag.spi, jtag.config.pin_tdo);

#if 0
	spiEndTransaction(jtag.spi);
#endif
	spiStopBus(jtag.spi);
	if (jtag.intr_handle)
	    esp_intr_free(jtag.intr_handle);

	jtag.spi = NULL;
	act = "disabled";
    }

    if (config) {
	jtag.config = *config;
	jtag.configured = true;
    }

    /* Tristate all signal pins */
    if (jtag.configured) {
	pinMode(jtag.config.pin_tdi, INPUT);
	pinMode(jtag.config.pin_tdo, INPUT);
	pinMode(jtag.config.pin_tms, INPUT);
	pinMode(jtag.config.pin_tck, INPUT);
    }

    printf("[JTAG] JTAG %s\n", act);
    return 0;
}

#if DEBUG > 2
static void dump_spi_regs(void)
{
    const volatile uint32_t *spiregs =
	(const volatile uint32_t *)jtag.spi->dev;

    for (unsigned int i = 0; i <= 0x100; i += 4) {
	uint32_t v = *spiregs++;
	MSG("SPI: reg 0x%03x = 0x%08x %u\n", i, v, v);
    }
}
#else
#define dump_spi_regs() ((void)0)
#endif

static inline unsigned int jtag_us_to_clocks(unsigned int us)
{
    return (us * jtag.us_to_clocks_multiplier) >> 32;
}

int jtag_enable(const struct jtag_config *config)
{
    if (jtag.spi)
	return -1;

    jtag.task = xTaskGetCurrentTaskHandle();

    if (config) {
	jtag.config = *config;
	jtag.configured = true;
    }

    if (!jtag.configured)
	return -1;

    jtag.pin_tdi_mask = 1U << jtag.config.pin_tdi;
    jtag.pin_tdo_mask = 1U << jtag.config.pin_tdo;
    jtag.pin_tms_mask = 1U << jtag.config.pin_tms;
    jtag.pin_tck_mask = 1U << jtag.config.pin_tck;

    pinMode(jtag.config.pin_tdo, INPUT);
    pinMode(jtag.config.pin_tdi, OUTPUT);
    pinMode(jtag.config.pin_tms, OUTPUT);
    pinMode(jtag.config.pin_tck, OUTPUT);

    jtag_set_hz(jtag.config.hz);

    printf("[JTAG] starting bus, clkdiv = 0x%08x (%u Hz)\n",
	   jtag.clkdiv, jtag.hz);
    jtag.spi = spiStartBus(JTAG_SPI_HOST, jtag.clkdiv, SPI_MODE0, SPI_LSBFIRST);
    if (!jtag.spi) {
	printf("[JTAG] failed to start bus\n");
	jtag_disable(NULL);
	return -1;
    }

    /* spiStartBus resets the bus and sets the frequency */

    spi_dev_t * const hw = jtag.spi->dev;

#if 0
    MSG("SPI: acquiring lock\n");
    spiSimpleTransaction(jtag.spi); /* Lock bus */
#endif

    spiAttachMISO(jtag.spi, jtag.config.pin_tdo);

    jtag_idle_pins();

    /* Initialize all buffers to a canary */
    for (unsigned int i = 0; i < 18; i++)
	hw->data_buf[i] = (i+1) * 0x01010101;

    /* Set bit order. Byte order is always littleendian. */
    uint32_t spi_ctrl_reg = 0;
    if (!jtag.config.be)
	spi_ctrl_reg |= SPI_RD_BIT_ORDER_M | SPI_WR_BIT_ORDER_M;

    hw->ctrl.val = spi_ctrl_reg;

    /* Enable buffers 16 & 17 */
    hw->ctrl1.val = SPI_W16_17_WR_ENA_M;

    /*
     * Hack: set 8 cycles of CS HOLD at the end of each SPI transaction;
     * this appears needed to flush out any fractional TDO bits.
     */
    /*
     * Register is called "slave" but interrupt control applies
     * to master mode too:
     * Disable all SPI interrupts sources
     */
    hw->slave.val = 0;

    /*
     * Allocate and enable the interrupt.
     * We want this to be a low priority interrupt (level 1).
     */
    const int spi_irq = spicommon_irqsource_for_host(JTAG_SPI_HOST);
    esp_intr_alloc(spi_irq, ESP_INTR_FLAG_LEVEL1,
		   jtag_isr, &jtag, &jtag.intr_handle);

    /*
     * enable trans_done IRQ
     */
    hw->slave.val = SPI_INT_TRANS_DONE_EN_M;

#if DEBUG > 1
    dump_spi_regs();
#endif

    jtag.tap_state = TAP_RUN_TEST_IDLE; /* Pure guess */

    printf("[JTAG] JTAG enabled\n");
    return 0;
}

int jtag_init(void)
{
    jtag.configured = false;
    return 0;
}

/*
 * The Arduino HAL API uses busy wait, so here is our own routine:
 * it can transfer up to 72 bytes at a time at arbitrary bit offsets
 * but requires that the actual state pointers are 32-bit aligned;
 * this is handled in jtag_io().
 *
 * In bigendian mode, this treats the input as a stream of *bytes*; a
 * bit offset or length that is not a multiple of 8 will result in
 * garbage data. The hardware always uses litteendian byte ordering,
 * but the bit ordering is set as appropriate.
 *
 * This is event driven, with the ISR simply unblocking the JTAG thread
 * via notifications. This isn't particularly fast, but avoids starving
 * higher-priority processes; it is assumed that the JTAG/SPI transactions
 * are not particularly performance critical and, being selectively
 * clocked, can handle significant pauses.
 *
 * Using DMA and/or double-buffering would reduce CPU overhead and/or
 * improve performance, but this is very simple especially with
 * arbitrary bit offsets and lengths.
 */
static int jtag_do_chunk(void)
{
    spi_dev_t * const hw = jtag.spi->dev;
    unsigned int bits;
    unsigned int tdo_bits_read;
    unsigned int i;
    const uint32_t *tdi = jtag.tdi;
    uint32_t *tdo = jtag.tdo;

    if (hw->cmd.usr)
	return -1;		/* Missed interrupt? */
    xTaskNotifyStateClearIndexed(jtag.task, NOTIFY_INDEX);

#if DEBUG > 1
    MSG("SPI: in jtag_do_chunk\n");
#endif

    tdo_bits_read = jtag.tdo_bits_read;

    if (tdo_bits_read) {
	/* Copy output data from SPI buffers */
	unsigned int tdo_offs = jtag.tdo_bitoffs;

#if DEBUG > 1
	MSG("SPI: *** After\n");
	dump_spi_regs();
#endif
	MSG("TDO: %u bits @ %p[%u]", tdo_bits_read, tdo, tdo_offs);

	const unsigned int fullwords = (tdo_bits_read + tdo_offs) >> 5;
	const unsigned int trailing = (tdo_bits_read + tdo_offs) & 31;
	const volatile uint32_t *p = hw->data_buf;
	uint32_t o;

	if (!tdo_offs) {
	    for (i = 0; i < fullwords; i++)
		*tdo++ = *p++;
	    o = 0;
	} else {
	    o = *tdo & LMASK(tdo_offs);
	    for (i = 0; i < fullwords; i++) {
		uint32_t w = *p++;
		*tdo++ = (w << tdo_offs) | o;
		o = w >> tdo_offs;
	    }
	}

	/*
	 * o here has tdo_offs == 0..31 bits of valid data,
	 * but we might need more than that.
	 */
	if (trailing > tdo_offs)
	    o |= *p << tdo_offs;

	const unsigned int trail_mask = LMASK(trailing);
	const unsigned int trail_omask = ~LMASK((trailing+7) & ~7);
	*tdo = (*tdo & trail_omask) | (o & trail_mask);

	MSG(" -> %p[%u]\n", tdo, trailing);

	jtag.tdo_bits_read = 0;
	jtag.tdo = tdo;
	jtag.tdo_bitoffs = trailing;
    }

    /* Now the previous chunk, if any, is complete */
    bits = jtag.bits;
    if (!bits)
	return 0;		/* Done */

    /*
     * Clamp the SPI transaction size to the size of the buffers;
     * for a totally dummy transaction just let the hardware run
     * until it's done.
     */
    const unsigned int max_bits = (sizeof hw->data_buf) << 3;
    if (tdo) {
	if (bits > max_bits)
	    bits = max_bits;
    }

    if (tdi) {
	/* Copy input data to SPI buffers */
	unsigned int tdi_offs = jtag.tdi_bitoffs;

	/* Clamp to max_bits-tdi_offs in the hope of future good alignment */
	if (bits > max_bits - tdi_offs)
	    bits = max_bits - tdi_offs;

	const unsigned int words  = (bits+31) >> 5;
	const uint32_t *p = tdi;
	volatile uint32_t *q = hw->data_buf;

	MSG("TDI: %u bits @ %p[%u]", bits, tdi, tdi_offs);

	if (!tdi_offs) {
	    for (i = 0; i < words; i++)
		*q++ = *p++;
	} else {
	    const unsigned int inv_offs = 32 - tdi_offs;
	    uint32_t o;

	    o = *p++;
	    for (i = 0; i < words; i++) {
		uint32_t w = *p++;
		*q++ = (w << inv_offs) | (o >> tdi_offs);
		o = w;
	    }
	}

	tdi     += (bits + tdi_offs) >> 5;
	tdi_offs = (bits + tdi_offs) & 31;

	CMSG(" -> %p[%u]\n", tdi, tdi_offs);

	jtag.tdi = tdi;
	jtag.tdi_bitoffs = tdi_offs;
    }

    /*
     * No command, address, or dummy phases. Byte order is always
     * littleendian.
     */
    hw->miso_dlen.val = bits-1;
    hw->mosi_dlen.val = bits-1;	/* Regardless of if there is a tdi or not */

    uint32_t spi_user_reg = SPI_DOUTDIN | SPI_USR_MOSI; /* Per ESP-IDF HAL */
    if (tdo)
	spi_user_reg |= SPI_USR_MISO;
    hw->user.val = spi_user_reg;

#if DEBUG > 1
    MSG("SPI: *** Before\n");
    dump_spi_regs();
#endif
    jtag.bits -= bits;
    jtag.tdo_bits_read = tdo ? bits : 0;

    if (jtag.tdo_bits_read) {
	MSG("SPI: reading %u bits, %u left\n",
	    jtag.tdo_bits_read, jtag.bits);
    }

    hw->cmd.val = SPI_USR_M; /* Start! */
    return bits;
}

static void ARDUINO_ISR_ATTR jtag_isr(void *jtag_p)
{
    struct jtag_state * const jtag = (struct jtag_state *)jtag_p;
    spi_dev_t * const hw = jtag->spi->dev;

    uint32_t spi_slave_reg = hw->slave.val;
    if (!(spi_slave_reg & SPI_TRANS_DONE_M))
	return;			/* Spurious/shared interrupt? */

    hw->slave.val = spi_slave_reg & ~SPI_TRANS_DONE_M;

    BaseType_t do_wakeup = pdFALSE;
    xTaskNotifyIndexedFromISR(jtag->task, NOTIFY_INDEX, 1,
			      eSetBits, &do_wakeup);
    if (do_wakeup)
	portYIELD_FROM_ISR(do_wakeup);
}

static inline bool spi_running(void)
{
    return jtag.spi->dev->cmd.usr;
}

#define APB_CLK_PER_READ 2	/* Plain guess */

/*
 * Set the value of the TMS/CS# line without any clocks.
 */
int jtag_set_tms(bool tms)
{
    set_gpio_mask(jtag.pin_tms_mask, tms);
    return 0;
}

/*
 * Bitbang up to 32 bits with TDI and TMS support; returns TDO.
 *
 * In bigendian mode, the whole value is treated as bigendian, including
 * byte order, but is still expected to be right-justified if bits < 32;
 * e.g. if the expected output is the 16-bit value 0x1234
 * (transmitted as 0001001000110100), tdi should be 0x1234, not
 * 0x12340000 or 0x3412.
 */
static inline void jtag_pulse_sleep(void)
{
    unsigned int loops = jtag.bitbang_loops;
    while (loops--)
	(void)GPIO.in;
}

uint32_t jtag_pulse(unsigned int bits, uint32_t tdi, uint32_t tms)
{
    uint32_t tdo = 0;
    uint32_t bit_val;

    MSG("SPI: bitbang %u bits TDI 0x%x TMS 0x%x", bits, tdi, tms);

    jtag_config_tck(false);
    jtag_config_tdi(false);

    /* PIN_TCK is always 0 between transactions */
    if (!jtag.config.be) {
	/* Littleendian */

	bit_val = 1;
	while (bits--) {
	    set_gpio_mask(jtag.pin_tms_mask, tms & 1); tms >>= 1;
	    set_gpio_mask(jtag.pin_tdi_mask, tdi & 1); tdi >>= 1;
	    jtag_pulse_sleep();
	    set_gpio_mask(jtag.pin_tck_mask, 1);
	    if (get_gpio_mask(jtag.pin_tdo_mask))
		tdo |= bit_val;
	    bit_val <<= 1;
	    jtag_pulse_sleep();
	    set_gpio_mask(jtag.pin_tck_mask, 0);
	}
    } else {
	/* Bigendian */

	bit_val = 1U << (bits-1);
	while (bits--) {
	    set_gpio_mask(jtag.pin_tms_mask, tms & bit_val);
	    set_gpio_mask(jtag.pin_tdi_mask, tdi & bit_val);
	    bit_val >>= 1;
	    jtag_pulse_sleep();
	    set_gpio_mask(jtag.pin_tck_mask, 1);
	    tdo = (tdo << 1) | !!get_gpio_mask(jtag.pin_tdo_mask);
	    jtag_pulse_sleep();
	    set_gpio_mask(jtag.pin_tck_mask, 0);
	}
    }

    CMSG(" -> TDO 0x%x\n", tdo);
    return tdo;
}

static void jtag_go(unsigned int bits)
{
    int chunk;

    MSG("SPI: jtag_go bits %u, tms 0, tdi %p[%u], tdo %p[%u]\n",
	bits, jtag.tdi, jtag.tdi_bitoffs, jtag.tdo, jtag.tdo_bitoffs);

    jtag.bits = bits;
    jtag.tdo_bits_read = 0;

    while ((chunk = jtag_do_chunk())) {
	MSG("SPI: started chunk of %u bits\n", chunk);
	while (spi_running())
	    xTaskNotifyWaitIndexed(NOTIFY_INDEX, 0, 1, NULL,
				   jtag.spi_timeout_ticks);
    }
}

void jtag_delay(unsigned int us)
{
    jtag_config_tck(false);	/* Don't emit a clock signal */
    jtag_config_tdi(false);

    jtag.tdi = jtag.tdo = NULL;
    jtag_go(jtag_us_to_clocks(us));
}

/*
 * Transmit an arbitrary number of bits. In bigendian mode,
 * the bytes will be transferred in the order presented, but
 * with the most significant bit first; if there is a fractional
 * byte at the end it should be right-justified.
 */
int jtag_io(unsigned int bits, enum jtag_io_flags flags,
	    const void *tdi, void *tdo)
{
    unsigned int s_bits;	/* Bits for SPI engine  */
    unsigned int b_bits;	/* Bits to bitbang */

#if DEBUG > 1
    MSG("SPI: jtag_io bits %u, tms %u, tdi %p, tdo %p\n",
	   bits, tms, tdi, tdo);
#endif

    if (!bits) {
	jtag_set_tms(!!(flags & (JIO_TMS|JIO_CS)));
	jtag_pulse_sleep();
	return 0;
    }

    unsigned int tms = !!(flags & JIO_TMS);

    /*
     * Bitbang fractional bytes and TMS cycles; this works around some
     * bugs in ESP32-S2 and makes the rest of the logic simpler.
     */
    s_bits = (bits - tms) & ~7;
    b_bits = bits - s_bits;

    MSG("jtag_io s_bits = %u, b_bits = %u, tms = %u\n",
	s_bits, b_bits, tms);

    if (s_bits) {
	/* Align pointers and set the bit offset accordingly. */
	jtag.tdi = (const uint32_t *)((size_t)tdi & ~3);
	jtag.tdo = (uint32_t *)((size_t)tdo & ~3);
	jtag.tdi_bitoffs = ((size_t)tdi & 3) << 3;
	jtag.tdo_bitoffs = ((size_t)tdo & 3) << 3;

	jtag_config_tck(true);
	jtag_config_tdi(!!tdi);
	jtag_set_tms(0);

	 /* Default TDI value if !tdi */
	set_gpio_mask(jtag.pin_tdi_mask, !!(flags & JIO_TDI));
	jtag_go(s_bits);
	MSG("jtag_io: tdi = %p[%u], tdo = %p[%u]\n",
	    jtag.tdi, jtag.tdi_bitoffs, jtag.tdo, jtag.tdo_bitoffs);
    }

    if (b_bits) {
	unsigned int s_bytes        = s_bits >> 3;
	const uint8_t * const tdi_p = (const uint8_t *)tdi;
	uint8_t * const tdo_p       = (uint8_t *)tdo;
	uint32_t tdi_d, tdo_d;

	tdi_d = tdi ? tdi_p[s_bytes] : -!!(flags & JIO_TDI);
	tdo_d = jtag_pulse(b_bits, tdi_d, (uint32_t)tms << (b_bits-1));
	if (tdo)
	    tdo_p[s_bytes] = tdo_d;
    }

    jtag.tap_state += tms;	/* SHIFT_DR/IR -> EXIT1_DR/IR */

    if (flags & JIO_CS) {
	jtag_set_tms(true);
	jtag_pulse_sleep();
    }

    return bits;
}

int tap_goto_state(enum TAP_STATE to_state)
{
    enum TAP_STATE current = jtag.tap_state;
    uint16_t tms_map = tap_state_route[to_state];
    int bits = 0;
    uint32_t tms = 0;

    MSG("~~~ moving to TAP state %s %2u from %s %2u, TMS = ",
	tap_state_names[to_state], to_state,
	tap_state_names[current], current);
    if (current == to_state)
	return 0;

    while (current != to_state) {
	uint32_t next_tms = (tms_map >> current) & 1;
	CMSG("%u", next_tms);
	tms |= next_tms << bits++;
	enum TAP_STATE next_state = tap_state_next[current][next_tms];
	current = next_state;
    }
    CMSG("\n");

    jtag_pulse(bits, -1, tms);
    jtag.tap_state = current;
    return 0;
}

/* Force the TAP into the TEST_LOGIC_RESET state */
int tap_reset(void)
{
    /* Per spec, only 5 pulses with TMS = 1 are needed */
    jtag_pulse(5, -1, -1);
    jtag.tap_state = TAP_TEST_LOGIC_RESET;
    return 0;
}

/*
 * Set the IR register
 */
void tap_set_ir(uint32_t ir, unsigned int bits)
{
    tap_goto_state(TAP_SHIFT_IR);
    jtag_pulse(bits, ir, 1U << (bits-1));
    jtag.tap_state = TAP_EXIT1_IR;
    tap_goto_state(TAP_UPDATE_IR);
}

/*
 * Issue a certain number of clocks in the RUN_TEST state
 */
void tap_run_test_idle(unsigned int clocks)
{
    tap_goto_state(TAP_RUN_TEST_IDLE);
    jtag_io(clocks, JIO_TDI, NULL, NULL);
}
