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