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 15 u8 eir_append_local_name(struct hci_dev *hdev, u8 *eir, u8 ad_len); 16 u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len); 17 18 static inline u16 eir_precalc_len(u8 data_len) 19 { 20 return sizeof(u8) * 2 + data_len; 21 } 22 23 static inline u16 eir_append_data(u8 *eir, u16 eir_len, u8 type, 24 u8 *data, u8 data_len) 25 { 26 eir[eir_len++] = sizeof(type) + data_len; 27 eir[eir_len++] = type; 28 memcpy(&eir[eir_len], data, data_len); 29 eir_len += data_len; 30 31 return eir_len; 32 } 33 34 static inline u16 eir_append_le16(u8 *eir, u16 eir_len, u8 type, u16 data) 35 { 36 eir[eir_len++] = sizeof(type) + sizeof(data); 37 eir[eir_len++] = type; 38 put_unaligned_le16(data, &eir[eir_len]); 39 eir_len += sizeof(data); 40 41 return eir_len; 42 } 43 44 static inline u16 eir_skb_put_data(struct sk_buff *skb, u8 type, u8 *data, u8 data_len) 45 { 46 u8 *eir; 47 u16 eir_len; 48 49 eir_len = eir_precalc_len(data_len); 50 eir = skb_put(skb, eir_len); 51 WARN_ON(sizeof(type) + data_len > U8_MAX); 52 eir[0] = sizeof(type) + data_len; 53 eir[1] = type; 54 memcpy(&eir[2], data, data_len); 55 56 return eir_len; 57 } 58 59 static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type, 60 size_t *data_len) 61 { 62 size_t parsed = 0; 63 64 if (eir_len < 2) 65 return NULL; 66 67 while (parsed < eir_len - 1) { 68 u8 field_len = eir[0]; 69 70 if (field_len == 0) 71 break; 72 73 parsed += field_len + 1; 74 75 if (parsed > eir_len) 76 break; 77 78 if (eir[1] != type) { 79 eir += field_len + 1; 80 continue; 81 } 82 83 /* Zero length data */ 84 if (field_len == 1) 85 return NULL; 86 87 if (data_len) 88 *data_len = field_len - 1; 89 90 return &eir[2]; 91 } 92 93 return NULL; 94 } 95