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_append_data(u8 *eir, u16 eir_len, u8 type, 19 u8 *data, u8 data_len) 20 { 21 eir[eir_len++] = sizeof(type) + data_len; 22 eir[eir_len++] = type; 23 memcpy(&eir[eir_len], data, data_len); 24 eir_len += data_len; 25 26 return eir_len; 27 } 28 29 static inline u16 eir_append_le16(u8 *eir, u16 eir_len, u8 type, u16 data) 30 { 31 eir[eir_len++] = sizeof(type) + sizeof(data); 32 eir[eir_len++] = type; 33 put_unaligned_le16(data, &eir[eir_len]); 34 eir_len += sizeof(data); 35 36 return eir_len; 37 } 38 39 static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type, 40 size_t *data_len) 41 { 42 size_t parsed = 0; 43 44 if (eir_len < 2) 45 return NULL; 46 47 while (parsed < eir_len - 1) { 48 u8 field_len = eir[0]; 49 50 if (field_len == 0) 51 break; 52 53 parsed += field_len + 1; 54 55 if (parsed > eir_len) 56 break; 57 58 if (eir[1] != type) { 59 eir += field_len + 1; 60 continue; 61 } 62 63 /* Zero length data */ 64 if (field_len == 1) 65 return NULL; 66 67 if (data_len) 68 *data_len = field_len - 1; 69 70 return &eir[2]; 71 } 72 73 return NULL; 74 } 75