diff --git a/src/include/jtagtap.h b/src/include/jtagtap.h
index b681859..56668ad 100644
--- a/src/include/jtagtap.h
+++ b/src/include/jtagtap.h
@@ -37,7 +37,7 @@ typedef struct jtag_proc_s {
 	 * - Release TCK.
 	 */
 	uint8_t (*jtagtap_next)(const uint8_t tms, const uint8_t tdi);
-	void (*jtagtap_tms_seq)(uint32_t tms_states, int ticks);
+	void (*jtagtap_tms_seq)(uint32_t tms_states, int clock_cycles);
 
 	/*
 	 * Shift out a sequence on MS and DI, capture data to DO.
@@ -45,8 +45,8 @@ typedef struct jtag_proc_s {
 	 * - DO may be NULL to ignore captured data.
 	 * - DO may be point to the same address as DI.
 	 */
-	void (*jtagtap_tdi_tdo_seq)(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
-	void (*jtagtap_tdi_seq)(const uint8_t final_tms, const uint8_t *DI, int ticks);
+	void (*jtagtap_tdi_tdo_seq)(uint8_t *data_out, const uint8_t final_tms, const uint8_t *data_in, int clock_cycles);
+	void (*jtagtap_tdi_seq)(const uint8_t final_tms, const uint8_t *DI, int clock_cycles);
 } jtag_proc_t;
 
 extern jtag_proc_t jtag_proc;
diff --git a/src/platforms/common/jtagtap.c b/src/platforms/common/jtagtap.c
index db91568..915d1ac 100644
--- a/src/platforms/common/jtagtap.c
+++ b/src/platforms/common/jtagtap.c
@@ -122,63 +122,72 @@ static void jtagtap_tms_seq(uint32_t MS, int ticks)
 		jtagtap_tms_seq_no_delay(MS, ticks);
 }
 
+static void jtagtap_tdi_tdo_seq_swd_delay(const uint8_t *data_in, uint8_t *data_out, const bool final_tms, size_t clock_cycles)
+{
+	size_t byte = 0;
+	size_t index = 0;
+	uint8_t value = 0;
+	while (clock_cycles--) {
+		/* On the last cycle, assert final_tms to TMS_PIN */
+		if (!clock_cycles)
+			gpio_set_val(TMS_PORT, TMS_PIN, final_tms);
+		/* Set up the TDI pin and start the clock cycle */
+		gpio_set_val(TDI_PORT, TDI_PIN, data_in[byte] & (1 << index));
+		gpio_set(TCK_PORT, TCK_PIN);
+		for (volatile int32_t cnt = swd_delay_cnt - 2; cnt > 0; cnt--)
+			continue;
+		/* If TDO is high, store a 1 in the appropriate position in the value being accumulated */
+		if (gpio_get(TDO_PORT, TDO_PIN))
+			value |= (1 << index);
+		if (index++ == 7U) {
+			data_out[byte++] = value;
+			index = 0;
+			value = 0;
+		}
+		/* Finish the clock cycle */
+		gpio_clear(TCK_PORT, TCK_PIN);
+		for (volatile int32_t cnt = swd_delay_cnt - 2; cnt > 0; cnt--)
+			continue;
+	}
+	if (index)
+		data_out[byte] = value;
+}
+
+static void jtagtap_tdi_tdo_seq_no_delay(const uint8_t *data_in, uint8_t *data_out, const bool final_tms, size_t clock_cycles)
+{
+	size_t byte = 0;
+	size_t index = 0;
+	uint8_t value = 0;
+	while (clock_cycles--) {
+		/* On the last tick, assert final_tms to TMS_PIN */
+		if (!clock_cycles)
+			gpio_set_val(TMS_PORT, TMS_PIN, final_tms);
+		/* Set up the TDI pin and start the clock cycle */
+		gpio_set_val(TDI_PORT, TDI_PIN, data_in[byte] & (1 << index));
+		gpio_set(TCK_PORT, TCK_PIN);
+		/* If TDO is high, store a 1 in the appropriate position in the value being accumulated */
+		if (gpio_get(TDO_PORT, TDO_PIN))
+			value |= (1 << index);
+		/* If we've got to the next whole byte, store the accumulated value and reset state */
+		if (index++ == 7U) {
+			data_out[byte++] = value;
+			index = 0;
+			value = 0;
+		}
+		/* Finish the clock cycle */
+		gpio_clear(TCK_PORT, TCK_PIN);
+	}
+	if (index)
+		data_out[byte] = value;
+}
+
 static void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
 {
-	uint8_t index = 1;
-	gpio_set_val(TMS_PORT, TMS_PIN, 0);
-	uint8_t res = 0;
-	register volatile int32_t cnt;
-	if (swd_delay_cnt) {
-		while (ticks > 1) {
-			gpio_set_val(TDI_PORT, TDI_PIN, *DI & index);
-			gpio_set(TCK_PORT, TCK_PIN);
-			for (cnt = swd_delay_cnt - 2; cnt > 0; cnt--)
-				;
-			if (gpio_get(TDO_PORT, TDO_PIN)) {
-				res |= index;
-			}
-			if (!(index <<= 1)) {
-				*DO = res;
-				res = 0;
-				index = 1;
-				DI++;
-				DO++;
-			}
-			ticks--;
-			gpio_clear(TCK_PORT, TCK_PIN);
-			for (cnt = swd_delay_cnt - 2; cnt > 0; cnt--)
-				;
-		}
-	} else {
-		while (ticks > 1) {
-			gpio_set_val(TDI_PORT, TDI_PIN, *DI & index);
-			gpio_set(TCK_PORT, TCK_PIN);
-			if (gpio_get(TDO_PORT, TDO_PIN)) {
-				res |= index;
-			}
-			if (!(index <<= 1)) {
-				*DO = res;
-				res = 0;
-				index = 1;
-				DI++;
-				DO++;
-			}
-			ticks--;
-			gpio_clear(TCK_PORT, TCK_PIN);
-		}
-	}
-	gpio_set_val(TMS_PORT, TMS_PIN, final_tms);
-	gpio_set_val(TDI_PORT, TDI_PIN, *DI & index);
-	gpio_set(TCK_PORT, TCK_PIN);
-	for (cnt = swd_delay_cnt - 2; cnt > 0; cnt--)
-		;
-	if (gpio_get(TDO_PORT, TDO_PIN)) {
-		res |= index;
-	}
-	*DO = res;
-	gpio_clear(TCK_PORT, TCK_PIN);
-	for (cnt = swd_delay_cnt - 2; cnt > 0; cnt--)
-		;
+	gpio_clear(TDI_PORT, TDI_PIN);
+	if (swd_delay_cnt)
+		jtagtap_tdi_tdo_seq_swd_delay(DI, DO, final_tms, ticks);
+	else
+		jtagtap_tdi_tdo_seq_no_delay(DI, DO, final_tms, ticks);
 }
 
 static void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI, int ticks)