Commit | Line | Data |
---|---|---|
01ce70b0 LAD |
1 | /* SPDX-License-Identifier: GPL-2.0 */ |
2 | /* | |
3 | * BlueZ - Bluetooth protocol stack for Linux | |
4 | * | |
5 | * Copyright (C) 2021 Intel Corporation | |
6 | */ | |
7 | ||
b6459415 JK |
8 | #include <asm/unaligned.h> |
9 | ||
01ce70b0 LAD |
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); | |
13 | u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr); | |
eca0ae4a | 14 | u8 eir_create_per_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr); |
01ce70b0 LAD |
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); | |
8f9ae5b3 LAD |
18 | u8 eir_append_service_data(u8 *eir, u16 eir_len, u16 uuid, u8 *data, |
19 | u8 data_len); | |
01ce70b0 | 20 | |
ba17bb62 RB |
21 | static inline u16 eir_precalc_len(u8 data_len) |
22 | { | |
23 | return sizeof(u8) * 2 + data_len; | |
24 | } | |
25 | ||
01ce70b0 LAD |
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 | ||
c2b2a1a7 RB |
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 | ||
01ce70b0 LAD |
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 | /* Zero length data */ | |
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 | } | |
8f9ae5b3 LAD |
98 | |
99 | void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len); |