diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 95cc68d..29278cc 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -27,6 +27,14 @@ enum {
 	SWDIO_STATUS_FLOAT = 0,
 	SWDIO_STATUS_DRIVE
 };
+static void swdptap_turnaround(int dir) __attribute__ ((optimize(3)));
+static uint32_t swdptap_seq_in(int ticks) __attribute__ ((optimize(3)));
+static bool swdptap_seq_in_parity(uint32_t *ret, int ticks)
+	__attribute__ ((optimize(3)));
+static void swdptap_seq_out(uint32_t MS, int ticks)
+	__attribute__ ((optimize(3)));
+static void swdptap_seq_out_parity(uint32_t MS, int ticks)
+	__attribute__ ((optimize(3)));
 
 static void swdptap_turnaround(int dir)
 {
@@ -76,33 +84,33 @@ static uint32_t swdptap_seq_in(int ticks)
 static bool swdptap_seq_in_parity(uint32_t *ret, int ticks)
 {
 	uint32_t index = 1;
-	uint8_t parity = 0;
 	uint32_t res = 0;
 	bool bit;
 	int len = ticks;
 
 	swdptap_turnaround(SWDIO_STATUS_FLOAT);
 	while (len--) {
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 		bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
 		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		if (bit) {
 			res |= index;
-			parity ^= 1;
 		}
 		index <<= 1;
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	int parity = __builtin_popcount(res);
 	bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	if (bit)
-		parity ^= 1;
+		parity++;
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 #ifdef DEBUG_SWD_BITS
 	for (int i = 0; i < len; i++)
 		DEBUG("%d", (res & (1 << i)) ? 1 : 0);
 #endif
 	*ret = res;
-	return parity;
+	return (parity & 1);
 }
 
 static void swdptap_seq_out(uint32_t MS, int ticks)
@@ -115,30 +123,30 @@ static void swdptap_seq_out(uint32_t MS, int ticks)
 	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 	while (ticks--) {
 		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 		MS >>= 1;
 		data = MS & 1;
 		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 }
 
 static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 {
-	uint8_t parity = 0;
+	int parity = __builtin_popcount(MS);
 	int data = MS & 1;
 #ifdef DEBUG_SWD_BITS
 	for (int i = 0; i < ticks; i++)
 		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
 #endif
 	swdptap_turnaround(SWDIO_STATUS_DRIVE);
-
+	gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+	MS >>= 1;
 	while (ticks--) {
-		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
-		parity ^= MS;
-		MS >>= 1;
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		data = MS & 1;
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
+		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+		MS >>= 1;
 		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, parity & 1);