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