drivers/bluetooth/virtio_bt.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2
   3 #include <linux/module.h>
   4 #include <linux/virtio.h>
   5 #include <linux/virtio_config.h>
   6 #include <linux/skbuff.h>
   7
   8 #include <uapi/linux/virtio_ids.h>
   9 #include <uapi/linux/virtio_bt.h>
  10
  11 #include <net/bluetooth/bluetooth.h>
  12 #include <net/bluetooth/hci_core.h>
  13
  14 #define VERSION "0.1"
  15
  16 enum {
  17         VIRTBT_VQ_TX,
  18         VIRTBT_VQ_RX,
  19         VIRTBT_NUM_VQS,
  20 };
  21
  22 struct virtio_bluetooth {
  23         struct virtio_device *vdev;
  24         struct virtqueue *vqs[VIRTBT_NUM_VQS];
  25         struct work_struct rx;
  26         struct hci_dev *hdev;
  27 };
  28
  29 static int virtbt_add_inbuf(struct virtio_bluetooth *vbt)
  30 {
  31         struct virtqueue *vq = vbt->vqs[VIRTBT_VQ_RX];
  32         struct scatterlist sg[1];
  33         struct sk_buff *skb;
  34         int err;
  35
  36         skb = alloc_skb(1000, GFP_KERNEL);
  37         if (!skb)
  38                 return -ENOMEM;
  39
  40         sg_init_one(sg, skb->data, 1000);
  41
  42         err = virtqueue_add_inbuf(vq, sg, 1, skb, GFP_KERNEL);
  43         if (err < 0) {
  44                 kfree_skb(skb);
  45                 return err;
  46         }
  47
  48         return 0;
  49 }
  50
  51 static int virtbt_open(struct hci_dev *hdev)
  52 {
  53         return 0;
  54 }
  55
  56 static int virtbt_open_vdev(struct virtio_bluetooth *vbt)
  57 {
  58         if (virtbt_add_inbuf(vbt) < 0)
  59                 return -EIO;
  60
  61         virtqueue_kick(vbt->vqs[VIRTBT_VQ_RX]);
  62         return 0;
  63 }
  64
  65 static int virtbt_close(struct hci_dev *hdev)
  66 {
  67         return 0;
  68 }
  69
  70 static int virtbt_close_vdev(struct virtio_bluetooth *vbt)
  71 {
  72         int i;
  73
  74         cancel_work_sync(&vbt->rx);
  75
  76         for (i = 0; i < ARRAY_SIZE(vbt->vqs); i++) {
  77                 struct virtqueue *vq = vbt->vqs[i];
  78                 struct sk_buff *skb;
  79
  80                 while ((skb = virtqueue_detach_unused_buf(vq)))
  81                         kfree_skb(skb);
  82                 cond_resched();
  83         }
  84
  85         return 0;
  86 }
  87
  88 static int virtbt_flush(struct hci_dev *hdev)
  89 {
  90         return 0;
  91 }
  92
  93 static int virtbt_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
  94 {
  95         struct virtio_bluetooth *vbt = hci_get_drvdata(hdev);
  96         struct scatterlist sg[1];
  97         int err;
  98
  99         memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 100
 101         sg_init_one(sg, skb->data, skb->len);
 102         err = virtqueue_add_outbuf(vbt->vqs[VIRTBT_VQ_TX], sg, 1, skb,
 103                                    GFP_KERNEL);
 104         if (err) {
 105                 kfree_skb(skb);
 106                 return err;
 107         }
 108
 109         virtqueue_kick(vbt->vqs[VIRTBT_VQ_TX]);
 110         return 0;
 111 }
 112
 113 static int virtbt_setup_zephyr(struct hci_dev *hdev)
 114 {
 115         struct sk_buff *skb;
 116
 117         /* Read Build Information */
 118         skb = __hci_cmd_sync(hdev, 0xfc08, 0, NULL, HCI_INIT_TIMEOUT);
 119         if (IS_ERR(skb))
 120                 return PTR_ERR(skb);
 121
 122         bt_dev_info(hdev, "%s", (char *)(skb->data + 1));
 123
 124         hci_set_fw_info(hdev, "%s", skb->data + 1);
 125
 126         kfree_skb(skb);
 127         return 0;
 128 }
 129
 130 static int virtbt_set_bdaddr_zephyr(struct hci_dev *hdev,
 131                                     const bdaddr_t *bdaddr)
 132 {
 133         struct sk_buff *skb;
 134
 135         /* Write BD_ADDR */
 136         skb = __hci_cmd_sync(hdev, 0xfc06, 6, bdaddr, HCI_INIT_TIMEOUT);
 137         if (IS_ERR(skb))
 138                 return PTR_ERR(skb);
 139
 140         kfree_skb(skb);
 141         return 0;
 142 }
 143
 144 static int virtbt_setup_intel(struct hci_dev *hdev)
 145 {
 146         struct sk_buff *skb;
 147
 148         /* Intel Read Version */
 149         skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_CMD_TIMEOUT);
 150         if (IS_ERR(skb))
 151                 return PTR_ERR(skb);
 152
 153         kfree_skb(skb);
 154         return 0;
 155 }
 156
 157 static int virtbt_set_bdaddr_intel(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 158 {
 159         struct sk_buff *skb;
 160
 161         /* Intel Write BD Address */
 162         skb = __hci_cmd_sync(hdev, 0xfc31, 6, bdaddr, HCI_INIT_TIMEOUT);
 163         if (IS_ERR(skb))
 164                 return PTR_ERR(skb);
 165
 166         kfree_skb(skb);
 167         return 0;
 168 }
 169
 170 static int virtbt_setup_realtek(struct hci_dev *hdev)
 171 {
 172         struct sk_buff *skb;
 173
 174         /* Read ROM Version */
 175         skb = __hci_cmd_sync(hdev, 0xfc6d, 0, NULL, HCI_INIT_TIMEOUT);
 176         if (IS_ERR(skb))
 177                 return PTR_ERR(skb);
 178
 179         bt_dev_info(hdev, "ROM version %u", *((__u8 *) (skb->data + 1)));
 180
 181         kfree_skb(skb);
 182         return 0;
 183 }
 184
 185 static int virtbt_shutdown_generic(struct hci_dev *hdev)
 186 {
 187         struct sk_buff *skb;
 188
 189         /* Reset */
 190         skb = __hci_cmd_sync(hdev, HCI_OP_RESET, 0, NULL, HCI_INIT_TIMEOUT);
 191         if (IS_ERR(skb))
 192                 return PTR_ERR(skb);
 193
 194         kfree_skb(skb);
 195         return 0;
 196 }
 197
 198 static void virtbt_rx_handle(struct virtio_bluetooth *vbt, struct sk_buff *skb)
 199 {
 200         __u8 pkt_type;
 201
 202         pkt_type = *((__u8 *) skb->data);
 203         skb_pull(skb, 1);
 204
 205         switch (pkt_type) {
 206         case HCI_EVENT_PKT:
 207         case HCI_ACLDATA_PKT:
 208         case HCI_SCODATA_PKT:
 209         case HCI_ISODATA_PKT:
 210                 hci_skb_pkt_type(skb) = pkt_type;
 211                 hci_recv_frame(vbt->hdev, skb);
 212                 break;
 213         default:
 214                 kfree_skb(skb);
 215                 break;
 216         }
 217 }
 218
 219 static void virtbt_rx_work(struct work_struct *work)
 220 {
 221         struct virtio_bluetooth *vbt = container_of(work,
 222                                                     struct virtio_bluetooth, rx);
 223         struct sk_buff *skb;
 224         unsigned int len;
 225
 226         skb = virtqueue_get_buf(vbt->vqs[VIRTBT_VQ_RX], &len);
 227         if (!skb)
 228                 return;
 229
 230         skb_put(skb, len);
 231         virtbt_rx_handle(vbt, skb);
 232
 233         if (virtbt_add_inbuf(vbt) < 0)
 234                 return;
 235
 236         virtqueue_kick(vbt->vqs[VIRTBT_VQ_RX]);
 237 }
 238
 239 static void virtbt_tx_done(struct virtqueue *vq)
 240 {
 241         struct sk_buff *skb;
 242         unsigned int len;
 243
 244         while ((skb = virtqueue_get_buf(vq, &len)))
 245                 kfree_skb(skb);
 246 }
 247
 248 static void virtbt_rx_done(struct virtqueue *vq)
 249 {
 250         struct virtio_bluetooth *vbt = vq->vdev->priv;
 251
 252         schedule_work(&vbt->rx);
 253 }
 254
 255 static int virtbt_probe(struct virtio_device *vdev)
 256 {
 257         struct virtqueue_info vqs_info[VIRTBT_NUM_VQS] = {
 258                 [VIRTBT_VQ_TX] = { "tx", virtbt_tx_done },
 259                 [VIRTBT_VQ_RX] = { "rx", virtbt_rx_done },
 260         };
 261         struct virtio_bluetooth *vbt;
 262         struct hci_dev *hdev;
 263         int err;
 264         __u8 type;
 265
 266         if (!virtio_has_feature(vdev, VIRTIO_F_VERSION_1))
 267                 return -ENODEV;
 268
 269         type = virtio_cread8(vdev, offsetof(struct virtio_bt_config, type));
 270
 271         switch (type) {
 272         case VIRTIO_BT_CONFIG_TYPE_PRIMARY:
 273                 break;
 274         default:
 275                 return -EINVAL;
 276         }
 277
 278         vbt = kzalloc(sizeof(*vbt), GFP_KERNEL);
 279         if (!vbt)
 280                 return -ENOMEM;
 281
 282         vdev->priv = vbt;
 283         vbt->vdev = vdev;
 284
 285         INIT_WORK(&vbt->rx, virtbt_rx_work);
 286
 287         err = virtio_find_vqs(vdev, VIRTBT_NUM_VQS, vbt->vqs, vqs_info, NULL);
 288         if (err)
 289                 return err;
 290
 291         hdev = hci_alloc_dev();
 292         if (!hdev) {
 293                 err = -ENOMEM;
 294                 goto failed;
 295         }
 296
 297         vbt->hdev = hdev;
 298
 299         hdev->bus = HCI_VIRTIO;
 300         hci_set_drvdata(hdev, vbt);
 301
 302         hdev->open  = virtbt_open;
 303         hdev->close = virtbt_close;
 304         hdev->flush = virtbt_flush;
 305         hdev->send  = virtbt_send_frame;
 306
 307         if (virtio_has_feature(vdev, VIRTIO_BT_F_VND_HCI)) {
 308                 __u16 vendor;
 309
 310                 if (virtio_has_feature(vdev, VIRTIO_BT_F_CONFIG_V2))
 311                         virtio_cread(vdev, struct virtio_bt_config_v2,
 312                                      vendor, &vendor);
 313                 else
 314                         virtio_cread(vdev, struct virtio_bt_config,
 315                                      vendor, &vendor);
 316
 317                 switch (vendor) {
 318                 case VIRTIO_BT_CONFIG_VENDOR_ZEPHYR:
 319                         hdev->manufacturer = 1521;
 320                         hdev->setup = virtbt_setup_zephyr;
 321                         hdev->shutdown = virtbt_shutdown_generic;
 322                         hdev->set_bdaddr = virtbt_set_bdaddr_zephyr;
 323                         break;
 324
 325                 case VIRTIO_BT_CONFIG_VENDOR_INTEL:
 326                         hdev->manufacturer = 2;
 327                         hdev->setup = virtbt_setup_intel;
 328                         hdev->shutdown = virtbt_shutdown_generic;
 329                         hdev->set_bdaddr = virtbt_set_bdaddr_intel;
 330                         set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
 331                         set_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks);
 332                         set_bit(HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED, &hdev->quirks);
 333                         break;
 334
 335                 case VIRTIO_BT_CONFIG_VENDOR_REALTEK:
 336                         hdev->manufacturer = 93;
 337                         hdev->setup = virtbt_setup_realtek;
 338                         hdev->shutdown = virtbt_shutdown_generic;
 339                         set_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks);
 340                         set_bit(HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED, &hdev->quirks);
 341                         break;
 342                 }
 343         }
 344
 345         if (virtio_has_feature(vdev, VIRTIO_BT_F_MSFT_EXT)) {
 346                 __u16 msft_opcode;
 347
 348                 if (virtio_has_feature(vdev, VIRTIO_BT_F_CONFIG_V2))
 349                         virtio_cread(vdev, struct virtio_bt_config_v2,
 350                                      msft_opcode, &msft_opcode);
 351                 else
 352                         virtio_cread(vdev, struct virtio_bt_config,
 353                                      msft_opcode, &msft_opcode);
 354
 355                 hci_set_msft_opcode(hdev, msft_opcode);
 356         }
 357
 358         if (virtio_has_feature(vdev, VIRTIO_BT_F_AOSP_EXT))
 359                 hci_set_aosp_capable(hdev);
 360
 361         if (hci_register_dev(hdev) < 0) {
 362                 hci_free_dev(hdev);
 363                 err = -EBUSY;
 364                 goto failed;
 365         }
 366
 367         virtio_device_ready(vdev);
 368         err = virtbt_open_vdev(vbt);
 369         if (err)
 370                 goto open_failed;
 371
 372         return 0;
 373
 374 open_failed:
 375         hci_free_dev(hdev);
 376 failed:
 377         vdev->config->del_vqs(vdev);
 378         return err;
 379 }
 380
 381 static void virtbt_remove(struct virtio_device *vdev)
 382 {
 383         struct virtio_bluetooth *vbt = vdev->priv;
 384         struct hci_dev *hdev = vbt->hdev;
 385
 386         hci_unregister_dev(hdev);
 387         virtio_reset_device(vdev);
 388         virtbt_close_vdev(vbt);
 389
 390         hci_free_dev(hdev);
 391         vbt->hdev = NULL;
 392
 393         vdev->config->del_vqs(vdev);
 394         kfree(vbt);
 395 }
 396
 397 static struct virtio_device_id virtbt_table[] = {
 398         { VIRTIO_ID_BT, VIRTIO_DEV_ANY_ID },
 399         { 0 },
 400 };
 401
 402 MODULE_DEVICE_TABLE(virtio, virtbt_table);
 403
 404 static const unsigned int virtbt_features[] = {
 405         VIRTIO_BT_F_VND_HCI,
 406         VIRTIO_BT_F_MSFT_EXT,
 407         VIRTIO_BT_F_AOSP_EXT,
 408         VIRTIO_BT_F_CONFIG_V2,
 409 };
 410
 411 static struct virtio_driver virtbt_driver = {
 412         .driver.name         = KBUILD_MODNAME,
 413         .feature_table       = virtbt_features,
 414         .feature_table_size  = ARRAY_SIZE(virtbt_features),
 415         .id_table            = virtbt_table,
 416         .probe               = virtbt_probe,
 417         .remove              = virtbt_remove,
 418 };
 419
 420 module_virtio_driver(virtbt_driver);
 421
 422 MODULE_AUTHOR("Marcel Holtmann <[email protected]>");
 423 MODULE_DESCRIPTION("Generic Bluetooth VIRTIO driver ver " VERSION);
 424 MODULE_VERSION(VERSION);
 425 MODULE_LICENSE("GPL");