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 = rp->num_caps; 29 if (rp->num_caps) 30 memcpy(entry->caps, caps, len); 31 list_add(&entry->list, list); 32 33 return 0; 34 } 35 36 void hci_codec_list_clear(struct list_head *codec_list) 37 { 38 struct codec_list *c, *n; 39 40 list_for_each_entry_safe(c, n, codec_list, list) { 41 list_del(&c->list); 42 kfree(c); 43 } 44 } 45 46 static void hci_read_codec_capabilities(struct hci_dev *hdev, __u8 transport, 47 struct hci_op_read_local_codec_caps 48 *cmd) 49 { 50 __u8 i; 51 52 for (i = 0; i < TRANSPORT_TYPE_MAX; i++) { 53 if (transport & BIT(i)) { 54 struct hci_rp_read_local_codec_caps *rp; 55 struct hci_codec_caps *caps; 56 struct sk_buff *skb; 57 __u8 j; 58 __u32 len; 59 60 cmd->transport = i; 61 skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_CODEC_CAPS, 62 sizeof(*cmd), cmd, 63 HCI_CMD_TIMEOUT); 64 if (IS_ERR(skb)) { 65 bt_dev_err(hdev, "Failed to read codec capabilities (%ld)", 66 PTR_ERR(skb)); 67 continue; 68 } 69 70 if (skb->len < sizeof(*rp)) 71 goto error; 72 73 rp = (void *)skb->data; 74 75 if (rp->status) 76 goto error; 77 78 if (!rp->num_caps) { 79 len = 0; 80 /* this codec doesn't have capabilities */ 81 goto skip_caps_parse; 82 } 83 84 skb_pull(skb, sizeof(*rp)); 85 86 for (j = 0, len = 0; j < rp->num_caps; j++) { 87 caps = (void *)skb->data; 88 if (skb->len < sizeof(*caps)) 89 goto error; 90 if (skb->len < caps->len) 91 goto error; 92 len += sizeof(caps->len) + caps->len; 93 skb_pull(skb, sizeof(caps->len) + caps->len); 94 } 95 96 skip_caps_parse: 97 hci_dev_lock(hdev); 98 hci_codec_list_add(&hdev->local_codecs, cmd, rp, 99 (__u8 *)rp + sizeof(*rp), len); 100 hci_dev_unlock(hdev); 101 error: 102 kfree_skb(skb); 103 } 104 } 105 } 106 107 void hci_read_supported_codecs(struct hci_dev *hdev) 108 { 109 struct sk_buff *skb; 110 struct hci_rp_read_local_supported_codecs *rp; 111 struct hci_std_codecs *std_codecs; 112 struct hci_vnd_codecs *vnd_codecs; 113 struct hci_op_read_local_codec_caps caps; 114 __u8 i; 115 116 skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_CODECS, 0, NULL, 117 HCI_CMD_TIMEOUT); 118 119 if (IS_ERR(skb)) { 120 bt_dev_err(hdev, "Failed to read local supported codecs (%ld)", 121 PTR_ERR(skb)); 122 return; 123 } 124 125 if (skb->len < sizeof(*rp)) 126 goto error; 127 128 rp = (void *)skb->data; 129 130 if (rp->status) 131 goto error; 132 133 skb_pull(skb, sizeof(rp->status)); 134 135 std_codecs = (void *)skb->data; 136 137 /* validate codecs length before accessing */ 138 if (skb->len < flex_array_size(std_codecs, codec, std_codecs->num) 139 + sizeof(std_codecs->num)) 140 goto error; 141 142 /* enumerate codec capabilities of standard codecs */ 143 memset(&caps, 0, sizeof(caps)); 144 for (i = 0; i < std_codecs->num; i++) { 145 caps.id = std_codecs->codec[i]; 146 caps.direction = 0x00; 147 hci_read_codec_capabilities(hdev, LOCAL_CODEC_ACL_MASK, &caps); 148 } 149 150 skb_pull(skb, flex_array_size(std_codecs, codec, std_codecs->num) 151 + sizeof(std_codecs->num)); 152 153 vnd_codecs = (void *)skb->data; 154 155 /* validate vendor codecs length before accessing */ 156 if (skb->len < 157 flex_array_size(vnd_codecs, codec, vnd_codecs->num) 158 + sizeof(vnd_codecs->num)) 159 goto error; 160 161 /* enumerate vendor codec capabilities */ 162 for (i = 0; i < vnd_codecs->num; i++) { 163 caps.id = 0xFF; 164 caps.cid = vnd_codecs->codec[i].cid; 165 caps.vid = vnd_codecs->codec[i].vid; 166 caps.direction = 0x00; 167 hci_read_codec_capabilities(hdev, LOCAL_CODEC_ACL_MASK, &caps); 168 } 169 170 error: 171 kfree_skb(skb); 172 } 173 174 void hci_read_supported_codecs_v2(struct hci_dev *hdev) 175 { 176 struct sk_buff *skb; 177 struct hci_rp_read_local_supported_codecs_v2 *rp; 178 struct hci_std_codecs_v2 *std_codecs; 179 struct hci_vnd_codecs_v2 *vnd_codecs; 180 struct hci_op_read_local_codec_caps caps; 181 __u8 i; 182 183 skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_CODECS_V2, 0, NULL, 184 HCI_CMD_TIMEOUT); 185 186 if (IS_ERR(skb)) { 187 bt_dev_err(hdev, "Failed to read local supported codecs (%ld)", 188 PTR_ERR(skb)); 189 return; 190 } 191 192 if (skb->len < sizeof(*rp)) 193 goto error; 194 195 rp = (void *)skb->data; 196 197 if (rp->status) 198 goto error; 199 200 skb_pull(skb, sizeof(rp->status)); 201 202 std_codecs = (void *)skb->data; 203 204 /* check for payload data length before accessing */ 205 if (skb->len < flex_array_size(std_codecs, codec, std_codecs->num) 206 + sizeof(std_codecs->num)) 207 goto error; 208 209 memset(&caps, 0, sizeof(caps)); 210 211 for (i = 0; i < std_codecs->num; i++) { 212 caps.id = std_codecs->codec[i].id; 213 hci_read_codec_capabilities(hdev, std_codecs->codec[i].transport, 214 &caps); 215 } 216 217 skb_pull(skb, flex_array_size(std_codecs, codec, std_codecs->num) 218 + sizeof(std_codecs->num)); 219 220 vnd_codecs = (void *)skb->data; 221 222 /* check for payload data length before accessing */ 223 if (skb->len < 224 flex_array_size(vnd_codecs, codec, vnd_codecs->num) 225 + sizeof(vnd_codecs->num)) 226 goto error; 227 228 for (i = 0; i < vnd_codecs->num; i++) { 229 caps.id = 0xFF; 230 caps.cid = vnd_codecs->codec[i].cid; 231 caps.vid = vnd_codecs->codec[i].vid; 232 hci_read_codec_capabilities(hdev, vnd_codecs->codec[i].transport, 233 &caps); 234 } 235 236 error: 237 kfree_skb(skb); 238 } 239