From 3c83c392c42d7129661cc667bdba58369e646016 Mon Sep 17 00:00:00 2001 From: Emil Gydesen Date: Wed, 3 Jan 2024 13:31:58 +0100 Subject: [PATCH] include: util: Add mem_xor functions Add functions to do XOR on arrays of memory, with one that takes arbitrary sizes and one for 32 bits and 128 bits as those are common sizes for this functionality. Signed-off-by: Emil Gydesen --- include/zephyr/sys/util.h | 39 ++++++++++ subsys/bluetooth/audio/csip_crypto.c | 13 +--- .../controller/ll_sw/nordic/lll/lll_adv_iso.c | 1 + .../ll_sw/nordic/lll/lll_sync_iso.c | 1 + .../bluetooth/controller/ll_sw/ull_conn_iso.c | 1 + subsys/bluetooth/controller/util/mem.c | 18 ----- subsys/bluetooth/controller/util/mem.h | 2 - subsys/bluetooth/host/smp.c | 13 +--- tests/unit/util/main.c | 71 +++++++++++++++++++ 9 files changed, 117 insertions(+), 42 deletions(-) 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);