| 1 |
|
| 2 | |
| 3 | * BlueZ - Bluetooth protocol stack for Linux
|
| 4 | *
|
| 5 | * Copyright (C) 2021 Intel Corporation
|
| 6 |
|
| 7 |
|
| 8 | #include <linux/unaligned.h>
|
| 9 |
|
| 10 | void eir_create(struct hci_dev *hdev, u8 *data);
|
| 11 |
|
| 12 | u8 eir_create_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr, u8 size);
|
| 13 | u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr);
|
| 14 | u8 eir_create_per_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr);
|
| 15 |
|
| 16 | u8 eir_append_local_name(struct hci_dev *hdev, u8 *eir, u8 ad_len);
|
| 17 | u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len);
|
| 18 | u8 eir_append_service_data(u8 *eir, u16 eir_len, u16 uuid, u8 *data,
|
| 19 | u8 data_len);
|
| 20 |
|
| 21 | static inline u16 eir_precalc_len(u8 data_len)
|
| 22 | {
|
| 23 | return sizeof(u8) * 2 + data_len;
|
| 24 | }
|
| 25 |
|
| 26 | static inline u16 eir_append_data(u8 *eir, u16 eir_len, u8 type,
|
| 27 | u8 *data, u8 data_len)
|
| 28 | {
|
| 29 | eir[eir_len++] = sizeof(type) + data_len;
|
| 30 | eir[eir_len++] = type;
|
| 31 | memcpy(&eir[eir_len], data, data_len);
|
| 32 | eir_len += data_len;
|
| 33 |
|
| 34 | return eir_len;
|
| 35 | }
|
| 36 |
|
| 37 | static inline u16 eir_append_le16(u8 *eir, u16 eir_len, u8 type, u16 data)
|
| 38 | {
|
| 39 | eir[eir_len++] = sizeof(type) + sizeof(data);
|
| 40 | eir[eir_len++] = type;
|
| 41 | put_unaligned_le16(data, &eir[eir_len]);
|
| 42 | eir_len += sizeof(data);
|
| 43 |
|
| 44 | return eir_len;
|
| 45 | }
|
| 46 |
|
| 47 | static inline u16 eir_skb_put_data(struct sk_buff *skb, u8 type, u8 *data, u8 data_len)
|
| 48 | {
|
| 49 | u8 *eir;
|
| 50 | u16 eir_len;
|
| 51 |
|
| 52 | eir_len = eir_precalc_len(data_len);
|
| 53 | eir = skb_put(skb, eir_len);
|
| 54 | WARN_ON(sizeof(type) + data_len > U8_MAX);
|
| 55 | eir[0] = sizeof(type) + data_len;
|
| 56 | eir[1] = type;
|
| 57 | memcpy(&eir[2], data, data_len);
|
| 58 |
|
| 59 | return eir_len;
|
| 60 | }
|
| 61 |
|
| 62 | static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type,
|
| 63 | size_t *data_len)
|
| 64 | {
|
| 65 | size_t parsed = 0;
|
| 66 |
|
| 67 | if (eir_len < 2)
|
| 68 | return NULL;
|
| 69 |
|
| 70 | while (parsed < eir_len - 1) {
|
| 71 | u8 field_len = eir[0];
|
| 72 |
|
| 73 | if (field_len == 0)
|
| 74 | break;
|
| 75 |
|
| 76 | parsed += field_len + 1;
|
| 77 |
|
| 78 | if (parsed > eir_len)
|
| 79 | break;
|
| 80 |
|
| 81 | if (eir[1] != type) {
|
| 82 | eir += field_len + 1;
|
| 83 | continue;
|
| 84 | }
|
| 85 |
|
| 86 |
|
| 87 | if (field_len == 1)
|
| 88 | return NULL;
|
| 89 |
|
| 90 | if (data_len)
|
| 91 | *data_len = field_len - 1;
|
| 92 |
|
| 93 | return &eir[2];
|
| 94 | }
|
| 95 |
|
| 96 | return NULL;
|
| 97 | }
|
| 98 |
|
| 99 | void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len);
|