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 = 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 }