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(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(&entry->list, 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(&c->list); 44 kfree(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(&hdev->local_codecs, cmd, 70 NULL, NULL, 0); 71 hci_dev_unlock(hdev); 72 continue; 73 } 74 75 skb = __hci_cmd_sync_sk(hdev, HCI_OP_READ_LOCAL_CODEC_CAPS, 76 sizeof(*cmd), cmd, 0, HCI_CMD_TIMEOUT, NULL); 77 if (IS_ERR(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, 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, sizeof(caps->len) + caps->len); 107 } 108 109 skip_caps_parse: 110 hci_dev_lock(hdev); 111 hci_codec_list_add(&hdev->local_codecs, cmd, rp, 112 (__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, 0, NULL, 130 0, HCI_CMD_TIMEOUT, NULL); 131 132 if (IS_ERR(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, 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, &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, &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, 0, NULL, 199 0, HCI_CMD_TIMEOUT, NULL); 200 201 if (IS_ERR(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, 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, std_codecs->codec[i].transport, 229 &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, vnd_codecs->codec[i].transport, 248 &caps); 249 } 250 251 error: 252 kfree_skb(skb); 253 } 254