101ce70b0SLuiz Augusto von Dentz /* SPDX-License-Identifier: GPL-2.0 */
201ce70b0SLuiz Augusto von Dentz /*
301ce70b0SLuiz Augusto von Dentz * BlueZ - Bluetooth protocol stack for Linux
401ce70b0SLuiz Augusto von Dentz *
501ce70b0SLuiz Augusto von Dentz * Copyright (C) 2021 Intel Corporation
601ce70b0SLuiz Augusto von Dentz */
701ce70b0SLuiz Augusto von Dentz
8b6459415SJakub Kicinski #include <asm/unaligned.h>
9b6459415SJakub Kicinski
1001ce70b0SLuiz Augusto von Dentz void eir_create(struct hci_dev *hdev, u8 *data);
1101ce70b0SLuiz Augusto von Dentz
1201ce70b0SLuiz Augusto von Dentz u8 eir_create_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr);
1301ce70b0SLuiz Augusto von Dentz u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr);
14*eca0ae4aSLuiz Augusto von Dentz u8 eir_create_per_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr);
1501ce70b0SLuiz Augusto von Dentz
1601ce70b0SLuiz Augusto von Dentz u8 eir_append_local_name(struct hci_dev *hdev, u8 *eir, u8 ad_len);
1701ce70b0SLuiz Augusto von Dentz u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len);
188f9ae5b3SLuiz Augusto von Dentz u8 eir_append_service_data(u8 *eir, u16 eir_len, u16 uuid, u8 *data,
198f9ae5b3SLuiz Augusto von Dentz u8 data_len);
2001ce70b0SLuiz Augusto von Dentz
eir_precalc_len(u8 data_len)21ba17bb62SRadoslaw Biernacki static inline u16 eir_precalc_len(u8 data_len)
22ba17bb62SRadoslaw Biernacki {
23ba17bb62SRadoslaw Biernacki return sizeof(u8) * 2 + data_len;
24ba17bb62SRadoslaw Biernacki }
25ba17bb62SRadoslaw Biernacki
eir_append_data(u8 * eir,u16 eir_len,u8 type,u8 * data,u8 data_len)2601ce70b0SLuiz Augusto von Dentz static inline u16 eir_append_data(u8 *eir, u16 eir_len, u8 type,
2701ce70b0SLuiz Augusto von Dentz u8 *data, u8 data_len)
2801ce70b0SLuiz Augusto von Dentz {
2901ce70b0SLuiz Augusto von Dentz eir[eir_len++] = sizeof(type) + data_len;
3001ce70b0SLuiz Augusto von Dentz eir[eir_len++] = type;
3101ce70b0SLuiz Augusto von Dentz memcpy(&eir[eir_len], data, data_len);
3201ce70b0SLuiz Augusto von Dentz eir_len += data_len;
3301ce70b0SLuiz Augusto von Dentz
3401ce70b0SLuiz Augusto von Dentz return eir_len;
3501ce70b0SLuiz Augusto von Dentz }
3601ce70b0SLuiz Augusto von Dentz
eir_append_le16(u8 * eir,u16 eir_len,u8 type,u16 data)3701ce70b0SLuiz Augusto von Dentz static inline u16 eir_append_le16(u8 *eir, u16 eir_len, u8 type, u16 data)
3801ce70b0SLuiz Augusto von Dentz {
3901ce70b0SLuiz Augusto von Dentz eir[eir_len++] = sizeof(type) + sizeof(data);
4001ce70b0SLuiz Augusto von Dentz eir[eir_len++] = type;
4101ce70b0SLuiz Augusto von Dentz put_unaligned_le16(data, &eir[eir_len]);
4201ce70b0SLuiz Augusto von Dentz eir_len += sizeof(data);
4301ce70b0SLuiz Augusto von Dentz
4401ce70b0SLuiz Augusto von Dentz return eir_len;
4501ce70b0SLuiz Augusto von Dentz }
4601ce70b0SLuiz Augusto von Dentz
eir_skb_put_data(struct sk_buff * skb,u8 type,u8 * data,u8 data_len)47c2b2a1a7SRadoslaw Biernacki static inline u16 eir_skb_put_data(struct sk_buff *skb, u8 type, u8 *data, u8 data_len)
48c2b2a1a7SRadoslaw Biernacki {
49c2b2a1a7SRadoslaw Biernacki u8 *eir;
50c2b2a1a7SRadoslaw Biernacki u16 eir_len;
51c2b2a1a7SRadoslaw Biernacki
52c2b2a1a7SRadoslaw Biernacki eir_len = eir_precalc_len(data_len);
53c2b2a1a7SRadoslaw Biernacki eir = skb_put(skb, eir_len);
54c2b2a1a7SRadoslaw Biernacki WARN_ON(sizeof(type) + data_len > U8_MAX);
55c2b2a1a7SRadoslaw Biernacki eir[0] = sizeof(type) + data_len;
56c2b2a1a7SRadoslaw Biernacki eir[1] = type;
57c2b2a1a7SRadoslaw Biernacki memcpy(&eir[2], data, data_len);
58c2b2a1a7SRadoslaw Biernacki
59c2b2a1a7SRadoslaw Biernacki return eir_len;
60c2b2a1a7SRadoslaw Biernacki }
61c2b2a1a7SRadoslaw Biernacki
eir_get_data(u8 * eir,size_t eir_len,u8 type,size_t * data_len)6201ce70b0SLuiz Augusto von Dentz static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type,
6301ce70b0SLuiz Augusto von Dentz size_t *data_len)
6401ce70b0SLuiz Augusto von Dentz {
6501ce70b0SLuiz Augusto von Dentz size_t parsed = 0;
6601ce70b0SLuiz Augusto von Dentz
6701ce70b0SLuiz Augusto von Dentz if (eir_len < 2)
6801ce70b0SLuiz Augusto von Dentz return NULL;
6901ce70b0SLuiz Augusto von Dentz
7001ce70b0SLuiz Augusto von Dentz while (parsed < eir_len - 1) {
7101ce70b0SLuiz Augusto von Dentz u8 field_len = eir[0];
7201ce70b0SLuiz Augusto von Dentz
7301ce70b0SLuiz Augusto von Dentz if (field_len == 0)
7401ce70b0SLuiz Augusto von Dentz break;
7501ce70b0SLuiz Augusto von Dentz
7601ce70b0SLuiz Augusto von Dentz parsed += field_len + 1;
7701ce70b0SLuiz Augusto von Dentz
7801ce70b0SLuiz Augusto von Dentz if (parsed > eir_len)
7901ce70b0SLuiz Augusto von Dentz break;
8001ce70b0SLuiz Augusto von Dentz
8101ce70b0SLuiz Augusto von Dentz if (eir[1] != type) {
8201ce70b0SLuiz Augusto von Dentz eir += field_len + 1;
8301ce70b0SLuiz Augusto von Dentz continue;
8401ce70b0SLuiz Augusto von Dentz }
8501ce70b0SLuiz Augusto von Dentz
8601ce70b0SLuiz Augusto von Dentz /* Zero length data */
8701ce70b0SLuiz Augusto von Dentz if (field_len == 1)
8801ce70b0SLuiz Augusto von Dentz return NULL;
8901ce70b0SLuiz Augusto von Dentz
9001ce70b0SLuiz Augusto von Dentz if (data_len)
9101ce70b0SLuiz Augusto von Dentz *data_len = field_len - 1;
9201ce70b0SLuiz Augusto von Dentz
9301ce70b0SLuiz Augusto von Dentz return &eir[2];
9401ce70b0SLuiz Augusto von Dentz }
9501ce70b0SLuiz Augusto von Dentz
9601ce70b0SLuiz Augusto von Dentz return NULL;
9701ce70b0SLuiz Augusto von Dentz }
988f9ae5b3SLuiz Augusto von Dentz
998f9ae5b3SLuiz Augusto von Dentz void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len);
100