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 8*b6459415SJakub Kicinski #include <asm/unaligned.h> 9*b6459415SJakub 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); 1401ce70b0SLuiz Augusto von Dentz 1501ce70b0SLuiz Augusto von Dentz u8 eir_append_local_name(struct hci_dev *hdev, u8 *eir, u8 ad_len); 1601ce70b0SLuiz Augusto von Dentz u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len); 1701ce70b0SLuiz Augusto von Dentz 1801ce70b0SLuiz Augusto von Dentz static inline u16 eir_append_data(u8 *eir, u16 eir_len, u8 type, 1901ce70b0SLuiz Augusto von Dentz u8 *data, u8 data_len) 2001ce70b0SLuiz Augusto von Dentz { 2101ce70b0SLuiz Augusto von Dentz eir[eir_len++] = sizeof(type) + data_len; 2201ce70b0SLuiz Augusto von Dentz eir[eir_len++] = type; 2301ce70b0SLuiz Augusto von Dentz memcpy(&eir[eir_len], data, data_len); 2401ce70b0SLuiz Augusto von Dentz eir_len += data_len; 2501ce70b0SLuiz Augusto von Dentz 2601ce70b0SLuiz Augusto von Dentz return eir_len; 2701ce70b0SLuiz Augusto von Dentz } 2801ce70b0SLuiz Augusto von Dentz 2901ce70b0SLuiz Augusto von Dentz static inline u16 eir_append_le16(u8 *eir, u16 eir_len, u8 type, u16 data) 3001ce70b0SLuiz Augusto von Dentz { 3101ce70b0SLuiz Augusto von Dentz eir[eir_len++] = sizeof(type) + sizeof(data); 3201ce70b0SLuiz Augusto von Dentz eir[eir_len++] = type; 3301ce70b0SLuiz Augusto von Dentz put_unaligned_le16(data, &eir[eir_len]); 3401ce70b0SLuiz Augusto von Dentz eir_len += sizeof(data); 3501ce70b0SLuiz Augusto von Dentz 3601ce70b0SLuiz Augusto von Dentz return eir_len; 3701ce70b0SLuiz Augusto von Dentz } 3801ce70b0SLuiz Augusto von Dentz 3901ce70b0SLuiz Augusto von Dentz static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type, 4001ce70b0SLuiz Augusto von Dentz size_t *data_len) 4101ce70b0SLuiz Augusto von Dentz { 4201ce70b0SLuiz Augusto von Dentz size_t parsed = 0; 4301ce70b0SLuiz Augusto von Dentz 4401ce70b0SLuiz Augusto von Dentz if (eir_len < 2) 4501ce70b0SLuiz Augusto von Dentz return NULL; 4601ce70b0SLuiz Augusto von Dentz 4701ce70b0SLuiz Augusto von Dentz while (parsed < eir_len - 1) { 4801ce70b0SLuiz Augusto von Dentz u8 field_len = eir[0]; 4901ce70b0SLuiz Augusto von Dentz 5001ce70b0SLuiz Augusto von Dentz if (field_len == 0) 5101ce70b0SLuiz Augusto von Dentz break; 5201ce70b0SLuiz Augusto von Dentz 5301ce70b0SLuiz Augusto von Dentz parsed += field_len + 1; 5401ce70b0SLuiz Augusto von Dentz 5501ce70b0SLuiz Augusto von Dentz if (parsed > eir_len) 5601ce70b0SLuiz Augusto von Dentz break; 5701ce70b0SLuiz Augusto von Dentz 5801ce70b0SLuiz Augusto von Dentz if (eir[1] != type) { 5901ce70b0SLuiz Augusto von Dentz eir += field_len + 1; 6001ce70b0SLuiz Augusto von Dentz continue; 6101ce70b0SLuiz Augusto von Dentz } 6201ce70b0SLuiz Augusto von Dentz 6301ce70b0SLuiz Augusto von Dentz /* Zero length data */ 6401ce70b0SLuiz Augusto von Dentz if (field_len == 1) 6501ce70b0SLuiz Augusto von Dentz return NULL; 6601ce70b0SLuiz Augusto von Dentz 6701ce70b0SLuiz Augusto von Dentz if (data_len) 6801ce70b0SLuiz Augusto von Dentz *data_len = field_len - 1; 6901ce70b0SLuiz Augusto von Dentz 7001ce70b0SLuiz Augusto von Dentz return &eir[2]; 7101ce70b0SLuiz Augusto von Dentz } 7201ce70b0SLuiz Augusto von Dentz 7301ce70b0SLuiz Augusto von Dentz return NULL; 7401ce70b0SLuiz Augusto von Dentz } 75