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         vq_callback_t *callbacks[VIRTBT_NUM_VQS] = {
 258                 [VIRTBT_VQ_TX] = virtbt_tx_done,
 259                 [VIRTBT_VQ_RX] = virtbt_rx_done,
 260         };
 261         const char *names[VIRTBT_NUM_VQS] = {
 262                 [VIRTBT_VQ_TX] = "tx",
 263                 [VIRTBT_VQ_RX] = "rx",
 264         };
 265         struct virtio_bluetooth *vbt;
 266         struct hci_dev *hdev;
 267         int err;
 268         __u8 type;
 269
 270         if (!virtio_has_feature(vdev, VIRTIO_F_VERSION_1))
 271                 return -ENODEV;
 272
 273         type = virtio_cread8(vdev, offsetof(struct virtio_bt_config, type));
 274
 275         switch (type) {
 276         case VIRTIO_BT_CONFIG_TYPE_PRIMARY:
 277                 break;
 278         default:
 279                 return -EINVAL;
 280         }
 281
 282         vbt = kzalloc(sizeof(*vbt), GFP_KERNEL);
 283         if (!vbt)
 284                 return -ENOMEM;
 285
 286         vdev->priv = vbt;
 287         vbt->vdev = vdev;
 288
 289         INIT_WORK(&vbt->rx, virtbt_rx_work);
 290
 291         err = virtio_find_vqs(vdev, VIRTBT_NUM_VQS, vbt->vqs, callbacks,
 292                               names, NULL);
 293         if (err)
 294                 return err;
 295
 296         hdev = hci_alloc_dev();
 297         if (!hdev) {
 298                 err = -ENOMEM;
 299                 goto failed;
 300         }
 301
 302         vbt->hdev = hdev;
 303
 304         hdev->bus = HCI_VIRTIO;
 305         hci_set_drvdata(hdev, vbt);
 306
 307         hdev->open  = virtbt_open;
 308         hdev->close = virtbt_close;
 309         hdev->flush = virtbt_flush;
 310         hdev->send  = virtbt_send_frame;
 311
 312         if (virtio_has_feature(vdev, VIRTIO_BT_F_VND_HCI)) {
 313                 __u16 vendor;
 314
 315                 if (virtio_has_feature(vdev, VIRTIO_BT_F_CONFIG_V2))
 316                         virtio_cread(vdev, struct virtio_bt_config_v2,
 317                                      vendor, &vendor);
 318                 else
 319                         virtio_cread(vdev, struct virtio_bt_config,
 320                                      vendor, &vendor);
 321
 322                 switch (vendor) {
 323                 case VIRTIO_BT_CONFIG_VENDOR_ZEPHYR:
 324                         hdev->manufacturer = 1521;
 325                         hdev->setup = virtbt_setup_zephyr;
 326                         hdev->shutdown = virtbt_shutdown_generic;
 327                         hdev->set_bdaddr = virtbt_set_bdaddr_zephyr;
 328                         break;
 329
 330                 case VIRTIO_BT_CONFIG_VENDOR_INTEL:
 331                         hdev->manufacturer = 2;
 332                         hdev->setup = virtbt_setup_intel;
 333                         hdev->shutdown = virtbt_shutdown_generic;
 334                         hdev->set_bdaddr = virtbt_set_bdaddr_intel;
 335                         set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
 336                         set_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks);
 337                         set_bit(HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED, &hdev->quirks);
 338                         break;
 339
 340                 case VIRTIO_BT_CONFIG_VENDOR_REALTEK:
 341                         hdev->manufacturer = 93;
 342                         hdev->setup = virtbt_setup_realtek;
 343                         hdev->shutdown = virtbt_shutdown_generic;
 344                         set_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks);
 345                         set_bit(HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED, &hdev->quirks);
 346                         break;
 347                 }
 348         }
 349
 350         if (virtio_has_feature(vdev, VIRTIO_BT_F_MSFT_EXT)) {
 351                 __u16 msft_opcode;
 352
 353                 if (virtio_has_feature(vdev, VIRTIO_BT_F_CONFIG_V2))
 354                         virtio_cread(vdev, struct virtio_bt_config_v2,
 355                                      msft_opcode, &msft_opcode);
 356                 else
 357                         virtio_cread(vdev, struct virtio_bt_config,
 358                                      msft_opcode, &msft_opcode);
 359
 360                 hci_set_msft_opcode(hdev, msft_opcode);
 361         }
 362
 363         if (virtio_has_feature(vdev, VIRTIO_BT_F_AOSP_EXT))
 364                 hci_set_aosp_capable(hdev);
 365
 366         if (hci_register_dev(hdev) < 0) {
 367                 hci_free_dev(hdev);
 368                 err = -EBUSY;
 369                 goto failed;
 370         }
 371
 372         virtio_device_ready(vdev);
 373         err = virtbt_open_vdev(vbt);
 374         if (err)
 375                 goto open_failed;
 376
 377         return 0;
 378
 379 open_failed:
 380         hci_free_dev(hdev);
 381 failed:
 382         vdev->config->del_vqs(vdev);
 383         return err;
 384 }
 385
 386 static void virtbt_remove(struct virtio_device *vdev)
 387 {
 388         struct virtio_bluetooth *vbt = vdev->priv;
 389         struct hci_dev *hdev = vbt->hdev;
 390
 391         hci_unregister_dev(hdev);
 392         virtio_reset_device(vdev);
 393         virtbt_close_vdev(vbt);
 394
 395         hci_free_dev(hdev);
 396         vbt->hdev = NULL;
 397
 398         vdev->config->del_vqs(vdev);
 399         kfree(vbt);
 400 }
 401
 402 static struct virtio_device_id virtbt_table[] = {
 403         { VIRTIO_ID_BT, VIRTIO_DEV_ANY_ID },
 404         { 0 },
 405 };
 406
 407 MODULE_DEVICE_TABLE(virtio, virtbt_table);
 408
 409 static const unsigned int virtbt_features[] = {
 410         VIRTIO_BT_F_VND_HCI,
 411         VIRTIO_BT_F_MSFT_EXT,
 412         VIRTIO_BT_F_AOSP_EXT,
 413         VIRTIO_BT_F_CONFIG_V2,
 414 };
 415
 416 static struct virtio_driver virtbt_driver = {
 417         .driver.name         = KBUILD_MODNAME,
 418         .driver.owner        = THIS_MODULE,
 419         .feature_table       = virtbt_features,
 420         .feature_table_size  = ARRAY_SIZE(virtbt_features),
 421         .id_table            = virtbt_table,
 422         .probe               = virtbt_probe,
 423         .remove              = virtbt_remove,
 424 };
 425
 426 module_virtio_driver(virtbt_driver);
 427
 428 MODULE_AUTHOR("Marcel Holtmann <[email protected]>");
 429 MODULE_DESCRIPTION("Generic Bluetooth VIRTIO driver ver " VERSION);
 430 MODULE_VERSION(VERSION);
 431 MODULE_LICENSE("GPL");