diff --git a/include/zephyr/sys/util.h b/include/zephyr/sys/util.h index a812ee4d37ea..fcfc178b459f 100644 --- a/include/zephyr/sys/util.h +++ b/include/zephyr/sys/util.h @@ -639,6 +639,45 @@ char *utf8_lcpy(char *dst, const char *src, size_t n); (((buflen) != 0) && \ ((UINTPTR_MAX - (uintptr_t)(addr)) <= ((uintptr_t)((buflen) - 1)))) +/** + * @brief XOR n bytes + * + * @param dst Destination of where to store result. Shall be @p len bytes. + * @param src1 First source. Shall be @p len bytes. + * @param src2 Second source. Shall be @p len bytes. + * @param len Number of bytes to XOR. + */ +static inline void mem_xor_n(uint8_t *dst, const uint8_t *src1, const uint8_t *src2, size_t len) +{ + while (len--) { + *dst++ = *src1++ ^ *src2++; + } +} + +/** + * @brief XOR 32 bits + * + * @param dst Destination of where to store result. Shall be 32 bits. + * @param src1 First source. Shall be 32 bits. + * @param src2 Second source. Shall be 32 bits. + */ +static inline void mem_xor_32(uint8_t dst[4], const uint8_t src1[4], const uint8_t src2[4]) +{ + mem_xor_n(dst, src1, src2, 4U); +} + +/** + * @brief XOR 128 bits + * + * @param dst Destination of where to store result. Shall be 128 bits. + * @param src1 First source. Shall be 128 bits. + * @param src2 Second source. Shall be 128 bits. + */ +static inline void mem_xor_128(uint8_t dst[16], const uint8_t src1[16], const uint8_t src2[16]) +{ + mem_xor_n(dst, src1, src2, 16); +} + #ifdef __cplusplus } #endif diff --git a/subsys/bluetooth/audio/csip_crypto.c b/subsys/bluetooth/audio/csip_crypto.c index d51855f02a58..d880ca36ce1d 100644 --- a/subsys/bluetooth/audio/csip_crypto.c +++ b/subsys/bluetooth/audio/csip_crypto.c @@ -17,6 +17,7 @@ #include #include #include +#include #include "common/bt_str.h" @@ -51,16 +52,6 @@ static int aes_cmac(const uint8_t key[BT_CSIP_CRYPTO_KEY_SIZE], return 0; } -static void xor_128(const uint8_t a[16], const uint8_t b[16], uint8_t out[16]) -{ - size_t len = 16; - /* TODO: Identical to the xor_128 from smp.c: Move to util */ - - while (len--) { - *out++ = *a++ ^ *b++; - } -} - int bt_csip_sih(const uint8_t sirk[BT_CSIP_SET_SIRK_SIZE], uint8_t r[BT_CSIP_CRYPTO_PRAND_SIZE], uint8_t out[BT_CSIP_CRYPTO_HASH_SIZE]) { @@ -229,7 +220,7 @@ int bt_csip_sef(const uint8_t k[BT_CSIP_CRYPTO_KEY_SIZE], sys_mem_swap(k1_out, sizeof(k1_out)); } - xor_128(k1_out, sirk, out_sirk); + mem_xor_128(out_sirk, k1_out, sirk); LOG_DBG("out %s", bt_hex(out_sirk, BT_CSIP_SET_SIRK_SIZE)); return 0; diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_iso.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_iso.c index 20e3fe528e1c..f5e566a7bccb 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_iso.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_iso.c @@ -9,6 +9,7 @@ #include #include +#include #include "hal/cpu.h" #include "hal/ccm.h" diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync_iso.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync_iso.c index 12e9fd342b97..d4611425f2c2 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync_iso.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync_iso.c @@ -9,6 +9,7 @@ #include #include +#include #include "hal/cpu.h" #include "hal/ccm.h" diff --git a/subsys/bluetooth/controller/ll_sw/ull_conn_iso.c b/subsys/bluetooth/controller/ll_sw/ull_conn_iso.c index 2ec6a73b4a4b..467460d2f30b 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_conn_iso.c +++ b/subsys/bluetooth/controller/ll_sw/ull_conn_iso.c @@ -6,6 +6,7 @@ #include #include +#include #include #include "util/util.h" diff --git a/subsys/bluetooth/controller/util/mem.c b/subsys/bluetooth/controller/util/mem.c index 1bd67ee7eb19..e0450ac815bc 100644 --- a/subsys/bluetooth/controller/util/mem.c +++ b/subsys/bluetooth/controller/util/mem.c @@ -134,24 +134,6 @@ uint8_t mem_nz(uint8_t *src, uint16_t len) return 0; } -/** - * @brief XOR bytes - */ -inline void mem_xor_n(uint8_t *dst, uint8_t *src1, uint8_t *src2, uint16_t len) -{ - while (len--) { - *dst++ = *src1++ ^ *src2++; - } -} - -/** - * @brief XOR 32-bits - */ -void mem_xor_32(uint8_t *dst, uint8_t *src1, uint8_t *src2) -{ - mem_xor_n(dst, src1, src2, 4U); -} - /** * @brief Unit test */ diff --git a/subsys/bluetooth/controller/util/mem.h b/subsys/bluetooth/controller/util/mem.h index 8a8bdb3a62d7..4a345ab253f4 100644 --- a/subsys/bluetooth/controller/util/mem.h +++ b/subsys/bluetooth/controller/util/mem.h @@ -63,7 +63,5 @@ uint16_t mem_index_get(const void *mem, const void *mem_pool, uint16_t mem_size) void mem_rcopy(uint8_t *dst, uint8_t const *src, uint16_t len); uint8_t mem_nz(uint8_t *src, uint16_t len); -void mem_xor_n(uint8_t *dst, uint8_t *src1, uint8_t *src2, uint16_t len); -void mem_xor_32(uint8_t *dst, uint8_t *src1, uint8_t *src2); uint32_t mem_ut(void); diff --git a/subsys/bluetooth/host/smp.c b/subsys/bluetooth/host/smp.c index 2c110dc43a38..95809d237686 100644 --- a/subsys/bluetooth/host/smp.c +++ b/subsys/bluetooth/host/smp.c @@ -1829,15 +1829,6 @@ static uint8_t smp_send_pairing_random(struct bt_smp *smp) } #if !defined(CONFIG_BT_SMP_SC_PAIR_ONLY) -static void xor_128(const uint8_t p[16], const uint8_t q[16], uint8_t r[16]) -{ - size_t len = 16; - - while (len--) { - *r++ = *p++ ^ *q++; - } -} - static int smp_c1(const uint8_t k[16], const uint8_t r[16], const uint8_t preq[7], const uint8_t pres[7], const bt_addr_le_t *ia, const bt_addr_le_t *ra, @@ -1864,7 +1855,7 @@ static int smp_c1(const uint8_t k[16], const uint8_t r[16], /* c1 = e(k, e(k, r XOR p1) XOR p2) */ /* Using enc_data as temporary output buffer */ - xor_128(r, p1, enc_data); + mem_xor_128(enc_data, r, p1); err = bt_encrypt_le(k, enc_data, enc_data); if (err) { @@ -1878,7 +1869,7 @@ static int smp_c1(const uint8_t k[16], const uint8_t r[16], LOG_DBG("p2 %s", bt_hex(p2, 16)); - xor_128(enc_data, p2, enc_data); + mem_xor_128(enc_data, p2, enc_data); return bt_encrypt_le(k, enc_data, enc_data); } diff --git a/tests/unit/util/main.c b/tests/unit/util/main.c index c1dbca35bdcc..b9d1cdedf52a 100644 --- a/tests/unit/util/main.c +++ b/tests/unit/util/main.c @@ -618,4 +618,75 @@ ZTEST(util, test_IF_DISABLED) #undef test_IF_DISABLED_FLAG_B } +ZTEST(util, test_mem_xor_n) +{ + const size_t max_len = 128; + uint8_t expected_result[max_len]; + uint8_t src1[max_len]; + uint8_t src2[max_len]; + uint8_t dst[max_len]; + + memset(expected_result, 0, sizeof(expected_result)); + memset(src1, 0, sizeof(src1)); + memset(src2, 0, sizeof(src2)); + memset(dst, 0, sizeof(dst)); + + for (size_t i = 0U; i < max_len; i++) { + const size_t len = i; + + for (size_t j = 0U; j < len; j++) { + src1[j] = 0x33; + src2[j] = 0x0F; + expected_result[j] = 0x3C; + } + + mem_xor_n(dst, src1, src2, len); + zassert_mem_equal(expected_result, dst, len); + } +} + +ZTEST(util, test_mem_xor_32) +{ + uint8_t expected_result[4]; + uint8_t src1[4]; + uint8_t src2[4]; + uint8_t dst[4]; + + memset(expected_result, 0, sizeof(expected_result)); + memset(src1, 0, sizeof(src1)); + memset(src2, 0, sizeof(src2)); + memset(dst, 0, sizeof(dst)); + + for (size_t i = 0U; i < 4; i++) { + src1[i] = 0x43; + src2[i] = 0x0F; + expected_result[i] = 0x4C; + } + + mem_xor_32(dst, src1, src2); + zassert_mem_equal(expected_result, dst, 4); +} + +ZTEST(util, test_mem_xor_128) +{ + uint8_t expected_result[16]; + uint8_t src1[16]; + uint8_t src2[16]; + uint8_t dst[16]; + + memset(expected_result, 0, sizeof(expected_result)); + memset(src1, 0, sizeof(src1)); + memset(src2, 0, sizeof(src2)); + memset(dst, 0, sizeof(dst)); + + for (size_t i = 0U; i < 16; i++) { + src1[i] = 0x53; + src2[i] = 0x0F; + expected_result[i] = 0x5C; + } + + mem_xor_128(dst, src1, src2); + zassert_mem_equal(expected_result, dst, 16); +} + ZTEST_SUITE(util, NULL, NULL, NULL, NULL, NULL);