jtag_vpi: fixed state transitions in "stableclocks"

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

diff --git a/src/jtag/drivers/jtag_vpi.c b/src/jtag/drivers/jtag_vpi.c
index c69f24fd0629ff5d90e6b789d125dd579b85f1b6..1033cedb28c95a18a38483afee01b2ebd9100a46 100644 (file)
--- a/src/jtag/drivers/jtag_vpi.c
+++ b/src/jtag/drivers/jtag_vpi.c
@@ -328,7 +328,27 @@ static int jtag_vpi_runtest(int cycles, tap_state_t state)
 
 static int jtag_vpi_stableclocks(int cycles)
 {
-       return jtag_vpi_queue_tdi(NULL, cycles, TAP_SHIFT);
+       uint8_t tms_bits[4];
+       int cycles_remain = cycles;
+       int nb_bits;
+       int retval;
+       const int CYCLES_ONE_BATCH = sizeof(tms_bits) * 8;
+
+       assert(cycles >= 0);
+
+       /* use TMS=1 in TAP RESET state, TMS=0 in all other stable states */
+       memset(&tms_bits, (tap_get_state() == TAP_RESET) ? 0xff : 0x00, sizeof(tms_bits));
+
+       /* send the TMS bits */
+       while (cycles_remain > 0) {
+               nb_bits = (cycles_remain < CYCLES_ONE_BATCH) ? cycles_remain : CYCLES_ONE_BATCH;
+               retval = jtag_vpi_tms_seq(tms_bits, nb_bits);
+               if (retval != ERROR_OK)
+                       return retval;
+               cycles_remain -= nb_bits;
+       }
+
+       return ERROR_OK;
 }
 
 static int jtag_vpi_execute_queue(void)