1 | /* SPDX-License-Identifier: GPL-2.0 */ |
2 | /* |
3 | * BlueZ - Bluetooth protocol stack for Linux |
4 | * |
5 | * Copyright (C) 2021 Intel Corporation |
6 | */ |
7 | |
8 | #include <asm/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); |
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(val: data, p: &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, len: 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 | /* 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 | } |
98 | |
99 | void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len); |
100 | |