1 /*
2 * Copyright (c) 2016 Nordic Semiconductor ASA
3 * Copyright (c) 2016 Vinayak Kariappa Chettimada
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7 #include <string.h>
8
9 #include <zephyr/arch/cpu.h>
10 #include <zephyr/types.h>
11 #include <zephyr/sys/byteorder.h>
12 #include <zephyr/drivers/entropy.h>
13
14 #include "util.h"
15 #include "util/memq.h"
16
17 #include "ll_sw/lll.h"
18
19 #include "ll_sw/pdu_df.h"
20 #include "lll/pdu_vendor.h"
21 #include "ll_sw/pdu.h"
22
23 /**
24 * @brief Population count: Count the number of bits set to 1
25 * @details
26 * TODO: Faster methods available at [1].
27 * [1] http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
28 *
29 * @param octets Data to count over
30 * @param octets_len Must not be bigger than 255/8 = 31 bytes
31 *
32 * @return popcnt of 'octets'
33 */
util_ones_count_get(const uint8_t * octets,uint8_t octets_len)34 uint8_t util_ones_count_get(const uint8_t *octets, uint8_t octets_len)
35 {
36 uint8_t one_count = 0U;
37
38 while (octets_len--) {
39 uint8_t bite;
40
41 bite = *octets;
42 while (bite) {
43 bite &= (bite - 1);
44 one_count++;
45 }
46 octets++;
47 }
48
49 return one_count;
50 }
51
52 /** @brief Prepare access address as per BT Spec.
53 *
54 * - It shall have no more than six consecutive zeros or ones.
55 * - It shall not be the advertising channel packets' Access Address.
56 * - It shall not be a sequence that differs from the advertising channel
57 * packets Access Address by only one bit.
58 * - It shall not have all four octets equal.
59 * - It shall have no more than 24 transitions.
60 * - It shall have a minimum of two transitions in the most significant six
61 * bits.
62 *
63 * LE Coded PHY requirements:
64 * - It shall have at least three ones in the least significant 8 bits.
65 * - It shall have no more than eleven transitions in the least significant 16
66 * bits.
67 */
util_aa_le32(uint8_t * dst)68 int util_aa_le32(uint8_t *dst)
69 {
70 #if defined(CONFIG_BT_CTLR_PHY_CODED)
71 uint8_t transitions_lsb16;
72 uint8_t ones_count_lsb8;
73 #endif /* CONFIG_BT_CTLR_PHY_CODED */
74 uint8_t consecutive_cnt;
75 uint8_t consecutive_bit;
76 uint32_t adv_aa_check;
77 uint32_t aa;
78 uint8_t transitions;
79 uint8_t bit_idx;
80 uint8_t retry;
81
82 retry = 3U;
83 again:
84 if (!retry) {
85 return -EFAULT;
86 }
87 retry--;
88
89 lll_csrand_get(dst, sizeof(uint32_t));
90 aa = sys_get_le32(dst);
91
92 bit_idx = 31U;
93 transitions = 0U;
94 consecutive_cnt = 1U;
95 #if defined(CONFIG_BT_CTLR_PHY_CODED)
96 ones_count_lsb8 = 0U;
97 transitions_lsb16 = 0U;
98 #endif /* CONFIG_BT_CTLR_PHY_CODED */
99 consecutive_bit = (aa >> bit_idx) & 0x01;
100 while (bit_idx--) {
101 #if defined(CONFIG_BT_CTLR_PHY_CODED)
102 uint8_t transitions_lsb16_prev = transitions_lsb16;
103 #endif /* CONFIG_BT_CTLR_PHY_CODED */
104 uint8_t consecutive_cnt_prev = consecutive_cnt;
105 uint8_t transitions_prev = transitions;
106 uint8_t bit;
107
108 bit = (aa >> bit_idx) & 0x01;
109 if (bit == consecutive_bit) {
110 consecutive_cnt++;
111 } else {
112 consecutive_cnt = 1U;
113 consecutive_bit = bit;
114 transitions++;
115
116 #if defined(CONFIG_BT_CTLR_PHY_CODED)
117 if (bit_idx < 15) {
118 transitions_lsb16++;
119 }
120 #endif /* CONFIG_BT_CTLR_PHY_CODED */
121 }
122
123 #if defined(CONFIG_BT_CTLR_PHY_CODED)
124 if ((bit_idx < 8) && consecutive_bit) {
125 ones_count_lsb8++;
126 }
127 #endif /* CONFIG_BT_CTLR_PHY_CODED */
128
129 /* It shall have no more than six consecutive zeros or ones. */
130 /* It shall have a minimum of two transitions in the most
131 * significant six bits.
132 */
133 if ((consecutive_cnt > 6) ||
134 #if defined(CONFIG_BT_CTLR_PHY_CODED)
135 (!consecutive_bit && (((bit_idx < 6) &&
136 (ones_count_lsb8 < 1)) ||
137 ((bit_idx < 5) &&
138 (ones_count_lsb8 < 2)) ||
139 ((bit_idx < 4) &&
140 (ones_count_lsb8 < 3)))) ||
141 #endif /* CONFIG_BT_CTLR_PHY_CODED */
142 ((consecutive_cnt < 6) &&
143 (((bit_idx < 29) && (transitions < 1)) ||
144 ((bit_idx < 28) && (transitions < 2))))) {
145 if (consecutive_bit) {
146 consecutive_bit = 0U;
147 aa &= ~BIT(bit_idx);
148 #if defined(CONFIG_BT_CTLR_PHY_CODED)
149 if (bit_idx < 8) {
150 ones_count_lsb8--;
151 }
152 #endif /* CONFIG_BT_CTLR_PHY_CODED */
153 } else {
154 consecutive_bit = 1U;
155 aa |= BIT(bit_idx);
156 #if defined(CONFIG_BT_CTLR_PHY_CODED)
157 if (bit_idx < 8) {
158 ones_count_lsb8++;
159 }
160 #endif /* CONFIG_BT_CTLR_PHY_CODED */
161 }
162
163 if (transitions != transitions_prev) {
164 consecutive_cnt = consecutive_cnt_prev;
165 transitions = transitions_prev;
166 } else {
167 consecutive_cnt = 1U;
168 transitions++;
169 }
170
171 #if defined(CONFIG_BT_CTLR_PHY_CODED)
172 if (bit_idx < 15) {
173 if (transitions_lsb16 !=
174 transitions_lsb16_prev) {
175 transitions_lsb16 =
176 transitions_lsb16_prev;
177 } else {
178 transitions_lsb16++;
179 }
180 }
181 #endif /* CONFIG_BT_CTLR_PHY_CODED */
182 }
183
184 /* It shall have no more than 24 transitions
185 * It shall have no more than eleven transitions in the least
186 * significant 16 bits.
187 */
188 if ((transitions > 24) ||
189 #if defined(CONFIG_BT_CTLR_PHY_CODED)
190 (transitions_lsb16 > 11) ||
191 #endif /* CONFIG_BT_CTLR_PHY_CODED */
192 0) {
193 if (consecutive_bit) {
194 aa &= ~(BIT(bit_idx + 1) - 1);
195 } else {
196 aa |= (BIT(bit_idx + 1) - 1);
197 }
198
199 break;
200 }
201 }
202
203 /* It shall not be the advertising channel packets Access Address.
204 * It shall not be a sequence that differs from the advertising channel
205 * packets Access Address by only one bit.
206 */
207 adv_aa_check = aa ^ PDU_AC_ACCESS_ADDR;
208 if (util_ones_count_get((uint8_t *)&adv_aa_check,
209 sizeof(adv_aa_check)) <= 1) {
210 goto again;
211 }
212
213 /* It shall not have all four octets equal. */
214 if (!((aa & 0xFFFF) ^ (aa >> 16)) &&
215 !((aa & 0xFF) ^ (aa >> 24))) {
216 goto again;
217 }
218
219 sys_put_le32(aa, dst);
220
221 return 0;
222 }
223
224 #if defined(CONFIG_BT_CTLR_ADV_ISO)
util_saa_le32(uint8_t * dst,uint8_t handle)225 int util_saa_le32(uint8_t *dst, uint8_t handle)
226 {
227 /* Refer to Bluetooth Core Specification Version 5.2 Vol 6, Part B,
228 * section 2.1.2 Access Address
229 */
230 uint32_t saa, saa_15, saa_16;
231 uint8_t bits;
232 int err;
233
234 /* Get access address */
235 err = util_aa_le32(dst);
236 if (err) {
237 return err;
238 }
239
240 saa = sys_get_le32(dst);
241
242 /* SAA_19 = SAA_15 */
243 saa_15 = (saa >> 15) & 0x01;
244 saa &= ~BIT(19);
245 saa |= saa_15 << 19;
246
247 /* SAA_16 != SAA_15 */
248 saa &= ~BIT(16);
249 saa_16 = ~saa_15 & 0x01;
250 saa |= saa_16 << 16;
251
252 /* SAA_22 = SAA_16 */
253 saa &= ~BIT(22);
254 saa |= saa_16 << 22;
255
256 /* SAA_25 = 0 */
257 saa &= ~BIT(25);
258
259 /* SAA_23 = 1 */
260 saa |= BIT(23);
261
262 /* For any pair of BIGs transmitted by the same device, the SAA 15-0
263 * values shall differ in at least two bits.
264 * - Find the number of bits required to support 3 times the maximum
265 * ISO connection handles supported
266 * - Clear those number many bits
267 * - Set the value that is 3 times the handle so that consecutive values
268 * differ in at least two bits.
269 */
270 bits = find_msb_set(CONFIG_BT_CTLR_ADV_ISO_SET * 0x03);
271 saa &= ~BIT_MASK(bits);
272 saa |= (handle * 0x03);
273
274 sys_put_le32(saa, dst);
275
276 return 0;
277 }
278 #endif /* CONFIG_BT_CTLR_ADV_ISO */
279
280 #if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_SYNC_ISO)
util_bis_aa_le32(uint8_t bis,uint8_t * saa,uint8_t * dst)281 void util_bis_aa_le32(uint8_t bis, uint8_t *saa, uint8_t *dst)
282 {
283 /* Refer to Bluetooth Core Specification Version 5.2 Vol 6, Part B,
284 * section 2.1.2 Access Address
285 */
286 uint8_t dwh[2]; /* Holds the two most significant bytes of DW */
287 uint8_t d;
288
289 /* 8-bits for d is enough due to wrapping math and requirement to do
290 * modulus 128.
291 */
292 d = ((35 * bis) + 42) & 0x7f;
293
294 /* Most significant 6 bits of DW are bit extension of least significant
295 * bit of D.
296 */
297 if (d & 1) {
298 dwh[1] = 0xFC;
299 } else {
300 dwh[1] = 0;
301 }
302
303 /* Set the bits 25 to 17 of DW */
304 dwh[1] |= (d & 0x02) | ((d >> 6) & 0x01);
305 dwh[0] = ((d & 0x02) << 6) | (d & 0x30) | ((d & 0x0C) >> 1);
306
307 /* Most significant 16-bits of SAA XOR DW, least significant 16-bit are
308 * zeroes, needing no operation on them.
309 */
310 memcpy(dst, saa, sizeof(uint32_t));
311 dst[3] ^= dwh[1];
312 dst[2] ^= dwh[0];
313 }
314 #endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_SYNC_ISO*/
315
316 /** @brief Get a bit aligned value from a byte array
317 * Converts bitsets to any size variable (<= 32 bit), which is returned
318 * as a uint32_t value.
319 *
320 * @param data Pointer to bytes containing the requested value
321 * @param bit_offs Bit offset into data[0] for value LSB
322 * @param num_bits Number of bits to extract and convert to value
323 */
util_get_bits(uint8_t * data,uint8_t bit_offs,uint8_t num_bits)324 uint32_t util_get_bits(uint8_t *data, uint8_t bit_offs, uint8_t num_bits)
325 {
326 uint32_t value;
327 uint8_t shift, byteIdx, bits;
328
329 value = 0;
330 shift = 0;
331 byteIdx = 0;
332
333 while (num_bits) {
334 bits = MIN(num_bits, 8 - bit_offs);
335 value |= ((data[byteIdx] >> bit_offs) & BIT_MASK(bits)) << shift;
336 shift += bits;
337 num_bits -= bits;
338 bit_offs = 0;
339 byteIdx++;
340 }
341
342 return value;
343 }
344
345 /** @brief Set a bit aligned value in a byte array
346 * Converts a value up to 32 bits to a bitset in a byte array.
347 *
348 * @param data Pointer to bytes in which to place the value
349 * @param bit_offs Bit offset into data[0] for value LSB
350 * @param num_bits Number of bits to set in data
351 */
util_set_bits(uint8_t * data,uint8_t bit_offs,uint8_t num_bits,uint32_t value)352 void util_set_bits(uint8_t *data, uint8_t bit_offs, uint8_t num_bits,
353 uint32_t value)
354 {
355 uint8_t byteIdx, bits;
356
357 byteIdx = 0;
358
359 while (num_bits) {
360 bits = MIN(num_bits, 8 - bit_offs);
361 data[byteIdx] = (data[byteIdx] & ~(BIT_MASK(bits) << bit_offs)) |
362 ((value & BIT_MASK(bits)) << bit_offs);
363 value >>= bits;
364 num_bits -= bits;
365 bit_offs = 0;
366 byteIdx++;
367 }
368 }
369
