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(hdev, HCI_OP_READ_LOCAL_CODEC_CAPS, 76 sizeof(*cmd), cmd, 77 HCI_CMD_TIMEOUT); 78 if (IS_ERR(skb)) { 79 bt_dev_err(hdev, "Failed to read codec capabilities (%ld)", 80 PTR_ERR(skb)); 81 continue; 82 } 83 84 if (skb->len < sizeof(*rp)) 85 goto error; 86 87 rp = (void *)skb->data; 88 89 if (rp->status) 90 goto error; 91 92 if (!rp->num_caps) { 93 len = 0; 94 /* this codec doesn't have capabilities */ 95 goto skip_caps_parse; 96 } 97 98 skb_pull(skb, sizeof(*rp)); 99 100 for (j = 0, len = 0; j < rp->num_caps; j++) { 101 caps = (void *)skb->data; 102 if (skb->len < sizeof(*caps)) 103 goto error; 104 if (skb->len < caps->len) 105 goto error; 106 len += sizeof(caps->len) + caps->len; 107 skb_pull(skb, sizeof(caps->len) + caps->len); 108 } 109 110 skip_caps_parse: 111 hci_dev_lock(hdev); 112 hci_codec_list_add(&hdev->local_codecs, cmd, rp, 113 (__u8 *)rp + sizeof(*rp), len); 114 hci_dev_unlock(hdev); 115 error: 116 kfree_skb(skb); 117 } 118 } 119 } 120 121 void hci_read_supported_codecs(struct hci_dev *hdev) 122 { 123 struct sk_buff *skb; 124 struct hci_rp_read_local_supported_codecs *rp; 125 struct hci_std_codecs *std_codecs; 126 struct hci_vnd_codecs *vnd_codecs; 127 struct hci_op_read_local_codec_caps caps; 128 __u8 i; 129 130 skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_CODECS, 0, NULL, 131 HCI_CMD_TIMEOUT); 132 133 if (IS_ERR(skb)) { 134 bt_dev_err(hdev, "Failed to read local supported codecs (%ld)", 135 PTR_ERR(skb)); 136 return; 137 } 138 139 if (skb->len < sizeof(*rp)) 140 goto error; 141 142 rp = (void *)skb->data; 143 144 if (rp->status) 145 goto error; 146 147 skb_pull(skb, sizeof(rp->status)); 148 149 std_codecs = (void *)skb->data; 150 151 /* validate codecs length before accessing */ 152 if (skb->len < flex_array_size(std_codecs, codec, std_codecs->num) 153 + sizeof(std_codecs->num)) 154 goto error; 155 156 /* enumerate codec capabilities of standard codecs */ 157 memset(&caps, 0, sizeof(caps)); 158 for (i = 0; i < std_codecs->num; i++) { 159 caps.id = std_codecs->codec[i]; 160 caps.direction = 0x00; 161 hci_read_codec_capabilities(hdev, LOCAL_CODEC_ACL_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, LOCAL_CODEC_ACL_MASK, &caps); 182 } 183 184 error: 185 kfree_skb(skb); 186 } 187 188 void hci_read_supported_codecs_v2(struct hci_dev *hdev) 189 { 190 struct sk_buff *skb; 191 struct hci_rp_read_local_supported_codecs_v2 *rp; 192 struct hci_std_codecs_v2 *std_codecs; 193 struct hci_vnd_codecs_v2 *vnd_codecs; 194 struct hci_op_read_local_codec_caps caps; 195 __u8 i; 196 197 skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_CODECS_V2, 0, NULL, 198 HCI_CMD_TIMEOUT); 199 200 if (IS_ERR(skb)) { 201 bt_dev_err(hdev, "Failed to read local supported codecs (%ld)", 202 PTR_ERR(skb)); 203 return; 204 } 205 206 if (skb->len < sizeof(*rp)) 207 goto error; 208 209 rp = (void *)skb->data; 210 211 if (rp->status) 212 goto error; 213 214 skb_pull(skb, sizeof(rp->status)); 215 216 std_codecs = (void *)skb->data; 217 218 /* check for payload data length before accessing */ 219 if (skb->len < flex_array_size(std_codecs, codec, std_codecs->num) 220 + sizeof(std_codecs->num)) 221 goto error; 222 223 memset(&caps, 0, sizeof(caps)); 224 225 for (i = 0; i < std_codecs->num; i++) { 226 caps.id = std_codecs->codec[i].id; 227 hci_read_codec_capabilities(hdev, std_codecs->codec[i].transport, 228 &caps); 229 } 230 231 skb_pull(skb, flex_array_size(std_codecs, codec, std_codecs->num) 232 + sizeof(std_codecs->num)); 233 234 vnd_codecs = (void *)skb->data; 235 236 /* check for payload data length before accessing */ 237 if (skb->len < 238 flex_array_size(vnd_codecs, codec, vnd_codecs->num) 239 + sizeof(vnd_codecs->num)) 240 goto error; 241 242 for (i = 0; i < vnd_codecs->num; i++) { 243 caps.id = 0xFF; 244 caps.cid = vnd_codecs->codec[i].cid; 245 caps.vid = vnd_codecs->codec[i].vid; 246 hci_read_codec_capabilities(hdev, vnd_codecs->codec[i].transport, 247 &caps); 248 } 249 250 error: 251 kfree_skb(skb); 252 } 253