| Luiz Augusto von Dentz | 01ce70b | 2021-09-20 15:59:37 -0700 | [diff] [blame] | 1 | /* SPDX-License-Identifier: GPL-2.0 */ |
| 2 | /* |
| 3 | * BlueZ - Bluetooth protocol stack for Linux |
| 4 | * |
| 5 | * Copyright (C) 2021 Intel Corporation |
| 6 | */ |
| 7 | |
| Jakub Kicinski | b645941 | 2021-12-28 16:49:13 -0800 | [diff] [blame] | 8 | #include <asm/unaligned.h> |
| 9 | |
| Luiz Augusto von Dentz | 01ce70b | 2021-09-20 15:59:37 -0700 | [diff] [blame] | 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 | } |