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(size: 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, num: 1, data: 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(vq: 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(work: &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(vq: vbt->vqs[VIRTBT_VQ_TX], sg, num: 1, data: skb, |
103 | GFP_KERNEL); |
104 | if (err) { |
105 | kfree_skb(skb); |
106 | return err; |
107 | } |
108 | |
109 | virtqueue_kick(vq: 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, opcode: 0xfc08, plen: 0, NULL, HCI_INIT_TIMEOUT); |
119 | if (IS_ERR(ptr: skb)) |
120 | return PTR_ERR(ptr: skb); |
121 | |
122 | bt_dev_info(hdev, "%s" , (char *)(skb->data + 1)); |
123 | |
124 | hci_set_fw_info(hdev, fmt: "%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, opcode: 0xfc06, plen: 6, param: bdaddr, HCI_INIT_TIMEOUT); |
137 | if (IS_ERR(ptr: skb)) |
138 | return PTR_ERR(ptr: 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, opcode: 0xfc05, plen: 0, NULL, HCI_CMD_TIMEOUT); |
150 | if (IS_ERR(ptr: skb)) |
151 | return PTR_ERR(ptr: 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, opcode: 0xfc31, plen: 6, param: bdaddr, HCI_INIT_TIMEOUT); |
163 | if (IS_ERR(ptr: skb)) |
164 | return PTR_ERR(ptr: 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, opcode: 0xfc6d, plen: 0, NULL, HCI_INIT_TIMEOUT); |
176 | if (IS_ERR(ptr: skb)) |
177 | return PTR_ERR(ptr: 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, plen: 0, NULL, HCI_INIT_TIMEOUT); |
191 | if (IS_ERR(ptr: skb)) |
192 | return PTR_ERR(ptr: 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, len: 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(hdev: 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(vq: vbt->vqs[VIRTBT_VQ_RX], len: &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(vq: 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: &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(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 | case VIRTIO_BT_CONFIG_TYPE_AMP: |
278 | break; |
279 | default: |
280 | return -EINVAL; |
281 | } |
282 | |
283 | vbt = kzalloc(size: sizeof(*vbt), GFP_KERNEL); |
284 | if (!vbt) |
285 | return -ENOMEM; |
286 | |
287 | vdev->priv = vbt; |
288 | vbt->vdev = vdev; |
289 | |
290 | INIT_WORK(&vbt->rx, virtbt_rx_work); |
291 | |
292 | err = virtio_find_vqs(vdev, nvqs: VIRTBT_NUM_VQS, vqs: vbt->vqs, callbacks, |
293 | names, NULL); |
294 | if (err) |
295 | return err; |
296 | |
297 | hdev = hci_alloc_dev(); |
298 | if (!hdev) { |
299 | err = -ENOMEM; |
300 | goto failed; |
301 | } |
302 | |
303 | vbt->hdev = hdev; |
304 | |
305 | hdev->bus = HCI_VIRTIO; |
306 | hdev->dev_type = type; |
307 | hci_set_drvdata(hdev, data: vbt); |
308 | |
309 | hdev->open = virtbt_open; |
310 | hdev->close = virtbt_close; |
311 | hdev->flush = virtbt_flush; |
312 | hdev->send = virtbt_send_frame; |
313 | |
314 | if (virtio_has_feature(vdev, VIRTIO_BT_F_VND_HCI)) { |
315 | __u16 vendor; |
316 | |
317 | if (virtio_has_feature(vdev, VIRTIO_BT_F_CONFIG_V2)) |
318 | virtio_cread(vdev, struct virtio_bt_config_v2, |
319 | vendor, &vendor); |
320 | else |
321 | virtio_cread(vdev, struct virtio_bt_config, |
322 | vendor, &vendor); |
323 | |
324 | switch (vendor) { |
325 | case VIRTIO_BT_CONFIG_VENDOR_ZEPHYR: |
326 | hdev->manufacturer = 1521; |
327 | hdev->setup = virtbt_setup_zephyr; |
328 | hdev->shutdown = virtbt_shutdown_generic; |
329 | hdev->set_bdaddr = virtbt_set_bdaddr_zephyr; |
330 | break; |
331 | |
332 | case VIRTIO_BT_CONFIG_VENDOR_INTEL: |
333 | hdev->manufacturer = 2; |
334 | hdev->setup = virtbt_setup_intel; |
335 | hdev->shutdown = virtbt_shutdown_generic; |
336 | hdev->set_bdaddr = virtbt_set_bdaddr_intel; |
337 | set_bit(nr: HCI_QUIRK_STRICT_DUPLICATE_FILTER, addr: &hdev->quirks); |
338 | set_bit(nr: HCI_QUIRK_SIMULTANEOUS_DISCOVERY, addr: &hdev->quirks); |
339 | set_bit(nr: HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED, addr: &hdev->quirks); |
340 | break; |
341 | |
342 | case VIRTIO_BT_CONFIG_VENDOR_REALTEK: |
343 | hdev->manufacturer = 93; |
344 | hdev->setup = virtbt_setup_realtek; |
345 | hdev->shutdown = virtbt_shutdown_generic; |
346 | set_bit(nr: HCI_QUIRK_SIMULTANEOUS_DISCOVERY, addr: &hdev->quirks); |
347 | set_bit(nr: HCI_QUIRK_WIDEBAND_SPEECH_SUPPORTED, addr: &hdev->quirks); |
348 | break; |
349 | } |
350 | } |
351 | |
352 | if (virtio_has_feature(vdev, VIRTIO_BT_F_MSFT_EXT)) { |
353 | __u16 msft_opcode; |
354 | |
355 | if (virtio_has_feature(vdev, VIRTIO_BT_F_CONFIG_V2)) |
356 | virtio_cread(vdev, struct virtio_bt_config_v2, |
357 | msft_opcode, &msft_opcode); |
358 | else |
359 | virtio_cread(vdev, struct virtio_bt_config, |
360 | msft_opcode, &msft_opcode); |
361 | |
362 | hci_set_msft_opcode(hdev, opcode: msft_opcode); |
363 | } |
364 | |
365 | if (virtio_has_feature(vdev, VIRTIO_BT_F_AOSP_EXT)) |
366 | hci_set_aosp_capable(hdev); |
367 | |
368 | if (hci_register_dev(hdev) < 0) { |
369 | hci_free_dev(hdev); |
370 | err = -EBUSY; |
371 | goto failed; |
372 | } |
373 | |
374 | virtio_device_ready(dev: vdev); |
375 | err = virtbt_open_vdev(vbt); |
376 | if (err) |
377 | goto open_failed; |
378 | |
379 | return 0; |
380 | |
381 | open_failed: |
382 | hci_free_dev(hdev); |
383 | failed: |
384 | vdev->config->del_vqs(vdev); |
385 | return err; |
386 | } |
387 | |
388 | static void virtbt_remove(struct virtio_device *vdev) |
389 | { |
390 | struct virtio_bluetooth *vbt = vdev->priv; |
391 | struct hci_dev *hdev = vbt->hdev; |
392 | |
393 | hci_unregister_dev(hdev); |
394 | virtio_reset_device(dev: vdev); |
395 | virtbt_close_vdev(vbt); |
396 | |
397 | hci_free_dev(hdev); |
398 | vbt->hdev = NULL; |
399 | |
400 | vdev->config->del_vqs(vdev); |
401 | kfree(objp: vbt); |
402 | } |
403 | |
404 | static struct virtio_device_id virtbt_table[] = { |
405 | { VIRTIO_ID_BT, VIRTIO_DEV_ANY_ID }, |
406 | { 0 }, |
407 | }; |
408 | |
409 | MODULE_DEVICE_TABLE(virtio, virtbt_table); |
410 | |
411 | static const unsigned int virtbt_features[] = { |
412 | VIRTIO_BT_F_VND_HCI, |
413 | VIRTIO_BT_F_MSFT_EXT, |
414 | VIRTIO_BT_F_AOSP_EXT, |
415 | VIRTIO_BT_F_CONFIG_V2, |
416 | }; |
417 | |
418 | static struct virtio_driver virtbt_driver = { |
419 | .driver.name = KBUILD_MODNAME, |
420 | .driver.owner = THIS_MODULE, |
421 | .feature_table = virtbt_features, |
422 | .feature_table_size = ARRAY_SIZE(virtbt_features), |
423 | .id_table = virtbt_table, |
424 | .probe = virtbt_probe, |
425 | .remove = virtbt_remove, |
426 | }; |
427 | |
428 | module_virtio_driver(virtbt_driver); |
429 | |
430 | MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>" ); |
431 | MODULE_DESCRIPTION("Generic Bluetooth VIRTIO driver ver " VERSION); |
432 | MODULE_VERSION(VERSION); |
433 | MODULE_LICENSE("GPL" ); |
434 | |