diff --git a/src/include/general.h b/src/include/general.h index e4c0093924c..027c8fdc028 100644 --- a/src/include/general.h +++ b/src/include/general.h @@ -48,7 +48,6 @@ #include #include -#include "maths_utils.h" #include "timing.h" #include "platform_support.h" #include "align.h" diff --git a/src/include/maths_utils.h b/src/include/maths_utils.h index d0a81e59dfc..ed501e5a71c 100644 --- a/src/include/maths_utils.h +++ b/src/include/maths_utils.h @@ -37,5 +37,6 @@ #include uint8_t ulog2(uint32_t value); +uint8_t calculate_odd_parity(uint32_t value); #endif /* INCLUDE_MATHS_UTILS_H */ diff --git a/src/maths_utils.c b/src/maths_utils.c index 0f055ca3761..ac9c49c1b90 100644 --- a/src/maths_utils.c +++ b/src/maths_utils.c @@ -74,3 +74,21 @@ uint8_t ulog2(uint32_t value) return (sizeof(uint8_t) * 8U) - result; #endif } + +uint8_t calculate_odd_parity(const uint32_t value) +{ +#if defined(__GNUC__) + /* Ask for the libgcc impl */ + return __builtin_parity(value); +#elif defined(_MSC_VER) + /* Ask for a CPU insn */ + return __popcount(value) & 1U; +#else + /* Generic impl */ + uint8_t result = 0; + for (uint32_t bit = 0; bit < 32; ++bit) + result ^= (value >> bit) & 1U; + + return result; +#endif +} diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c index 09751286808..8a1a42b75fb 100644 --- a/src/platforms/common/swdptap.c +++ b/src/platforms/common/swdptap.c @@ -24,6 +24,7 @@ #include "platform.h" #include "timing.h" #include "swd.h" +#include "maths_utils.h" #if !defined(SWDIO_IN_PORT) #define SWDIO_IN_PORT SWDIO_PORT @@ -130,8 +131,8 @@ static bool swdptap_seq_in_parity(uint32_t *ret, size_t clock_cycles) for (volatile uint32_t counter = target_clk_divider + 1; counter > 0; --counter) continue; - size_t parity = __builtin_popcount(result); - parity += gpio_get(SWDIO_IN_PORT, SWDIO_IN_PIN) ? 1U : 0U; + const bool parity = calculate_odd_parity(result); + const bool bit = gpio_get(SWDIO_IN_PORT, SWDIO_IN_PIN); gpio_set(SWCLK_PORT, SWCLK_PIN); for (volatile uint32_t counter = target_clk_divider + 1; counter > 0; --counter) @@ -140,7 +141,7 @@ static bool swdptap_seq_in_parity(uint32_t *ret, size_t clock_cycles) *ret = result; /* Terminate the read cycle now */ swdptap_turnaround(SWDIO_STATUS_DRIVE); - return parity & 1U; + return parity != bit; } static void swdptap_seq_out_clk_delay(uint32_t tms_states, size_t clock_cycles) __attribute__((optimize(3))); @@ -182,9 +183,9 @@ 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 bool parity = calculate_odd_parity(tms_states); swdptap_seq_out(tms_states, clock_cycles); - gpio_set_val(SWDIO_PORT, SWDIO_PIN, parity & 1U); + gpio_set_val(SWDIO_PORT, SWDIO_PIN, parity); for (volatile uint32_t counter = target_clk_divider + 1; counter > 0; --counter) continue; gpio_set(SWCLK_PORT, SWCLK_PIN); diff --git a/src/platforms/hosted/dap_swd.c b/src/platforms/hosted/dap_swd.c index 8da65855256..9395ac7516d 100644 --- a/src/platforms/hosted/dap_swd.c +++ b/src/platforms/hosted/dap_swd.c @@ -34,6 +34,7 @@ #include "dap.h" #include "dap_command.h" #include "buffer_utils.h" +#include "maths_utils.h" typedef enum dap_swd_turnaround_cycles { DAP_SWD_TURNAROUND_1_CYCLE = 0U, @@ -127,7 +128,7 @@ static void dap_swd_seq_out_parity(const uint32_t tms_states, const size_t clock DAP_SWD_OUT_SEQUENCE, }; write_le4(sequence.data, 0, tms_states); - sequence.data[4] = __builtin_parity(tms_states); + sequence.data[4] = calculate_odd_parity(tms_states); /* And perform it */ if (!perform_dap_swd_sequences(&sequence, 1U)) DEBUG_ERROR("dap_swd_seq_out_parity failed\n"); @@ -169,7 +170,7 @@ static bool dap_swd_seq_in_parity(uint32_t *const result, const size_t clock_cyc for (size_t offset = 0; offset < clock_cycles; offset += 8U) data |= sequence.data[offset >> 3U] << offset; *result = data; - uint8_t parity = __builtin_parity(data) & 1U; + uint8_t parity = calculate_odd_parity(data); parity ^= sequence.data[4] & 1U; return !parity; } @@ -194,7 +195,7 @@ static bool dap_write_reg_no_check(const uint16_t addr, const uint32_t data) 33U, DAP_SWD_OUT_SEQUENCE, /* The 4 data bytes are filled in below with write_le4() */ - {0U, 0U, 0U, 0U, __builtin_parity(data)}, + {0U, 0U, 0U, 0U, calculate_odd_parity(data)}, }, }; write_le4(sequences[3].data, 0, data); diff --git a/src/platforms/hosted/ftdi_swd.c b/src/platforms/hosted/ftdi_swd.c index 78f7395941f..7fadcc5bc36 100644 --- a/src/platforms/hosted/ftdi_swd.c +++ b/src/platforms/hosted/ftdi_swd.c @@ -32,6 +32,7 @@ #include #include "ftdi_bmp.h" #include "buffer_utils.h" +#include "maths_utils.h" typedef enum swdio_status { SWDIO_STATUS_DRIVE, @@ -237,7 +238,9 @@ static bool ftdi_swd_seq_in_parity_mpsse(uint32_t *const result, const size_t cl uint8_t data_out[5U] = {0}; ftdi_jtag_tdi_tdo_seq(data_out, false, NULL, clock_cycles + 1U); const uint32_t data = read_le4(data_out, 0); - uint8_t parity = __builtin_parity(data & ((UINT64_C(1) << clock_cycles) - 1U)); + /* NB: This calculation must be done in 64-bit space due to `1U << 32U` value of 0x00000001'00000000ULL */ + const uint32_t bitmask = (UINT64_C(1) << clock_cycles) - 1U; + uint8_t parity = calculate_odd_parity(data & bitmask); parity ^= data_out[4] & 1U; DEBUG_PROBE("%s %zu clock_cycles: %08" PRIx32 " %s\n", __func__, clock_cycles, data, parity ? "ERR" : "OK"); *result = data; @@ -428,7 +431,7 @@ static void ftdi_swd_seq_out_parity(uint32_t tms_states, size_t clock_cycles) { if (clock_cycles > 32U) return; - const uint8_t parity = __builtin_parity(tms_states) & 1U; + const uint8_t parity = calculate_odd_parity(tms_states); ftdi_swd_turnaround(SWDIO_STATUS_DRIVE); if (do_mpsse) ftdi_swd_seq_out_parity_mpsse(tms_states, parity, clock_cycles); diff --git a/src/platforms/hosted/jlink_swd.c b/src/platforms/hosted/jlink_swd.c index 10a85c10d7d..31a873f5296 100644 --- a/src/platforms/hosted/jlink_swd.c +++ b/src/platforms/hosted/jlink_swd.c @@ -34,6 +34,7 @@ #include "jlink.h" #include "jlink_protocol.h" #include "buffer_utils.h" +#include "maths_utils.h" #include "cli.h" /* @@ -121,7 +122,7 @@ static void jlink_swd_seq_out_parity(const uint32_t tms_states, const size_t clo /* Construct the parity bit */ const size_t byte = clock_cycles >> 3U; const uint8_t bit = clock_cycles & 7U; - data[byte] |= (__builtin_parity(tms_states) & 1U) << bit; + data[byte] |= calculate_odd_parity(tms_states) << bit; /* Attempt the transfer */ if (!jlink_transfer_swd(clock_cycles + 1U, JLINK_SWD_OUT, data, NULL)) { /* If things go wrong, report it */ @@ -159,9 +160,9 @@ static bool jlink_swd_seq_in_parity(uint32_t *const result, const size_t clock_c /* Compute the parity and validate it */ const size_t byte = clock_cycles >> 3U; const uint8_t bit = clock_cycles & 7U; - uint8_t parity = __builtin_parity(data) & 1U; + uint8_t parity = calculate_odd_parity(data); parity ^= (data_out[byte] >> bit) & 1U; - /* Retrn the result of the calculation */ + /* Return the result of the calculation */ DEBUG_PROBE("%s %zu clock_cycles: %08" PRIx32 " %s\n", __func__, clock_cycles, data, parity ? "ERR" : "OK"); *result = data; return !parity; @@ -183,7 +184,7 @@ static bool jlink_adiv5_raw_write_no_check(const uint16_t addr, const uint32_t d /* Build the response payload buffer */ uint8_t response[6] = {0}; write_le4(response, 0, data); - response[4] = __builtin_popcount(data) & 1U; + response[4] = calculate_odd_parity(data); /* Try sending the data to the device */ if (!jlink_transfer(33U + 8U, jlink_adiv5_write_request, response, NULL)) { DEBUG_ERROR("jlink_adiv5_raw_write_no_check failed\n"); @@ -213,7 +214,7 @@ static uint32_t jlink_adiv5_raw_read_no_check(const uint16_t addr) /* Extract the data phase and return it if the transaction succeeded */ const uint32_t data = read_le4(response, 0); DEBUG_PROBE("jlink_adiv5_raw_read_no_check %04x -> %08" PRIx32 " %s\n", addr, data, - __builtin_parity(data) ^ response[4] ? "ERR" : "OK"); + calculate_odd_parity(data) != response[4] ? "ERR" : "OK"); return ack == SWDP_ACK_OK ? data : 0U; } @@ -228,7 +229,7 @@ static uint32_t jlink_adiv5_raw_read(adiv5_debug_port_s *const dp) /* Extract the data phase */ const uint32_t response = read_le4(result, 0); /* Calculate and do a parity check */ - uint8_t parity = __builtin_parity(response) & 1U; + uint8_t parity = calculate_odd_parity(response); parity ^= result[4] & 1U; /* If that fails, turn it into an error */ if (parity) { @@ -244,7 +245,7 @@ static uint32_t jlink_adiv5_raw_write(const uint32_t request_value) /* Build the response payload buffer */ uint8_t request[6] = {0}; write_le4(request, 0, request_value); - request[4] = __builtin_popcount(request_value) & 1U; + request[4] = calculate_odd_parity(request_value); /* Allocate storage for the result */ uint8_t result[6] = {0}; /* Try sending the data to the device */ diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c index 68411c97d33..e0e061c1a1c 100644 --- a/src/platforms/hosted/stlinkv2.c +++ b/src/platforms/hosted/stlinkv2.c @@ -35,6 +35,7 @@ #include "exception.h" #include "cortexm.h" #include "buffer_utils.h" +#include "maths_utils.h" #include #include diff --git a/src/target/cortexm.c b/src/target/cortexm.c index 3bb1ea8b275..6ae11a64745 100644 --- a/src/target/cortexm.c +++ b/src/target/cortexm.c @@ -42,6 +42,7 @@ #include "gdb_packet.h" #include "semihosting.h" #include "platform.h" +#include "maths_utils.h" #include #include