1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one
3 * or more contributor license agreements. See the NOTICE file
4 * distributed with this work for additional information
5 * regarding copyright ownership. The ASF licenses this file
6 * to you under the Apache License, Version 2.0 (the
7 * "License"); you may not use this file except in compliance
8 * with the License. You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing,
13 * software distributed under the License is distributed on an
14 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
15 * KIND, either express or implied. See the License for the
16 * specific language governing permissions and limitations
17 * under the License.
18 */
19
20 #include <string.h>
21 #include <errno.h>
22 #include "securec.h"
23 #include "nimble/ble.h"
24 #include "ble_hs_priv.h"
25 #include "host/ble_hs_adv.h"
26
27 struct find_field_data {
28 uint8_t type;
29 const struct ble_hs_adv_field *field;
30 };
31
32 static ble_uuid16_t ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2];
33 static ble_uuid32_t ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4];
34 static ble_uuid128_t ble_hs_adv_uuids128[BLE_HS_ADV_MAX_FIELD_SZ / 16];
35
ble_hs_adv_set_hdr(uint8_t type,uint8_t data_len,uint8_t max_len,uint8_t * dst,uint8_t * dst_len,struct os_mbuf * om)36 static int ble_hs_adv_set_hdr(uint8_t type, uint8_t data_len, uint8_t max_len,
37 uint8_t *dst, uint8_t *dst_len, struct os_mbuf *om)
38 {
39 uint8_t data_len_tmp = data_len;
40 if (om) {
41 data_len_tmp++;
42 int rc = os_mbuf_append(om, &data_len_tmp, sizeof(data_len_tmp));
43 if (rc) {
44 return rc;
45 }
46
47 return os_mbuf_append(om, &type, sizeof(type));
48 }
49
50 if (*dst_len + 2 + data_len_tmp > max_len) { // 2:byte alignment
51 return BLE_HS_EMSGSIZE;
52 }
53
54 dst[*dst_len] = data_len_tmp + 1;
55 dst[*dst_len + 1] = type;
56 *dst_len += 2; // 2:byte alignment
57 return 0;
58 }
59
ble_hs_adv_set_flat_mbuf(uint8_t type,int data_len,const void * data,uint8_t * dst,uint8_t * dst_len,uint8_t max_len,struct os_mbuf * om)60 static int ble_hs_adv_set_flat_mbuf(uint8_t type, int data_len, const void *data,
61 uint8_t *dst, uint8_t *dst_len, uint8_t max_len,
62 struct os_mbuf *om)
63 {
64 int rc;
65 BLE_HS_DBG_ASSERT(data_len > 0);
66 rc = ble_hs_adv_set_hdr(type, data_len, max_len, dst, dst_len, om);
67 if (rc != 0) {
68 return rc;
69 }
70
71 if (om) {
72 return os_mbuf_append(om, data, data_len);
73 }
74
75 memcpy_s(dst + *dst_len, sizeof(dst + *dst_len), data, data_len);
76 *dst_len += data_len;
77 return 0;
78 }
79
ble_hs_adv_set_flat(uint8_t type,int data_len,const void * data,uint8_t * dst,uint8_t * dst_len,uint8_t max_len)80 int ble_hs_adv_set_flat(uint8_t type, int data_len, const void *data,
81 uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
82 {
83 #if !NIMBLE_BLE_ADVERTISE
84 return BLE_HS_ENOTSUP;
85 #endif
86 return ble_hs_adv_set_flat_mbuf(type, data_len, data, dst, dst_len, max_len,
87 NULL);
88 }
89
ble_hs_adv_set_array_uuid16(uint8_t type,uint8_t num_elems,const ble_uuid16_t * elems,uint8_t * dst,uint8_t * dst_len,uint8_t max_len,struct os_mbuf * om)90 static int ble_hs_adv_set_array_uuid16(uint8_t type, uint8_t num_elems,
91 const ble_uuid16_t *elems, uint8_t *dst,
92 uint8_t *dst_len, uint8_t max_len,
93 struct os_mbuf *om)
94 {
95 int rc;
96 int i;
97 rc = ble_hs_adv_set_hdr(type, num_elems * 2, max_len, dst, // 2:byte alignment
98 dst_len, om);
99 if (rc != 0) {
100 return rc;
101 }
102
103 for (i = 0; i < num_elems; i++) {
104 if (om) {
105 rc = ble_uuid_to_mbuf(&elems[i].u, om);
106 if (rc) {
107 return rc;
108 }
109 } else {
110 ble_uuid_flat(&elems[i].u, dst + *dst_len);
111 *dst_len += 2; // 2:byte alignment
112 }
113 }
114
115 return 0;
116 }
117
ble_hs_adv_set_array_uuid32(uint8_t type,uint8_t num_elems,const ble_uuid32_t * elems,uint8_t * dst,uint8_t * dst_len,uint8_t max_len,struct os_mbuf * om)118 static int ble_hs_adv_set_array_uuid32(uint8_t type, uint8_t num_elems,
119 const ble_uuid32_t *elems, uint8_t *dst,
120 uint8_t *dst_len, uint8_t max_len,
121 struct os_mbuf *om)
122 {
123 uint32_t uuid_le;
124 int rc;
125 int i;
126 rc = ble_hs_adv_set_hdr(type, num_elems * 4, max_len, dst, // 4:byte alignment
127 dst_len, om);
128 if (rc != 0) {
129 return rc;
130 }
131
132 for (i = 0; i < num_elems; i++) {
133 /* We cannot use ble_uuid_flat here since it converts 32-bit UUIDs to
134 * 128-bit as ATT requires. In AD, 32-bit UUID shall be written as an
135 * actual 32-bit value.
136 */
137 if (om) {
138 uuid_le = htole32(elems[i].value);
139 rc = os_mbuf_append(om, &uuid_le, sizeof(uuid_le));
140 if (rc) {
141 return rc;
142 }
143 } else {
144 put_le32(dst + *dst_len, elems[i].value);
145 *dst_len += 4; // 4:byte alignment
146 }
147 }
148
149 return 0;
150 }
151
ble_hs_adv_set_array_uuid128(uint8_t type,uint8_t num_elems,const ble_uuid128_t * elems,uint8_t * dst,uint8_t * dst_len,uint8_t max_len,struct os_mbuf * om)152 static int ble_hs_adv_set_array_uuid128(uint8_t type, uint8_t num_elems,
153 const ble_uuid128_t *elems, uint8_t *dst,
154 uint8_t *dst_len, uint8_t max_len,
155 struct os_mbuf *om)
156 {
157 int rc;
158 int i;
159 rc = ble_hs_adv_set_hdr(type, num_elems * 16, max_len, dst, // 16:byte alignment
160 dst_len, om);
161 if (rc != 0) {
162 return rc;
163 }
164
165 for (i = 0; i < num_elems; i++) {
166 if (om) {
167 rc = ble_uuid_to_mbuf(&elems[i].u, om);
168 if (rc) {
169 return rc;
170 }
171 } else {
172 ble_uuid_flat(&elems[i].u, dst + *dst_len);
173 *dst_len += 16; // 16:byte alignment
174 }
175 }
176
177 return 0;
178 }
179
ble_hs_adv_set_array16(uint8_t type,uint8_t num_elems,const uint16_t * elems,uint8_t * dst,uint8_t * dst_len,uint8_t max_len,struct os_mbuf * om)180 static int ble_hs_adv_set_array16(uint8_t type, uint8_t num_elems, const uint16_t *elems,
181 uint8_t *dst, uint8_t *dst_len, uint8_t max_len,
182 struct os_mbuf *om)
183 {
184 uint16_t tmp;
185 int rc;
186 int i;
187 rc = ble_hs_adv_set_hdr(type, num_elems * sizeof * elems, max_len, dst,
188 dst_len, om);
189 if (rc != 0) {
190 return rc;
191 }
192
193 for (i = 0; i < num_elems; i++) {
194 if (om) {
195 tmp = htole16(elems[i]);
196 rc = os_mbuf_append(om, &tmp, sizeof(tmp));
197 if (rc) {
198 return rc;
199 }
200 } else {
201 put_le16(dst + *dst_len, elems[i]);
202 *dst_len += sizeof elems[i];
203 }
204 }
205
206 return 0;
207 }
208
adv_set_fields(const struct ble_hs_adv_fields * adv_fields,uint8_t * dst,uint8_t * dst_len,uint8_t max_len,struct os_mbuf * om)209 static int adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
210 uint8_t *dst, uint8_t *dst_len, uint8_t max_len,
211 struct os_mbuf *om)
212 {
213 #if !NIMBLE_BLE_ADVERTISE
214 return BLE_HS_ENOTSUP;
215 #endif
216 uint8_t type;
217 int8_t tx_pwr_lvl;
218 uint8_t dst_len_local;
219 int rc;
220 dst_len_local = 0;
221
222 /*** 0x01 - Flags. */
223 /* The application has two options concerning the flags field:
224 * 1. Don't include it in advertisements (flags == 0).
225 * 2. Explicitly specify the value (flags != 0).
226 *
227 * Note: The CSS prohibits advertising a flags value of 0, so this method
228 * of specifying option 1 vs. 2 is sound.
229 */
230 if (adv_fields->flags != 0) {
231 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_FLAGS, 1,
232 &adv_fields->flags, dst, &dst_len_local,
233 max_len, om);
234 if (rc != 0) {
235 return rc;
236 }
237 }
238
239 /*** 0x02,0x03 - 16-bit service class UUIDs. */
240 if (adv_fields->uuids16 != NULL && adv_fields->num_uuids16) {
241 if (adv_fields->uuids16_is_complete) {
242 type = BLE_HS_ADV_TYPE_COMP_UUIDS16;
243 } else {
244 type = BLE_HS_ADV_TYPE_INCOMP_UUIDS16;
245 }
246
247 rc = ble_hs_adv_set_array_uuid16(type, adv_fields->num_uuids16,
248 adv_fields->uuids16, dst, &dst_len_local,
249 max_len, om);
250 if (rc != 0) {
251 return rc;
252 }
253 }
254
255 /*** 0x04,0x05 - 32-bit service class UUIDs. */
256 if (adv_fields->uuids32 != NULL && adv_fields->num_uuids32) {
257 if (adv_fields->uuids32_is_complete) {
258 type = BLE_HS_ADV_TYPE_COMP_UUIDS32;
259 } else {
260 type = BLE_HS_ADV_TYPE_INCOMP_UUIDS32;
261 }
262
263 rc = ble_hs_adv_set_array_uuid32(type, adv_fields->num_uuids32,
264 adv_fields->uuids32, dst, &dst_len_local,
265 max_len, om);
266 if (rc != 0) {
267 return rc;
268 }
269 }
270
271 /*** 0x06,0x07 - 128-bit service class UUIDs. */
272 if (adv_fields->uuids128 != NULL && adv_fields->num_uuids128 > 0) {
273 if (adv_fields->uuids128_is_complete) {
274 type = BLE_HS_ADV_TYPE_COMP_UUIDS128;
275 } else {
276 type = BLE_HS_ADV_TYPE_INCOMP_UUIDS128;
277 }
278
279 rc = ble_hs_adv_set_array_uuid128(type, adv_fields->num_uuids128,
280 adv_fields->uuids128, dst, &dst_len_local,
281 max_len, om);
282 if (rc != 0) {
283 return rc;
284 }
285 }
286
287 /*** 0x08,0x09 - Local name. */
288 if (adv_fields->name != NULL && adv_fields->name_len > 0) {
289 if (adv_fields->name_is_complete) {
290 type = BLE_HS_ADV_TYPE_COMP_NAME;
291 } else {
292 type = BLE_HS_ADV_TYPE_INCOMP_NAME;
293 }
294
295 rc = ble_hs_adv_set_flat_mbuf(type, adv_fields->name_len,
296 adv_fields->name, dst, &dst_len_local, max_len,
297 om);
298 if (rc != 0) {
299 return rc;
300 }
301 }
302
303 /*** 0x0a - Tx power level. */
304 if (adv_fields->tx_pwr_lvl_is_present) {
305 /* Read the power level from the controller if requested; otherwise use
306 * the explicitly specified value.
307 */
308 if (adv_fields->tx_pwr_lvl == BLE_HS_ADV_TX_PWR_LVL_AUTO) {
309 rc = ble_hs_hci_util_read_adv_tx_pwr(&tx_pwr_lvl);
310 if (rc != 0) {
311 return rc;
312 }
313 } else {
314 tx_pwr_lvl = adv_fields->tx_pwr_lvl;
315 }
316
317 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_TX_PWR_LVL, 1,
318 &tx_pwr_lvl, dst, &dst_len_local, max_len, om);
319 if (rc != 0) {
320 return rc;
321 }
322 }
323
324 /*** 0x12 - Slave connection interval range. */
325 if (adv_fields->slave_itvl_range != NULL) {
326 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE,
327 BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN,
328 adv_fields->slave_itvl_range, dst,
329 &dst_len_local, max_len, om);
330 if (rc != 0) {
331 return rc;
332 }
333 }
334
335 /*** 0x16 - Service data - 16-bit UUID. */
336 if (adv_fields->svc_data_uuid16 != NULL && adv_fields->svc_data_uuid16_len) {
337 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID16,
338 adv_fields->svc_data_uuid16_len,
339 adv_fields->svc_data_uuid16, dst, &dst_len_local,
340 max_len, om);
341 if (rc != 0) {
342 return rc;
343 }
344 }
345
346 /*** 0x17 - Public target address. */
347 if (adv_fields->public_tgt_addr != NULL &&
348 adv_fields->num_public_tgt_addrs != 0) {
349 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR,
350 BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN *
351 adv_fields->num_public_tgt_addrs,
352 adv_fields->public_tgt_addr, dst, &dst_len_local,
353 max_len, om);
354 if (rc != 0) {
355 return rc;
356 }
357 }
358
359 /*** 0x19 - Appearance. */
360 if (adv_fields->appearance_is_present) {
361 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_APPEARANCE,
362 BLE_HS_ADV_APPEARANCE_LEN,
363 &adv_fields->appearance, dst, &dst_len_local,
364 max_len, om);
365 if (rc != 0) {
366 return rc;
367 }
368 }
369
370 /*** 0x1a - Advertising interval. */
371 if (adv_fields->adv_itvl_is_present) {
372 rc = ble_hs_adv_set_array16(BLE_HS_ADV_TYPE_ADV_ITVL, 1,
373 &adv_fields->adv_itvl, dst, &dst_len_local,
374 max_len, om);
375 if (rc != 0) {
376 return rc;
377 }
378 }
379
380 /*** 0x20 - Service data - 32-bit UUID. */
381 if (adv_fields->svc_data_uuid32 != NULL && adv_fields->svc_data_uuid32_len) {
382 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID32,
383 adv_fields->svc_data_uuid32_len,
384 adv_fields->svc_data_uuid32, dst, &dst_len_local,
385 max_len, om);
386 if (rc != 0) {
387 return rc;
388 }
389 }
390
391 /*** 0x21 - Service data - 128-bit UUID. */
392 if (adv_fields->svc_data_uuid128 != NULL && adv_fields->svc_data_uuid128_len) {
393 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_SVC_DATA_UUID128,
394 adv_fields->svc_data_uuid128_len,
395 adv_fields->svc_data_uuid128, dst,
396 &dst_len_local, max_len, om);
397 if (rc != 0) {
398 return rc;
399 }
400 }
401
402 /*** 0x24 - URI. */
403 if (adv_fields->uri != NULL && adv_fields->uri_len) {
404 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_URI, adv_fields->uri_len,
405 adv_fields->uri, dst, &dst_len_local, max_len,
406 om);
407 if (rc != 0) {
408 return rc;
409 }
410 }
411
412 /*** 0xff - Manufacturer specific data. */
413 if ((adv_fields->mfg_data != NULL) && (adv_fields->mfg_data_len >= 2)) { // 2:byte alignment
414 rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_MFG_DATA,
415 adv_fields->mfg_data_len,
416 adv_fields->mfg_data,
417 dst, &dst_len_local, max_len, om);
418 if (rc != 0) {
419 return rc;
420 }
421 }
422
423 if (dst_len) {
424 *dst_len = dst_len_local;
425 }
426
427 return 0;
428 }
429
430 /**
431 * Converts a high-level set of fields to a byte buffer.
432 *
433 * @return 0 on success; nonzero on failure.
434 */
ble_hs_adv_set_fields(const struct ble_hs_adv_fields * adv_fields,uint8_t * dst,uint8_t * dst_len,uint8_t max_len)435 int ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
436 uint8_t *dst, uint8_t *dst_len, uint8_t max_len)
437 {
438 #if !NIMBLE_BLE_ADVERTISE
439 return BLE_HS_ENOTSUP;
440 #endif
441 return adv_set_fields(adv_fields, dst, dst_len, max_len, NULL);
442 }
443
ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields * adv_fields,struct os_mbuf * om)444 int ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields,
445 struct os_mbuf *om)
446 {
447 #if !NIMBLE_BLE_ADVERTISE
448 return BLE_HS_ENOTSUP;
449 #endif
450 return adv_set_fields(adv_fields, NULL, NULL, 0, om);
451 }
452
ble_hs_adv_parse_uuids16(struct ble_hs_adv_fields * adv_fields,const uint8_t * data,uint8_t data_len)453 static int ble_hs_adv_parse_uuids16(struct ble_hs_adv_fields *adv_fields,
454 const uint8_t *data, uint8_t data_len)
455 {
456 ble_uuid_any_t uuid;
457 int i;
458
459 if (data_len % 2 != 0) { // 2:byte alignment
460 return BLE_HS_EBADDATA;
461 }
462
463 adv_fields->uuids16 = ble_hs_adv_uuids16;
464 adv_fields->num_uuids16 = data_len / 2; // 2:byte alignment
465
466 for (i = 0; i < adv_fields->num_uuids16; i++) {
467 ble_uuid_init_from_buf(&uuid, data + i * 2, 2); // 2:byte alignment
468 ble_hs_adv_uuids16[i] = uuid.u16;
469 }
470
471 return 0;
472 }
473
ble_hs_adv_parse_uuids32(struct ble_hs_adv_fields * adv_fields,const uint8_t * data,uint8_t data_len)474 static int ble_hs_adv_parse_uuids32(struct ble_hs_adv_fields *adv_fields,
475 const uint8_t *data, uint8_t data_len)
476 {
477 ble_uuid_any_t uuid;
478 int i;
479
480 if (data_len % 4 != 0) { // 4:byte alignment
481 return BLE_HS_EBADDATA;
482 }
483
484 adv_fields->uuids32 = ble_hs_adv_uuids32;
485 adv_fields->num_uuids32 = data_len / 4; // 4:byte alignment
486
487 for (i = 0; i < adv_fields->num_uuids32; i++) {
488 ble_uuid_init_from_buf(&uuid, data + i * 4, 4); // 4:byte alignment
489 ble_hs_adv_uuids32[i] = uuid.u32;
490 }
491
492 return 0;
493 }
494
ble_hs_adv_parse_uuids128(struct ble_hs_adv_fields * adv_fields,const uint8_t * data,uint8_t data_len)495 static int ble_hs_adv_parse_uuids128(struct ble_hs_adv_fields *adv_fields,
496 const uint8_t *data, uint8_t data_len)
497 {
498 ble_uuid_any_t uuid;
499 int i;
500
501 if (data_len % 16 != 0) { // 16:byte alignment
502 return BLE_HS_EBADDATA;
503 }
504
505 adv_fields->uuids128 = ble_hs_adv_uuids128;
506 adv_fields->num_uuids128 = data_len / 16; // 16:byte alignment
507
508 for (i = 0; i < adv_fields->num_uuids128; i++) {
509 ble_uuid_init_from_buf(&uuid, data + i * 16, 16); // 16:byte alignment
510 ble_hs_adv_uuids128[i] = uuid.u128;
511 }
512
513 return 0;
514 }
515
ble_hs_adv_parse_one_field(struct ble_hs_adv_fields * adv_fields,uint8_t * total_len,const uint8_t * src,uint8_t src_len)516 static int ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields,
517 uint8_t *total_len, const uint8_t *src,
518 uint8_t src_len)
519 {
520 uint8_t data_len;
521 uint8_t type;
522 const uint8_t *data;
523 int rc;
524
525 if (src_len < 1) {
526 return BLE_HS_EMSGSIZE;
527 }
528
529 *total_len = src[0] + 1;
530
531 if (src_len < *total_len) {
532 return BLE_HS_EMSGSIZE;
533 }
534
535 type = src[1];
536 data = src + 2; // 2:byte alignment
537 data_len = *total_len - 2; // 2:byte alignment
538
539 if (data_len > BLE_HS_ADV_MAX_FIELD_SZ) {
540 return BLE_HS_EBADDATA;
541 }
542
543 switch (type) {
544 case BLE_HS_ADV_TYPE_FLAGS:
545 if (data_len != BLE_HS_ADV_FLAGS_LEN) {
546 return BLE_HS_EBADDATA;
547 }
548
549 adv_fields->flags = *data;
550 break;
551
552 case BLE_HS_ADV_TYPE_INCOMP_UUIDS16:
553 rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len);
554 if (rc != 0) {
555 return rc;
556 }
557
558 adv_fields->uuids16_is_complete = 0;
559 break;
560
561 case BLE_HS_ADV_TYPE_COMP_UUIDS16:
562 rc = ble_hs_adv_parse_uuids16(adv_fields, data, data_len);
563 if (rc != 0) {
564 return rc;
565 }
566
567 adv_fields->uuids16_is_complete = 1;
568 break;
569
570 case BLE_HS_ADV_TYPE_INCOMP_UUIDS32:
571 rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len);
572 if (rc != 0) {
573 return rc;
574 }
575
576 adv_fields->uuids32_is_complete = 0;
577 break;
578
579 case BLE_HS_ADV_TYPE_COMP_UUIDS32:
580 rc = ble_hs_adv_parse_uuids32(adv_fields, data, data_len);
581 if (rc != 0) {
582 return rc;
583 }
584
585 adv_fields->uuids32_is_complete = 1;
586 break;
587
588 case BLE_HS_ADV_TYPE_INCOMP_UUIDS128:
589 rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len);
590 if (rc != 0) {
591 return rc;
592 }
593
594 adv_fields->uuids128_is_complete = 0;
595 break;
596
597 case BLE_HS_ADV_TYPE_COMP_UUIDS128:
598 rc = ble_hs_adv_parse_uuids128(adv_fields, data, data_len);
599 if (rc != 0) {
600 return rc;
601 }
602
603 adv_fields->uuids128_is_complete = 1;
604 break;
605
606 case BLE_HS_ADV_TYPE_INCOMP_NAME:
607 adv_fields->name = data;
608 adv_fields->name_len = data_len;
609 adv_fields->name_is_complete = 0;
610 break;
611
612 case BLE_HS_ADV_TYPE_COMP_NAME:
613 adv_fields->name = data;
614 adv_fields->name_len = data_len;
615 adv_fields->name_is_complete = 1;
616 break;
617
618 case BLE_HS_ADV_TYPE_TX_PWR_LVL:
619 if (data_len != BLE_HS_ADV_TX_PWR_LVL_LEN) {
620 return BLE_HS_EBADDATA;
621 }
622
623 adv_fields->tx_pwr_lvl = *data;
624 adv_fields->tx_pwr_lvl_is_present = 1;
625 break;
626
627 case BLE_HS_ADV_TYPE_SLAVE_ITVL_RANGE:
628 if (data_len != BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN) {
629 return BLE_HS_EBADDATA;
630 }
631
632 adv_fields->slave_itvl_range = data;
633 break;
634
635 case BLE_HS_ADV_TYPE_SVC_DATA_UUID16:
636 if (data_len < BLE_HS_ADV_SVC_DATA_UUID16_MIN_LEN) {
637 return BLE_HS_EBADDATA;
638 }
639
640 adv_fields->svc_data_uuid16 = data;
641 adv_fields->svc_data_uuid16_len = data_len;
642 break;
643
644 case BLE_HS_ADV_TYPE_PUBLIC_TGT_ADDR:
645 if (data_len % BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN != 0) {
646 return BLE_HS_EBADDATA;
647 }
648
649 adv_fields->public_tgt_addr = data;
650 adv_fields->num_public_tgt_addrs =
651 data_len / BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN;
652 break;
653
654 case BLE_HS_ADV_TYPE_APPEARANCE:
655 if (data_len != BLE_HS_ADV_APPEARANCE_LEN) {
656 return BLE_HS_EBADDATA;
657 }
658
659 adv_fields->appearance = get_le16(data);
660 adv_fields->appearance_is_present = 1;
661 break;
662
663 case BLE_HS_ADV_TYPE_ADV_ITVL:
664 if (data_len != BLE_HS_ADV_ADV_ITVL_LEN) {
665 return BLE_HS_EBADDATA;
666 }
667
668 adv_fields->adv_itvl = get_le16(data);
669 adv_fields->adv_itvl_is_present = 1;
670 break;
671
672 case BLE_HS_ADV_TYPE_SVC_DATA_UUID32:
673 if (data_len < BLE_HS_ADV_SVC_DATA_UUID32_MIN_LEN) {
674 return BLE_HS_EBADDATA;
675 }
676
677 adv_fields->svc_data_uuid32 = data;
678 adv_fields->svc_data_uuid32_len = data_len;
679 break;
680
681 case BLE_HS_ADV_TYPE_SVC_DATA_UUID128:
682 if (data_len < BLE_HS_ADV_SVC_DATA_UUID128_MIN_LEN) {
683 return BLE_HS_EBADDATA;
684 }
685
686 adv_fields->svc_data_uuid128 = data;
687 adv_fields->svc_data_uuid128_len = data_len;
688 break;
689
690 case BLE_HS_ADV_TYPE_URI:
691 adv_fields->uri = data;
692 adv_fields->uri_len = data_len;
693 break;
694
695 case BLE_HS_ADV_TYPE_MFG_DATA:
696 adv_fields->mfg_data = data;
697 adv_fields->mfg_data_len = data_len;
698 break;
699
700 default:
701 break;
702 }
703
704 return 0;
705 }
706
ble_hs_adv_parse_fields(struct ble_hs_adv_fields * adv_fields,const uint8_t * src,uint8_t src_len)707 int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields,
708 const uint8_t *src, uint8_t src_len)
709 {
710 uint8_t field_len;
711 const uint8_t *src_tmp = src;
712 uint8_t src_len_tmp = src_len;
713 memset_s(adv_fields, sizeof * adv_fields, 0, sizeof * adv_fields);
714
715 while (src_len_tmp > 0) {
716 int rc = ble_hs_adv_parse_one_field(adv_fields, &field_len, src_tmp, src_len_tmp);
717 if (rc != 0) {
718 return rc;
719 }
720
721 src_tmp += field_len;
722 src_len_tmp -= field_len;
723 }
724
725 return 0;
726 }
727
ble_hs_adv_parse(const uint8_t * data,uint8_t length,ble_hs_adv_parse_func_t func,void * user_data)728 int ble_hs_adv_parse(const uint8_t *data, uint8_t length,
729 ble_hs_adv_parse_func_t func, void *user_data)
730 {
731 while (length > 1) {
732 const struct ble_hs_adv_field *field = (const void *) data;
733
734 if (field->length >= length) {
735 return BLE_HS_EBADDATA;
736 }
737
738 if (func(field, user_data) == 0) {
739 return 0;
740 }
741
742 length -= 1 + field->length;
743 data += 1 + field->length;
744 }
745
746 return 0;
747 }
748
find_field_func(const struct ble_hs_adv_field * field,void * user_data)749 static int find_field_func(const struct ble_hs_adv_field *field, void *user_data)
750 {
751 struct find_field_data *ffd = user_data;
752
753 if (field->type != ffd->type) {
754 return BLE_HS_EAGAIN;
755 }
756
757 ffd->field = field;
758 return 0;
759 }
760
ble_hs_adv_find_field(uint8_t type,const uint8_t * data,uint8_t length,const struct ble_hs_adv_field ** out)761 int ble_hs_adv_find_field(uint8_t type, const uint8_t *data, uint8_t length,
762 const struct ble_hs_adv_field **out)
763 {
764 int rc;
765 struct find_field_data ffd = {
766 .type = type,
767 .field = NULL,
768 };
769 rc = ble_hs_adv_parse(data, length, find_field_func, &ffd);
770 if (rc != 0) {
771 return rc;
772 }
773
774 if (!ffd.field) {
775 return BLE_HS_ENOENT;
776 }
777
778 *out = ffd.field;
779 return 0;
780 }