1 | // SPDX-License-Identifier: GPL-2.0 |
2 | |
3 | /* Copyright (C) 2021 Intel Corporation */ |
4 | |
5 | #include <net/bluetooth/bluetooth.h> |
6 | #include <net/bluetooth/hci_core.h> |
7 | #include "hci_codec.h" |
8 | |
9 | static int hci_codec_list_add(struct list_head *list, |
10 | struct hci_op_read_local_codec_caps *sent, |
11 | struct hci_rp_read_local_codec_caps *rp, |
12 | void *caps, |
13 | __u32 len) |
14 | { |
15 | struct codec_list *entry; |
16 | |
17 | entry = kzalloc(size: sizeof(*entry) + len, GFP_KERNEL); |
18 | if (!entry) |
19 | return -ENOMEM; |
20 | |
21 | entry->id = sent->id; |
22 | if (sent->id == 0xFF) { |
23 | entry->cid = __le16_to_cpu(sent->cid); |
24 | entry->vid = __le16_to_cpu(sent->vid); |
25 | } |
26 | entry->transport = sent->transport; |
27 | entry->len = len; |
28 | entry->num_caps = 0; |
29 | if (rp) { |
30 | entry->num_caps = rp->num_caps; |
31 | memcpy(entry->caps, caps, len); |
32 | } |
33 | list_add(new: &entry->list, head: list); |
34 | |
35 | return 0; |
36 | } |
37 | |
38 | void hci_codec_list_clear(struct list_head *codec_list) |
39 | { |
40 | struct codec_list *c, *n; |
41 | |
42 | list_for_each_entry_safe(c, n, codec_list, list) { |
43 | list_del(entry: &c->list); |
44 | kfree(objp: c); |
45 | } |
46 | } |
47 | |
48 | static void hci_read_codec_capabilities(struct hci_dev *hdev, __u8 transport, |
49 | struct hci_op_read_local_codec_caps |
50 | *cmd) |
51 | { |
52 | __u8 i; |
53 | |
54 | for (i = 0; i < TRANSPORT_TYPE_MAX; i++) { |
55 | if (transport & BIT(i)) { |
56 | struct hci_rp_read_local_codec_caps *rp; |
57 | struct hci_codec_caps *caps; |
58 | struct sk_buff *skb; |
59 | __u8 j; |
60 | __u32 len; |
61 | |
62 | cmd->transport = i; |
63 | |
64 | /* If Read_Codec_Capabilities command is not supported |
65 | * then just add codec to the list without caps |
66 | */ |
67 | if (!(hdev->commands[45] & 0x08)) { |
68 | hci_dev_lock(hdev); |
69 | hci_codec_list_add(list: &hdev->local_codecs, sent: cmd, |
70 | NULL, NULL, len: 0); |
71 | hci_dev_unlock(hdev); |
72 | continue; |
73 | } |
74 | |
75 | skb = __hci_cmd_sync_sk(hdev, HCI_OP_READ_LOCAL_CODEC_CAPS, |
76 | plen: sizeof(*cmd), param: cmd, event: 0, HCI_CMD_TIMEOUT, NULL); |
77 | if (IS_ERR(ptr: skb)) { |
78 | bt_dev_err(hdev, "Failed to read codec capabilities (%ld)" , |
79 | PTR_ERR(skb)); |
80 | continue; |
81 | } |
82 | |
83 | if (skb->len < sizeof(*rp)) |
84 | goto error; |
85 | |
86 | rp = (void *)skb->data; |
87 | |
88 | if (rp->status) |
89 | goto error; |
90 | |
91 | if (!rp->num_caps) { |
92 | len = 0; |
93 | /* this codec doesn't have capabilities */ |
94 | goto skip_caps_parse; |
95 | } |
96 | |
97 | skb_pull(skb, len: sizeof(*rp)); |
98 | |
99 | for (j = 0, len = 0; j < rp->num_caps; j++) { |
100 | caps = (void *)skb->data; |
101 | if (skb->len < sizeof(*caps)) |
102 | goto error; |
103 | if (skb->len < caps->len) |
104 | goto error; |
105 | len += sizeof(caps->len) + caps->len; |
106 | skb_pull(skb, len: sizeof(caps->len) + caps->len); |
107 | } |
108 | |
109 | skip_caps_parse: |
110 | hci_dev_lock(hdev); |
111 | hci_codec_list_add(list: &hdev->local_codecs, sent: cmd, rp, |
112 | caps: (__u8 *)rp + sizeof(*rp), len); |
113 | hci_dev_unlock(hdev); |
114 | error: |
115 | kfree_skb(skb); |
116 | } |
117 | } |
118 | } |
119 | |
120 | void hci_read_supported_codecs(struct hci_dev *hdev) |
121 | { |
122 | struct sk_buff *skb; |
123 | struct hci_rp_read_local_supported_codecs *rp; |
124 | struct hci_std_codecs *std_codecs; |
125 | struct hci_vnd_codecs *vnd_codecs; |
126 | struct hci_op_read_local_codec_caps caps; |
127 | __u8 i; |
128 | |
129 | skb = __hci_cmd_sync_sk(hdev, HCI_OP_READ_LOCAL_CODECS, plen: 0, NULL, |
130 | event: 0, HCI_CMD_TIMEOUT, NULL); |
131 | |
132 | if (IS_ERR(ptr: skb)) { |
133 | bt_dev_err(hdev, "Failed to read local supported codecs (%ld)" , |
134 | PTR_ERR(skb)); |
135 | return; |
136 | } |
137 | |
138 | if (skb->len < sizeof(*rp)) |
139 | goto error; |
140 | |
141 | rp = (void *)skb->data; |
142 | |
143 | if (rp->status) |
144 | goto error; |
145 | |
146 | skb_pull(skb, len: sizeof(rp->status)); |
147 | |
148 | std_codecs = (void *)skb->data; |
149 | |
150 | /* validate codecs length before accessing */ |
151 | if (skb->len < flex_array_size(std_codecs, codec, std_codecs->num) |
152 | + sizeof(std_codecs->num)) |
153 | goto error; |
154 | |
155 | /* enumerate codec capabilities of standard codecs */ |
156 | memset(&caps, 0, sizeof(caps)); |
157 | for (i = 0; i < std_codecs->num; i++) { |
158 | caps.id = std_codecs->codec[i]; |
159 | caps.direction = 0x00; |
160 | hci_read_codec_capabilities(hdev, |
161 | LOCAL_CODEC_ACL_MASK | LOCAL_CODEC_SCO_MASK, cmd: &caps); |
162 | } |
163 | |
164 | skb_pull(skb, flex_array_size(std_codecs, codec, std_codecs->num) |
165 | + sizeof(std_codecs->num)); |
166 | |
167 | vnd_codecs = (void *)skb->data; |
168 | |
169 | /* validate vendor codecs length before accessing */ |
170 | if (skb->len < |
171 | flex_array_size(vnd_codecs, codec, vnd_codecs->num) |
172 | + sizeof(vnd_codecs->num)) |
173 | goto error; |
174 | |
175 | /* enumerate vendor codec capabilities */ |
176 | for (i = 0; i < vnd_codecs->num; i++) { |
177 | caps.id = 0xFF; |
178 | caps.cid = vnd_codecs->codec[i].cid; |
179 | caps.vid = vnd_codecs->codec[i].vid; |
180 | caps.direction = 0x00; |
181 | hci_read_codec_capabilities(hdev, |
182 | LOCAL_CODEC_ACL_MASK | LOCAL_CODEC_SCO_MASK, cmd: &caps); |
183 | } |
184 | |
185 | error: |
186 | kfree_skb(skb); |
187 | } |
188 | |
189 | void hci_read_supported_codecs_v2(struct hci_dev *hdev) |
190 | { |
191 | struct sk_buff *skb; |
192 | struct hci_rp_read_local_supported_codecs_v2 *rp; |
193 | struct hci_std_codecs_v2 *std_codecs; |
194 | struct hci_vnd_codecs_v2 *vnd_codecs; |
195 | struct hci_op_read_local_codec_caps caps; |
196 | __u8 i; |
197 | |
198 | skb = __hci_cmd_sync_sk(hdev, HCI_OP_READ_LOCAL_CODECS_V2, plen: 0, NULL, |
199 | event: 0, HCI_CMD_TIMEOUT, NULL); |
200 | |
201 | if (IS_ERR(ptr: skb)) { |
202 | bt_dev_err(hdev, "Failed to read local supported codecs (%ld)" , |
203 | PTR_ERR(skb)); |
204 | return; |
205 | } |
206 | |
207 | if (skb->len < sizeof(*rp)) |
208 | goto error; |
209 | |
210 | rp = (void *)skb->data; |
211 | |
212 | if (rp->status) |
213 | goto error; |
214 | |
215 | skb_pull(skb, len: sizeof(rp->status)); |
216 | |
217 | std_codecs = (void *)skb->data; |
218 | |
219 | /* check for payload data length before accessing */ |
220 | if (skb->len < flex_array_size(std_codecs, codec, std_codecs->num) |
221 | + sizeof(std_codecs->num)) |
222 | goto error; |
223 | |
224 | memset(&caps, 0, sizeof(caps)); |
225 | |
226 | for (i = 0; i < std_codecs->num; i++) { |
227 | caps.id = std_codecs->codec[i].id; |
228 | hci_read_codec_capabilities(hdev, transport: std_codecs->codec[i].transport, |
229 | cmd: &caps); |
230 | } |
231 | |
232 | skb_pull(skb, flex_array_size(std_codecs, codec, std_codecs->num) |
233 | + sizeof(std_codecs->num)); |
234 | |
235 | vnd_codecs = (void *)skb->data; |
236 | |
237 | /* check for payload data length before accessing */ |
238 | if (skb->len < |
239 | flex_array_size(vnd_codecs, codec, vnd_codecs->num) |
240 | + sizeof(vnd_codecs->num)) |
241 | goto error; |
242 | |
243 | for (i = 0; i < vnd_codecs->num; i++) { |
244 | caps.id = 0xFF; |
245 | caps.cid = vnd_codecs->codec[i].cid; |
246 | caps.vid = vnd_codecs->codec[i].vid; |
247 | hci_read_codec_capabilities(hdev, transport: vnd_codecs->codec[i].transport, |
248 | cmd: &caps); |
249 | } |
250 | |
251 | error: |
252 | kfree_skb(skb); |
253 | } |
254 | |