369 lines
8.9 KiB
C
369 lines
8.9 KiB
C
/*
|
|
* Copyright (c) 2016 Nordic Semiconductor ASA
|
|
* Copyright (c) 2016 Vinayak Kariappa Chettimada
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
#include <string.h>
|
|
|
|
#include <zephyr/arch/cpu.h>
|
|
#include <zephyr/types.h>
|
|
#include <zephyr/sys/byteorder.h>
|
|
#include <zephyr/drivers/entropy.h>
|
|
|
|
#include "util.h"
|
|
#include "util/memq.h"
|
|
|
|
#include "ll_sw/lll.h"
|
|
|
|
#include "ll_sw/pdu_df.h"
|
|
#include "lll/pdu_vendor.h"
|
|
#include "ll_sw/pdu.h"
|
|
|
|
/**
|
|
* @brief Population count: Count the number of bits set to 1
|
|
* @details
|
|
* TODO: Faster methods available at [1].
|
|
* [1] http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
|
|
*
|
|
* @param octets Data to count over
|
|
* @param octets_len Must not be bigger than 255/8 = 31 bytes
|
|
*
|
|
* @return popcnt of 'octets'
|
|
*/
|
|
uint8_t util_ones_count_get(const uint8_t *octets, uint8_t octets_len)
|
|
{
|
|
uint8_t one_count = 0U;
|
|
|
|
while (octets_len--) {
|
|
uint8_t bite;
|
|
|
|
bite = *octets;
|
|
while (bite) {
|
|
bite &= (bite - 1);
|
|
one_count++;
|
|
}
|
|
octets++;
|
|
}
|
|
|
|
return one_count;
|
|
}
|
|
|
|
/** @brief Prepare access address as per BT Spec.
|
|
*
|
|
* - It shall have no more than six consecutive zeros or ones.
|
|
* - It shall not be the advertising channel packets' Access Address.
|
|
* - It shall not be a sequence that differs from the advertising channel
|
|
* packets Access Address by only one bit.
|
|
* - It shall not have all four octets equal.
|
|
* - It shall have no more than 24 transitions.
|
|
* - It shall have a minimum of two transitions in the most significant six
|
|
* bits.
|
|
*
|
|
* LE Coded PHY requirements:
|
|
* - It shall have at least three ones in the least significant 8 bits.
|
|
* - It shall have no more than eleven transitions in the least significant 16
|
|
* bits.
|
|
*/
|
|
int util_aa_le32(uint8_t *dst)
|
|
{
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
uint8_t transitions_lsb16;
|
|
uint8_t ones_count_lsb8;
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
uint8_t consecutive_cnt;
|
|
uint8_t consecutive_bit;
|
|
uint32_t adv_aa_check;
|
|
uint32_t aa;
|
|
uint8_t transitions;
|
|
uint8_t bit_idx;
|
|
uint8_t retry;
|
|
|
|
retry = 3U;
|
|
again:
|
|
if (!retry) {
|
|
return -EFAULT;
|
|
}
|
|
retry--;
|
|
|
|
lll_csrand_get(dst, sizeof(uint32_t));
|
|
aa = sys_get_le32(dst);
|
|
|
|
bit_idx = 31U;
|
|
transitions = 0U;
|
|
consecutive_cnt = 1U;
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
ones_count_lsb8 = 0U;
|
|
transitions_lsb16 = 0U;
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
consecutive_bit = (aa >> bit_idx) & 0x01;
|
|
while (bit_idx--) {
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
uint8_t transitions_lsb16_prev = transitions_lsb16;
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
uint8_t consecutive_cnt_prev = consecutive_cnt;
|
|
uint8_t transitions_prev = transitions;
|
|
uint8_t bit;
|
|
|
|
bit = (aa >> bit_idx) & 0x01;
|
|
if (bit == consecutive_bit) {
|
|
consecutive_cnt++;
|
|
} else {
|
|
consecutive_cnt = 1U;
|
|
consecutive_bit = bit;
|
|
transitions++;
|
|
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
if (bit_idx < 15) {
|
|
transitions_lsb16++;
|
|
}
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
}
|
|
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
if ((bit_idx < 8) && consecutive_bit) {
|
|
ones_count_lsb8++;
|
|
}
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
|
|
/* It shall have no more than six consecutive zeros or ones. */
|
|
/* It shall have a minimum of two transitions in the most
|
|
* significant six bits.
|
|
*/
|
|
if ((consecutive_cnt > 6) ||
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
(!consecutive_bit && (((bit_idx < 6) &&
|
|
(ones_count_lsb8 < 1)) ||
|
|
((bit_idx < 5) &&
|
|
(ones_count_lsb8 < 2)) ||
|
|
((bit_idx < 4) &&
|
|
(ones_count_lsb8 < 3)))) ||
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
((consecutive_cnt < 6) &&
|
|
(((bit_idx < 29) && (transitions < 1)) ||
|
|
((bit_idx < 28) && (transitions < 2))))) {
|
|
if (consecutive_bit) {
|
|
consecutive_bit = 0U;
|
|
aa &= ~BIT(bit_idx);
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
if (bit_idx < 8) {
|
|
ones_count_lsb8--;
|
|
}
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
} else {
|
|
consecutive_bit = 1U;
|
|
aa |= BIT(bit_idx);
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
if (bit_idx < 8) {
|
|
ones_count_lsb8++;
|
|
}
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
}
|
|
|
|
if (transitions != transitions_prev) {
|
|
consecutive_cnt = consecutive_cnt_prev;
|
|
transitions = transitions_prev;
|
|
} else {
|
|
consecutive_cnt = 1U;
|
|
transitions++;
|
|
}
|
|
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
if (bit_idx < 15) {
|
|
if (transitions_lsb16 !=
|
|
transitions_lsb16_prev) {
|
|
transitions_lsb16 =
|
|
transitions_lsb16_prev;
|
|
} else {
|
|
transitions_lsb16++;
|
|
}
|
|
}
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
}
|
|
|
|
/* It shall have no more than 24 transitions
|
|
* It shall have no more than eleven transitions in the least
|
|
* significant 16 bits.
|
|
*/
|
|
if ((transitions > 24) ||
|
|
#if defined(CONFIG_BT_CTLR_PHY_CODED)
|
|
(transitions_lsb16 > 11) ||
|
|
#endif /* CONFIG_BT_CTLR_PHY_CODED */
|
|
0) {
|
|
if (consecutive_bit) {
|
|
aa &= ~(BIT(bit_idx + 1) - 1);
|
|
} else {
|
|
aa |= (BIT(bit_idx + 1) - 1);
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* It shall not be the advertising channel packets Access Address.
|
|
* It shall not be a sequence that differs from the advertising channel
|
|
* packets Access Address by only one bit.
|
|
*/
|
|
adv_aa_check = aa ^ PDU_AC_ACCESS_ADDR;
|
|
if (util_ones_count_get((uint8_t *)&adv_aa_check,
|
|
sizeof(adv_aa_check)) <= 1) {
|
|
goto again;
|
|
}
|
|
|
|
/* It shall not have all four octets equal. */
|
|
if (!((aa & 0xFFFF) ^ (aa >> 16)) &&
|
|
!((aa & 0xFF) ^ (aa >> 24))) {
|
|
goto again;
|
|
}
|
|
|
|
sys_put_le32(aa, dst);
|
|
|
|
return 0;
|
|
}
|
|
|
|
#if defined(CONFIG_BT_CTLR_ADV_ISO)
|
|
int util_saa_le32(uint8_t *dst, uint8_t handle)
|
|
{
|
|
/* Refer to Bluetooth Core Specification Version 5.2 Vol 6, Part B,
|
|
* section 2.1.2 Access Address
|
|
*/
|
|
uint32_t saa, saa_15, saa_16;
|
|
uint8_t bits;
|
|
int err;
|
|
|
|
/* Get access address */
|
|
err = util_aa_le32(dst);
|
|
if (err) {
|
|
return err;
|
|
}
|
|
|
|
saa = sys_get_le32(dst);
|
|
|
|
/* SAA_19 = SAA_15 */
|
|
saa_15 = (saa >> 15) & 0x01;
|
|
saa &= ~BIT(19);
|
|
saa |= saa_15 << 19;
|
|
|
|
/* SAA_16 != SAA_15 */
|
|
saa &= ~BIT(16);
|
|
saa_16 = ~saa_15 & 0x01;
|
|
saa |= saa_16 << 16;
|
|
|
|
/* SAA_22 = SAA_16 */
|
|
saa &= ~BIT(22);
|
|
saa |= saa_16 << 22;
|
|
|
|
/* SAA_25 = 0 */
|
|
saa &= ~BIT(25);
|
|
|
|
/* SAA_23 = 1 */
|
|
saa |= BIT(23);
|
|
|
|
/* For any pair of BIGs transmitted by the same device, the SAA 15-0
|
|
* values shall differ in at least two bits.
|
|
* - Find the number of bits required to support 3 times the maximum
|
|
* ISO connection handles supported
|
|
* - Clear those number many bits
|
|
* - Set the value that is 3 times the handle so that consecutive values
|
|
* differ in at least two bits.
|
|
*/
|
|
bits = find_msb_set(CONFIG_BT_CTLR_ADV_ISO_SET * 0x03);
|
|
saa &= ~BIT_MASK(bits);
|
|
saa |= (handle * 0x03);
|
|
|
|
sys_put_le32(saa, dst);
|
|
|
|
return 0;
|
|
}
|
|
#endif /* CONFIG_BT_CTLR_ADV_ISO */
|
|
|
|
#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_SYNC_ISO)
|
|
void util_bis_aa_le32(uint8_t bis, uint8_t *saa, uint8_t *dst)
|
|
{
|
|
/* Refer to Bluetooth Core Specification Version 5.2 Vol 6, Part B,
|
|
* section 2.1.2 Access Address
|
|
*/
|
|
uint8_t dwh[2]; /* Holds the two most significant bytes of DW */
|
|
uint8_t d;
|
|
|
|
/* 8-bits for d is enough due to wrapping math and requirement to do
|
|
* modulus 128.
|
|
*/
|
|
d = ((35 * bis) + 42) & 0x7f;
|
|
|
|
/* Most significant 6 bits of DW are bit extension of least significant
|
|
* bit of D.
|
|
*/
|
|
if (d & 1) {
|
|
dwh[1] = 0xFC;
|
|
} else {
|
|
dwh[1] = 0;
|
|
}
|
|
|
|
/* Set the bits 25 to 17 of DW */
|
|
dwh[1] |= (d & 0x02) | ((d >> 6) & 0x01);
|
|
dwh[0] = ((d & 0x02) << 6) | (d & 0x30) | ((d & 0x0C) >> 1);
|
|
|
|
/* Most significant 16-bits of SAA XOR DW, least significant 16-bit are
|
|
* zeroes, needing no operation on them.
|
|
*/
|
|
memcpy(dst, saa, sizeof(uint32_t));
|
|
dst[3] ^= dwh[1];
|
|
dst[2] ^= dwh[0];
|
|
}
|
|
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_SYNC_ISO*/
|
|
|
|
/** @brief Get a bit aligned value from a byte array
|
|
* Converts bitsets to any size variable (<= 32 bit), which is returned
|
|
* as a uint32_t value.
|
|
*
|
|
* @param data Pointer to bytes containing the requested value
|
|
* @param bit_offs Bit offset into data[0] for value LSB
|
|
* @param num_bits Number of bits to extract and convert to value
|
|
*/
|
|
uint32_t util_get_bits(uint8_t *data, uint8_t bit_offs, uint8_t num_bits)
|
|
{
|
|
uint32_t value;
|
|
uint8_t shift, byteIdx, bits;
|
|
|
|
value = 0;
|
|
shift = 0;
|
|
byteIdx = 0;
|
|
|
|
while (num_bits) {
|
|
bits = MIN(num_bits, 8 - bit_offs);
|
|
value |= ((data[byteIdx] >> bit_offs) & BIT_MASK(bits)) << shift;
|
|
shift += bits;
|
|
num_bits -= bits;
|
|
bit_offs = 0;
|
|
byteIdx++;
|
|
}
|
|
|
|
return value;
|
|
}
|
|
|
|
/** @brief Set a bit aligned value in a byte array
|
|
* Converts a value up to 32 bits to a bitset in a byte array.
|
|
*
|
|
* @param data Pointer to bytes in which to place the value
|
|
* @param bit_offs Bit offset into data[0] for value LSB
|
|
* @param num_bits Number of bits to set in data
|
|
*/
|
|
void util_set_bits(uint8_t *data, uint8_t bit_offs, uint8_t num_bits,
|
|
uint32_t value)
|
|
{
|
|
uint8_t byteIdx, bits;
|
|
|
|
byteIdx = 0;
|
|
|
|
while (num_bits) {
|
|
bits = MIN(num_bits, 8 - bit_offs);
|
|
data[byteIdx] = (data[byteIdx] & ~(BIT_MASK(bits) << bit_offs)) |
|
|
((value & BIT_MASK(bits)) << bit_offs);
|
|
value >>= bits;
|
|
num_bits -= bits;
|
|
bit_offs = 0;
|
|
byteIdx++;
|
|
}
|
|
}
|