1 // SPDX-License-Identifier: GPL-2.0
3 /* Copyright (C) 2021 Intel Corporation */
5 #include <net/bluetooth/bluetooth.h>
6 #include <net/bluetooth/hci_core.h>
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
,
15 struct codec_list
*entry
;
17 entry
= kzalloc(sizeof(*entry
) + len
, GFP_KERNEL
);
22 if (sent
->id
== 0xFF) {
23 entry
->cid
= __le16_to_cpu(sent
->cid
);
24 entry
->vid
= __le16_to_cpu(sent
->vid
);
26 entry
->transport
= sent
->transport
;
30 entry
->num_caps
= rp
->num_caps
;
31 memcpy(entry
->caps
, caps
, len
);
33 list_add(&entry
->list
, list
);
38 void hci_codec_list_clear(struct list_head
*codec_list
)
40 struct codec_list
*c
, *n
;
42 list_for_each_entry_safe(c
, n
, codec_list
, list
) {
48 static void hci_read_codec_capabilities(struct hci_dev
*hdev
, __u8 transport
,
49 struct hci_op_read_local_codec_caps
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
;
64 /* If Read_Codec_Capabilities command is not supported
65 * then just add codec to the list without caps
67 if (!(hdev
->commands
[45] & 0x08)) {
69 hci_codec_list_add(&hdev
->local_codecs
, cmd
,
75 skb
= __hci_cmd_sync_sk(hdev
, HCI_OP_READ_LOCAL_CODEC_CAPS
,
76 sizeof(*cmd
), cmd
, 0, HCI_CMD_TIMEOUT
, NULL
);
78 bt_dev_err(hdev
, "Failed to read codec capabilities (%ld)",
83 if (skb
->len
< sizeof(*rp
))
86 rp
= (void *)skb
->data
;
93 /* this codec doesn't have capabilities */
97 skb_pull(skb
, sizeof(*rp
));
99 for (j
= 0, len
= 0; j
< rp
->num_caps
; j
++) {
100 caps
= (void *)skb
->data
;
101 if (skb
->len
< sizeof(*caps
))
103 if (skb
->len
< caps
->len
)
105 len
+= sizeof(caps
->len
) + caps
->len
;
106 skb_pull(skb
, sizeof(caps
->len
) + caps
->len
);
111 hci_codec_list_add(&hdev
->local_codecs
, cmd
, rp
,
112 (__u8
*)rp
+ sizeof(*rp
), len
);
113 hci_dev_unlock(hdev
);
120 void hci_read_supported_codecs(struct hci_dev
*hdev
)
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
;
129 skb
= __hci_cmd_sync_sk(hdev
, HCI_OP_READ_LOCAL_CODECS
, 0, NULL
,
130 0, HCI_CMD_TIMEOUT
, NULL
);
133 bt_dev_err(hdev
, "Failed to read local supported codecs (%ld)",
138 if (skb
->len
< sizeof(*rp
))
141 rp
= (void *)skb
->data
;
146 skb_pull(skb
, sizeof(rp
->status
));
148 std_codecs
= (void *)skb
->data
;
150 /* validate codecs length before accessing */
151 if (skb
->len
< flex_array_size(std_codecs
, codec
, std_codecs
->num
)
152 + sizeof(std_codecs
->num
))
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
, &caps
);
164 skb_pull(skb
, flex_array_size(std_codecs
, codec
, std_codecs
->num
)
165 + sizeof(std_codecs
->num
));
167 vnd_codecs
= (void *)skb
->data
;
169 /* validate vendor codecs length before accessing */
171 flex_array_size(vnd_codecs
, codec
, vnd_codecs
->num
)
172 + sizeof(vnd_codecs
->num
))
175 /* enumerate vendor codec capabilities */
176 for (i
= 0; i
< vnd_codecs
->num
; i
++) {
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
, &caps
);
189 void hci_read_supported_codecs_v2(struct hci_dev
*hdev
)
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
;
198 skb
= __hci_cmd_sync_sk(hdev
, HCI_OP_READ_LOCAL_CODECS_V2
, 0, NULL
,
199 0, HCI_CMD_TIMEOUT
, NULL
);
202 bt_dev_err(hdev
, "Failed to read local supported codecs (%ld)",
207 if (skb
->len
< sizeof(*rp
))
210 rp
= (void *)skb
->data
;
215 skb_pull(skb
, sizeof(rp
->status
));
217 std_codecs
= (void *)skb
->data
;
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
))
224 memset(&caps
, 0, sizeof(caps
));
226 for (i
= 0; i
< std_codecs
->num
; i
++) {
227 caps
.id
= std_codecs
->codec
[i
].id
;
228 hci_read_codec_capabilities(hdev
, std_codecs
->codec
[i
].transport
,
232 skb_pull(skb
, flex_array_size(std_codecs
, codec
, std_codecs
->num
)
233 + sizeof(std_codecs
->num
));
235 vnd_codecs
= (void *)skb
->data
;
237 /* check for payload data length before accessing */
239 flex_array_size(vnd_codecs
, codec
, vnd_codecs
->num
)
240 + sizeof(vnd_codecs
->num
))
243 for (i
= 0; i
< vnd_codecs
->num
; i
++) {
245 caps
.cid
= vnd_codecs
->codec
[i
].cid
;
246 caps
.vid
= vnd_codecs
->codec
[i
].vid
;
247 hci_read_codec_capabilities(hdev
, vnd_codecs
->codec
[i
].transport
,