net/bluetooth/hci_codec.c

   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 }