diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c index c00feba4da1..6f51f761c14 100644 --- a/src/platforms/common/swdptap.c +++ b/src/platforms/common/swdptap.c @@ -58,8 +58,6 @@ void swdptap_init(void) static inline void platform_delay_busy(const uint32_t loops) { #if 1 - if (loops < 1U) - return; register uint32_t i = loops; do { __asm__("nop"); @@ -86,11 +84,13 @@ static void swdptap_turnaround(const swdio_status_t dir) SWDIO_MODE_FLOAT(); } else { gpio_clear(SWCLK_PORT, SWCLK_PIN); - platform_delay_busy(target_clk_divider + 1); + if (target_clk_divider != UINT32_MAX) + platform_delay_busy(target_clk_divider); } gpio_set(SWCLK_PORT, SWCLK_PIN); - platform_delay_busy(target_clk_divider + 1); + if (target_clk_divider != UINT32_MAX) + platform_delay_busy(target_clk_divider); if (dir == SWDIO_STATUS_DRIVE) { SWDIO_MODE_DRIVE(); @@ -141,13 +141,15 @@ static uint32_t swdptap_seq_in(size_t clock_cycles) static bool swdptap_seq_in_parity(uint32_t *ret, size_t clock_cycles) { const uint32_t result = swdptap_seq_in(clock_cycles); - platform_delay_busy(target_clk_divider + 1); + if (target_clk_divider != UINT32_MAX) + platform_delay_busy(target_clk_divider); size_t parity = __builtin_popcount(result); parity += gpio_get(SWDIO_IN_PORT, SWDIO_IN_PIN) ? 1U : 0U; gpio_set(SWCLK_PORT, SWCLK_PIN); - platform_delay_busy(target_clk_divider + 1); + if (target_clk_divider != UINT32_MAX) + platform_delay_busy(target_clk_divider); *ret = result; /* Terminate the read cycle now */ @@ -197,11 +199,16 @@ static void swdptap_seq_out(const uint32_t tms_states, const size_t clock_cycles static void swdptap_seq_out_parity(const uint32_t tms_states, const size_t clock_cycles) { - int parity = __builtin_popcount(tms_states); + const int parity = __builtin_popcount(tms_states); swdptap_seq_out(tms_states, clock_cycles); + gpio_set_val(SWDIO_PORT, SWDIO_PIN, parity & 1U); - platform_delay_busy(target_clk_divider + 1); + if (target_clk_divider != UINT32_MAX) + platform_delay_busy(target_clk_divider); + gpio_set(SWCLK_PORT, SWCLK_PIN); - platform_delay_busy(target_clk_divider + 1); + if (target_clk_divider != UINT32_MAX) + platform_delay_busy(target_clk_divider); + gpio_clear(SWCLK_PORT, SWCLK_PIN); }