1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * VIRTIO based driver for vDPA device |
4 | * |
5 | * Copyright (c) 2020, Red Hat. All rights reserved. |
6 | * Author: Jason Wang <jasowang@redhat.com> |
7 | * |
8 | */ |
9 | |
10 | #include <linux/init.h> |
11 | #include <linux/module.h> |
12 | #include <linux/device.h> |
13 | #include <linux/kernel.h> |
14 | #include <linux/slab.h> |
15 | #include <linux/uuid.h> |
16 | #include <linux/group_cpus.h> |
17 | #include <linux/virtio.h> |
18 | #include <linux/vdpa.h> |
19 | #include <linux/virtio_config.h> |
20 | #include <linux/virtio_ring.h> |
21 | |
22 | #define MOD_VERSION "0.1" |
23 | #define MOD_AUTHOR "Jason Wang <jasowang@redhat.com>" |
24 | #define MOD_DESC "vDPA bus driver for virtio devices" |
25 | #define MOD_LICENSE "GPL v2" |
26 | |
27 | struct virtio_vdpa_device { |
28 | struct virtio_device vdev; |
29 | struct vdpa_device *vdpa; |
30 | u64 features; |
31 | |
32 | /* The lock to protect virtqueue list */ |
33 | spinlock_t lock; |
34 | /* List of virtio_vdpa_vq_info */ |
35 | struct list_head virtqueues; |
36 | }; |
37 | |
38 | struct virtio_vdpa_vq_info { |
39 | /* the actual virtqueue */ |
40 | struct virtqueue *vq; |
41 | |
42 | /* the list node for the virtqueues list */ |
43 | struct list_head node; |
44 | }; |
45 | |
46 | static inline struct virtio_vdpa_device * |
47 | to_virtio_vdpa_device(struct virtio_device *dev) |
48 | { |
49 | return container_of(dev, struct virtio_vdpa_device, vdev); |
50 | } |
51 | |
52 | static struct vdpa_device *vd_get_vdpa(struct virtio_device *vdev) |
53 | { |
54 | return to_virtio_vdpa_device(dev: vdev)->vdpa; |
55 | } |
56 | |
57 | static void virtio_vdpa_get(struct virtio_device *vdev, unsigned int offset, |
58 | void *buf, unsigned int len) |
59 | { |
60 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
61 | |
62 | vdpa_get_config(vdev: vdpa, offset, buf, len); |
63 | } |
64 | |
65 | static void virtio_vdpa_set(struct virtio_device *vdev, unsigned int offset, |
66 | const void *buf, unsigned int len) |
67 | { |
68 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
69 | |
70 | vdpa_set_config(dev: vdpa, offset, buf, length: len); |
71 | } |
72 | |
73 | static u32 virtio_vdpa_generation(struct virtio_device *vdev) |
74 | { |
75 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
76 | const struct vdpa_config_ops *ops = vdpa->config; |
77 | |
78 | if (ops->get_generation) |
79 | return ops->get_generation(vdpa); |
80 | |
81 | return 0; |
82 | } |
83 | |
84 | static u8 virtio_vdpa_get_status(struct virtio_device *vdev) |
85 | { |
86 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
87 | const struct vdpa_config_ops *ops = vdpa->config; |
88 | |
89 | return ops->get_status(vdpa); |
90 | } |
91 | |
92 | static void virtio_vdpa_set_status(struct virtio_device *vdev, u8 status) |
93 | { |
94 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
95 | |
96 | return vdpa_set_status(vdev: vdpa, status); |
97 | } |
98 | |
99 | static void virtio_vdpa_reset(struct virtio_device *vdev) |
100 | { |
101 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
102 | |
103 | vdpa_reset(vdev: vdpa, flags: 0); |
104 | } |
105 | |
106 | static bool virtio_vdpa_notify(struct virtqueue *vq) |
107 | { |
108 | struct vdpa_device *vdpa = vd_get_vdpa(vdev: vq->vdev); |
109 | const struct vdpa_config_ops *ops = vdpa->config; |
110 | |
111 | ops->kick_vq(vdpa, vq->index); |
112 | |
113 | return true; |
114 | } |
115 | |
116 | static bool virtio_vdpa_notify_with_data(struct virtqueue *vq) |
117 | { |
118 | struct vdpa_device *vdpa = vd_get_vdpa(vdev: vq->vdev); |
119 | const struct vdpa_config_ops *ops = vdpa->config; |
120 | u32 data = vring_notification_data(vq: vq); |
121 | |
122 | ops->kick_vq_with_data(vdpa, data); |
123 | |
124 | return true; |
125 | } |
126 | |
127 | static irqreturn_t virtio_vdpa_config_cb(void *private) |
128 | { |
129 | struct virtio_vdpa_device *vd_dev = private; |
130 | |
131 | virtio_config_changed(dev: &vd_dev->vdev); |
132 | |
133 | return IRQ_HANDLED; |
134 | } |
135 | |
136 | static irqreturn_t virtio_vdpa_virtqueue_cb(void *private) |
137 | { |
138 | struct virtio_vdpa_vq_info *info = private; |
139 | |
140 | return vring_interrupt(irq: 0, vq: info->vq); |
141 | } |
142 | |
143 | static struct virtqueue * |
144 | virtio_vdpa_setup_vq(struct virtio_device *vdev, unsigned int index, |
145 | void (*callback)(struct virtqueue *vq), |
146 | const char *name, bool ctx) |
147 | { |
148 | struct virtio_vdpa_device *vd_dev = to_virtio_vdpa_device(dev: vdev); |
149 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
150 | struct device *dma_dev; |
151 | const struct vdpa_config_ops *ops = vdpa->config; |
152 | struct virtio_vdpa_vq_info *info; |
153 | bool (*notify)(struct virtqueue *vq) = virtio_vdpa_notify; |
154 | struct vdpa_callback cb; |
155 | struct virtqueue *vq; |
156 | u64 desc_addr, driver_addr, device_addr; |
157 | /* Assume split virtqueue, switch to packed if necessary */ |
158 | struct vdpa_vq_state state = {0}; |
159 | unsigned long flags; |
160 | u32 align, max_num, min_num = 1; |
161 | bool may_reduce_num = true; |
162 | int err; |
163 | |
164 | if (!name) |
165 | return NULL; |
166 | |
167 | if (index >= vdpa->nvqs) |
168 | return ERR_PTR(error: -ENOENT); |
169 | |
170 | /* We cannot accept VIRTIO_F_NOTIFICATION_DATA without kick_vq_with_data */ |
171 | if (__virtio_test_bit(vdev, VIRTIO_F_NOTIFICATION_DATA)) { |
172 | if (ops->kick_vq_with_data) |
173 | notify = virtio_vdpa_notify_with_data; |
174 | else |
175 | __virtio_clear_bit(vdev, VIRTIO_F_NOTIFICATION_DATA); |
176 | } |
177 | |
178 | /* Queue shouldn't already be set up. */ |
179 | if (ops->get_vq_ready(vdpa, index)) |
180 | return ERR_PTR(error: -ENOENT); |
181 | |
182 | /* Allocate and fill out our active queue description */ |
183 | info = kmalloc(size: sizeof(*info), GFP_KERNEL); |
184 | if (!info) |
185 | return ERR_PTR(error: -ENOMEM); |
186 | |
187 | max_num = ops->get_vq_num_max(vdpa); |
188 | if (max_num == 0) { |
189 | err = -ENOENT; |
190 | goto error_new_virtqueue; |
191 | } |
192 | |
193 | if (ops->get_vq_num_min) |
194 | min_num = ops->get_vq_num_min(vdpa); |
195 | |
196 | may_reduce_num = (max_num == min_num) ? false : true; |
197 | |
198 | /* Create the vring */ |
199 | align = ops->get_vq_align(vdpa); |
200 | |
201 | if (ops->get_vq_dma_dev) |
202 | dma_dev = ops->get_vq_dma_dev(vdpa, index); |
203 | else |
204 | dma_dev = vdpa_get_dma_dev(vdev: vdpa); |
205 | vq = vring_create_virtqueue_dma(index, num: max_num, vring_align: align, vdev, |
206 | weak_barriers: true, may_reduce_num, ctx, |
207 | notify, callback, name, dma_dev); |
208 | if (!vq) { |
209 | err = -ENOMEM; |
210 | goto error_new_virtqueue; |
211 | } |
212 | |
213 | vq->num_max = max_num; |
214 | |
215 | /* Setup virtqueue callback */ |
216 | cb.callback = callback ? virtio_vdpa_virtqueue_cb : NULL; |
217 | cb.private = info; |
218 | cb.trigger = NULL; |
219 | ops->set_vq_cb(vdpa, index, &cb); |
220 | ops->set_vq_num(vdpa, index, virtqueue_get_vring_size(vq)); |
221 | |
222 | desc_addr = virtqueue_get_desc_addr(vq); |
223 | driver_addr = virtqueue_get_avail_addr(vq); |
224 | device_addr = virtqueue_get_used_addr(vq); |
225 | |
226 | if (ops->set_vq_address(vdpa, index, |
227 | desc_addr, driver_addr, |
228 | device_addr)) { |
229 | err = -EINVAL; |
230 | goto err_vq; |
231 | } |
232 | |
233 | /* reset virtqueue state index */ |
234 | if (virtio_has_feature(vdev, VIRTIO_F_RING_PACKED)) { |
235 | struct vdpa_vq_state_packed *s = &state.packed; |
236 | |
237 | s->last_avail_counter = 1; |
238 | s->last_avail_idx = 0; |
239 | s->last_used_counter = 1; |
240 | s->last_used_idx = 0; |
241 | } |
242 | err = ops->set_vq_state(vdpa, index, &state); |
243 | if (err) |
244 | goto err_vq; |
245 | |
246 | ops->set_vq_ready(vdpa, index, 1); |
247 | |
248 | vq->priv = info; |
249 | info->vq = vq; |
250 | |
251 | spin_lock_irqsave(&vd_dev->lock, flags); |
252 | list_add(new: &info->node, head: &vd_dev->virtqueues); |
253 | spin_unlock_irqrestore(lock: &vd_dev->lock, flags); |
254 | |
255 | return vq; |
256 | |
257 | err_vq: |
258 | vring_del_virtqueue(vq); |
259 | error_new_virtqueue: |
260 | ops->set_vq_ready(vdpa, index, 0); |
261 | /* VDPA driver should make sure vq is stopeed here */ |
262 | WARN_ON(ops->get_vq_ready(vdpa, index)); |
263 | kfree(objp: info); |
264 | return ERR_PTR(error: err); |
265 | } |
266 | |
267 | static void virtio_vdpa_del_vq(struct virtqueue *vq) |
268 | { |
269 | struct virtio_vdpa_device *vd_dev = to_virtio_vdpa_device(dev: vq->vdev); |
270 | struct vdpa_device *vdpa = vd_dev->vdpa; |
271 | const struct vdpa_config_ops *ops = vdpa->config; |
272 | struct virtio_vdpa_vq_info *info = vq->priv; |
273 | unsigned int index = vq->index; |
274 | unsigned long flags; |
275 | |
276 | spin_lock_irqsave(&vd_dev->lock, flags); |
277 | list_del(entry: &info->node); |
278 | spin_unlock_irqrestore(lock: &vd_dev->lock, flags); |
279 | |
280 | /* Select and deactivate the queue (best effort) */ |
281 | ops->set_vq_ready(vdpa, index, 0); |
282 | |
283 | vring_del_virtqueue(vq); |
284 | |
285 | kfree(objp: info); |
286 | } |
287 | |
288 | static void virtio_vdpa_del_vqs(struct virtio_device *vdev) |
289 | { |
290 | struct virtqueue *vq, *n; |
291 | |
292 | list_for_each_entry_safe(vq, n, &vdev->vqs, list) |
293 | virtio_vdpa_del_vq(vq); |
294 | } |
295 | |
296 | static void default_calc_sets(struct irq_affinity *affd, unsigned int affvecs) |
297 | { |
298 | affd->nr_sets = 1; |
299 | affd->set_size[0] = affvecs; |
300 | } |
301 | |
302 | static struct cpumask * |
303 | create_affinity_masks(unsigned int nvecs, struct irq_affinity *affd) |
304 | { |
305 | unsigned int affvecs = 0, curvec, usedvecs, i; |
306 | struct cpumask *masks = NULL; |
307 | |
308 | if (nvecs > affd->pre_vectors + affd->post_vectors) |
309 | affvecs = nvecs - affd->pre_vectors - affd->post_vectors; |
310 | |
311 | if (!affd->calc_sets) |
312 | affd->calc_sets = default_calc_sets; |
313 | |
314 | affd->calc_sets(affd, affvecs); |
315 | |
316 | if (!affvecs) |
317 | return NULL; |
318 | |
319 | masks = kcalloc(n: nvecs, size: sizeof(*masks), GFP_KERNEL); |
320 | if (!masks) |
321 | return NULL; |
322 | |
323 | /* Fill out vectors at the beginning that don't need affinity */ |
324 | for (curvec = 0; curvec < affd->pre_vectors; curvec++) |
325 | cpumask_setall(dstp: &masks[curvec]); |
326 | |
327 | for (i = 0, usedvecs = 0; i < affd->nr_sets; i++) { |
328 | unsigned int this_vecs = affd->set_size[i]; |
329 | int j; |
330 | struct cpumask *result = group_cpus_evenly(numgrps: this_vecs); |
331 | |
332 | if (!result) { |
333 | kfree(objp: masks); |
334 | return NULL; |
335 | } |
336 | |
337 | for (j = 0; j < this_vecs; j++) |
338 | cpumask_copy(dstp: &masks[curvec + j], srcp: &result[j]); |
339 | kfree(objp: result); |
340 | |
341 | curvec += this_vecs; |
342 | usedvecs += this_vecs; |
343 | } |
344 | |
345 | /* Fill out vectors at the end that don't need affinity */ |
346 | if (usedvecs >= affvecs) |
347 | curvec = affd->pre_vectors + affvecs; |
348 | else |
349 | curvec = affd->pre_vectors + usedvecs; |
350 | for (; curvec < nvecs; curvec++) |
351 | cpumask_setall(dstp: &masks[curvec]); |
352 | |
353 | return masks; |
354 | } |
355 | |
356 | static int virtio_vdpa_find_vqs(struct virtio_device *vdev, unsigned int nvqs, |
357 | struct virtqueue *vqs[], |
358 | vq_callback_t *callbacks[], |
359 | const char * const names[], |
360 | const bool *ctx, |
361 | struct irq_affinity *desc) |
362 | { |
363 | struct virtio_vdpa_device *vd_dev = to_virtio_vdpa_device(dev: vdev); |
364 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
365 | const struct vdpa_config_ops *ops = vdpa->config; |
366 | struct irq_affinity default_affd = { 0 }; |
367 | struct cpumask *masks; |
368 | struct vdpa_callback cb; |
369 | bool has_affinity = desc && ops->set_vq_affinity; |
370 | int i, err, queue_idx = 0; |
371 | |
372 | if (has_affinity) { |
373 | masks = create_affinity_masks(nvecs: nvqs, affd: desc ? desc : &default_affd); |
374 | if (!masks) |
375 | return -ENOMEM; |
376 | } |
377 | |
378 | for (i = 0; i < nvqs; ++i) { |
379 | if (!names[i]) { |
380 | vqs[i] = NULL; |
381 | continue; |
382 | } |
383 | |
384 | vqs[i] = virtio_vdpa_setup_vq(vdev, index: queue_idx++, |
385 | callback: callbacks[i], name: names[i], ctx: ctx ? |
386 | ctx[i] : false); |
387 | if (IS_ERR(ptr: vqs[i])) { |
388 | err = PTR_ERR(ptr: vqs[i]); |
389 | goto err_setup_vq; |
390 | } |
391 | |
392 | if (has_affinity) |
393 | ops->set_vq_affinity(vdpa, i, &masks[i]); |
394 | } |
395 | |
396 | cb.callback = virtio_vdpa_config_cb; |
397 | cb.private = vd_dev; |
398 | ops->set_config_cb(vdpa, &cb); |
399 | if (has_affinity) |
400 | kfree(objp: masks); |
401 | |
402 | return 0; |
403 | |
404 | err_setup_vq: |
405 | virtio_vdpa_del_vqs(vdev); |
406 | if (has_affinity) |
407 | kfree(objp: masks); |
408 | return err; |
409 | } |
410 | |
411 | static u64 virtio_vdpa_get_features(struct virtio_device *vdev) |
412 | { |
413 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
414 | const struct vdpa_config_ops *ops = vdpa->config; |
415 | |
416 | return ops->get_device_features(vdpa); |
417 | } |
418 | |
419 | static int virtio_vdpa_finalize_features(struct virtio_device *vdev) |
420 | { |
421 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
422 | |
423 | /* Give virtio_ring a chance to accept features. */ |
424 | vring_transport_features(vdev); |
425 | |
426 | return vdpa_set_features(vdev: vdpa, features: vdev->features); |
427 | } |
428 | |
429 | static const char *virtio_vdpa_bus_name(struct virtio_device *vdev) |
430 | { |
431 | struct virtio_vdpa_device *vd_dev = to_virtio_vdpa_device(dev: vdev); |
432 | struct vdpa_device *vdpa = vd_dev->vdpa; |
433 | |
434 | return dev_name(dev: &vdpa->dev); |
435 | } |
436 | |
437 | static int virtio_vdpa_set_vq_affinity(struct virtqueue *vq, |
438 | const struct cpumask *cpu_mask) |
439 | { |
440 | struct virtio_vdpa_device *vd_dev = to_virtio_vdpa_device(dev: vq->vdev); |
441 | struct vdpa_device *vdpa = vd_dev->vdpa; |
442 | const struct vdpa_config_ops *ops = vdpa->config; |
443 | unsigned int index = vq->index; |
444 | |
445 | if (ops->set_vq_affinity) |
446 | return ops->set_vq_affinity(vdpa, index, cpu_mask); |
447 | |
448 | return 0; |
449 | } |
450 | |
451 | static const struct cpumask * |
452 | virtio_vdpa_get_vq_affinity(struct virtio_device *vdev, int index) |
453 | { |
454 | struct vdpa_device *vdpa = vd_get_vdpa(vdev); |
455 | const struct vdpa_config_ops *ops = vdpa->config; |
456 | |
457 | if (ops->get_vq_affinity) |
458 | return ops->get_vq_affinity(vdpa, index); |
459 | |
460 | return NULL; |
461 | } |
462 | |
463 | static const struct virtio_config_ops virtio_vdpa_config_ops = { |
464 | .get = virtio_vdpa_get, |
465 | .set = virtio_vdpa_set, |
466 | .generation = virtio_vdpa_generation, |
467 | .get_status = virtio_vdpa_get_status, |
468 | .set_status = virtio_vdpa_set_status, |
469 | .reset = virtio_vdpa_reset, |
470 | .find_vqs = virtio_vdpa_find_vqs, |
471 | .del_vqs = virtio_vdpa_del_vqs, |
472 | .get_features = virtio_vdpa_get_features, |
473 | .finalize_features = virtio_vdpa_finalize_features, |
474 | .bus_name = virtio_vdpa_bus_name, |
475 | .set_vq_affinity = virtio_vdpa_set_vq_affinity, |
476 | .get_vq_affinity = virtio_vdpa_get_vq_affinity, |
477 | }; |
478 | |
479 | static void virtio_vdpa_release_dev(struct device *_d) |
480 | { |
481 | struct virtio_device *vdev = |
482 | container_of(_d, struct virtio_device, dev); |
483 | struct virtio_vdpa_device *vd_dev = |
484 | container_of(vdev, struct virtio_vdpa_device, vdev); |
485 | |
486 | kfree(objp: vd_dev); |
487 | } |
488 | |
489 | static int virtio_vdpa_probe(struct vdpa_device *vdpa) |
490 | { |
491 | const struct vdpa_config_ops *ops = vdpa->config; |
492 | struct virtio_vdpa_device *vd_dev, *reg_dev = NULL; |
493 | int ret = -EINVAL; |
494 | |
495 | vd_dev = kzalloc(size: sizeof(*vd_dev), GFP_KERNEL); |
496 | if (!vd_dev) |
497 | return -ENOMEM; |
498 | |
499 | vd_dev->vdev.dev.parent = vdpa_get_dma_dev(vdev: vdpa); |
500 | vd_dev->vdev.dev.release = virtio_vdpa_release_dev; |
501 | vd_dev->vdev.config = &virtio_vdpa_config_ops; |
502 | vd_dev->vdpa = vdpa; |
503 | INIT_LIST_HEAD(list: &vd_dev->virtqueues); |
504 | spin_lock_init(&vd_dev->lock); |
505 | |
506 | vd_dev->vdev.id.device = ops->get_device_id(vdpa); |
507 | if (vd_dev->vdev.id.device == 0) |
508 | goto err; |
509 | |
510 | vd_dev->vdev.id.vendor = ops->get_vendor_id(vdpa); |
511 | ret = register_virtio_device(dev: &vd_dev->vdev); |
512 | reg_dev = vd_dev; |
513 | if (ret) |
514 | goto err; |
515 | |
516 | vdpa_set_drvdata(vdev: vdpa, data: vd_dev); |
517 | |
518 | return 0; |
519 | |
520 | err: |
521 | if (reg_dev) |
522 | put_device(dev: &vd_dev->vdev.dev); |
523 | else |
524 | kfree(objp: vd_dev); |
525 | return ret; |
526 | } |
527 | |
528 | static void virtio_vdpa_remove(struct vdpa_device *vdpa) |
529 | { |
530 | struct virtio_vdpa_device *vd_dev = vdpa_get_drvdata(vdev: vdpa); |
531 | |
532 | unregister_virtio_device(dev: &vd_dev->vdev); |
533 | } |
534 | |
535 | static struct vdpa_driver virtio_vdpa_driver = { |
536 | .driver = { |
537 | .name = "virtio_vdpa" , |
538 | }, |
539 | .probe = virtio_vdpa_probe, |
540 | .remove = virtio_vdpa_remove, |
541 | }; |
542 | |
543 | module_vdpa_driver(virtio_vdpa_driver); |
544 | |
545 | MODULE_VERSION(MOD_VERSION); |
546 | MODULE_LICENSE(MOD_LICENSE); |
547 | MODULE_AUTHOR(MOD_AUTHOR); |
548 | MODULE_DESCRIPTION(MOD_DESC); |
549 | |