diff --git a/src/include/swdptap.h b/src/include/swdptap.h
index c51a28c2..706a21ad 100644
--- a/src/include/swdptap.h
+++ b/src/include/swdptap.h
@@ -23,10 +23,6 @@
 
 int swdptap_init(void);
 
-/* Primitive functions */
-bool swdptap_bit_in(void);
-void swdptap_bit_out(bool val);
-
 /* High level functions, provided as weak in swdptap_generic.c */
 uint32_t swdptap_seq_in(int ticks);
 bool swdptap_seq_in_parity(uint32_t *data, int ticks);
diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 64cdc58a..33749a4c 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -23,58 +23,4 @@
 #include "general.h"
 #include "swdptap.h"
 
-int swdptap_init(void)
-{
-	return 0;
-}
-
-static void swdptap_turnaround(uint8_t dir)
-{
-	static uint8_t olddir = 0;
-
-	/* Don't turnaround if direction not changing */
-	if(dir == olddir) return;
-	olddir = dir;
-
-#ifdef DEBUG_SWD_BITS
-	DEBUG("%s", dir ? "\n-> ":"\n<- ");
-#endif
-
-	if(dir)
-		SWDIO_MODE_FLOAT();
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-	if(!dir)
-		SWDIO_MODE_DRIVE();
-}
-
-bool swdptap_bit_in(void)
-{
-	uint16_t ret;
-
-	swdptap_turnaround(1);
-
-	ret = gpio_get(SWDIO_PORT, SWDIO_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-
-#ifdef DEBUG_SWD_BITS
-	DEBUG("%d", ret?1:0);
-#endif
-
-	return ret != 0;
-}
-
-void swdptap_bit_out(bool val)
-{
-#ifdef DEBUG_SWD_BITS
-	DEBUG("%d", val);
-#endif
-
-	swdptap_turnaround(0);
-
-	gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-}
-
+/* Optimized this by moving directly inline */
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index d62a18e8..b7009601 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -42,6 +42,17 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 
 static void adiv5_swdp_abort(ADIv5_DP_t *dp, uint32_t abort);
 
+static inline void swdptap_bit_out(bool val)
+{
+#ifdef DEBUG_SWD_BITS
+	DEBUG("%d", val);
+#endif
+
+	gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+}
+
 int adiv5_swdp_scan(void)
 {
 	uint8_t ack;
diff --git a/src/target/swdptap_generic.c b/src/target/swdptap_generic.c
index 13ee02f0..f7954100 100644
--- a/src/target/swdptap_generic.c
+++ b/src/target/swdptap_generic.c
@@ -20,12 +20,78 @@
 #include "general.h"
 #include "swdptap.h"
 
+int swdptap_init(void)
+{
+	return 0;
+}
+
+static uint8_t olddir = 0;
+
+static inline void swdptap_set_out(void)
+{
+	/* Don't turnaround if direction not changing */
+	if(0 == olddir) return;
+	olddir = 0;
+
+#ifdef DEBUG_SWD_BITS
+	DEBUG("%s", dir ? "\n-> ":"\n<- ");
+#endif
+
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	SWDIO_MODE_DRIVE();
+}
+
+
+static inline void swdptap_set_in(void)
+{
+	/* Don't turnaround if direction not changing */
+	if(1 == olddir) return;
+	olddir = 1;
+
+#ifdef DEBUG_SWD_BITS
+	DEBUG("%s", dir ? "\n-> ":"\n<- ");
+#endif
+
+	SWDIO_MODE_FLOAT();
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+}
+
+static inline bool swdptap_bit_in(void)
+{
+	uint16_t ret;
+
+	ret = gpio_get(SWDIO_PORT, SWDIO_PIN);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+
+#ifdef DEBUG_SWD_BITS
+	DEBUG("%d", ret?1:0);
+#endif
+
+	return ret != 0;
+}
+
+static inline void swdptap_bit_out(bool val)
+{
+#ifdef DEBUG_SWD_BITS
+	DEBUG("%d", val);
+#endif
+
+	gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
+	gpio_set(SWCLK_PORT, SWCLK_PIN);
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+}
+
 uint32_t __attribute__((weak))
 swdptap_seq_in(int ticks)
 {
 	uint32_t index = 1;
 	uint32_t ret = 0;
 
+	swdptap_set_in();
+
 	while (ticks--) {
 		if (swdptap_bit_in())
 			ret |= index;
@@ -42,6 +108,8 @@ swdptap_seq_in_parity(uint32_t *ret, int ticks)
 	uint8_t parity = 0;
 	*ret = 0;
 
+	swdptap_set_in();
+
 	while (ticks--) {
 		if (swdptap_bit_in()) {
 			*ret |= index;
@@ -58,6 +126,8 @@ swdptap_seq_in_parity(uint32_t *ret, int ticks)
 void __attribute__((weak))
 swdptap_seq_out(uint32_t MS, int ticks)
 {
+	swdptap_set_out();
+
 	while (ticks--) {
 		swdptap_bit_out(MS & 1);
 		MS >>= 1;
@@ -69,6 +139,8 @@ swdptap_seq_out_parity(uint32_t MS, int ticks)
 {
 	uint8_t parity = 0;
 
+	swdptap_set_out();
+
 	while (ticks--) {
 		swdptap_bit_out(MS & 1);
 		parity ^= MS;