426 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2 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 16enum {17 VIRTBT_VQ_TX,18 VIRTBT_VQ_RX,19 VIRTBT_NUM_VQS,20};21 22struct 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 29static 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 51static int virtbt_open(struct hci_dev *hdev)52{53 return 0;54}55 56static 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 65static int virtbt_close(struct hci_dev *hdev)66{67 return 0;68}69 70static 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 88static int virtbt_flush(struct hci_dev *hdev)89{90 return 0;91}92 93static 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 113static 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 130static 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 144static 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 157static 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 170static 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 185static 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 198static 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 219static 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 239static 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 248static void virtbt_rx_done(struct virtqueue *vq)249{250 struct virtio_bluetooth *vbt = vq->vdev->priv;251 252 schedule_work(&vbt->rx);253}254 255static 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 else314 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 else352 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 374open_failed:375 hci_free_dev(hdev);376failed:377 vdev->config->del_vqs(vdev);378 return err;379}380 381static 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 397static struct virtio_device_id virtbt_table[] = {398 { VIRTIO_ID_BT, VIRTIO_DEV_ANY_ID },399 { 0 },400};401 402MODULE_DEVICE_TABLE(virtio, virtbt_table);403 404static 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 411static 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 420module_virtio_driver(virtbt_driver);421 422MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");423MODULE_DESCRIPTION("Generic Bluetooth VIRTIO driver ver " VERSION);424MODULE_VERSION(VERSION);425MODULE_LICENSE("GPL");426