libvhost-user: remove watch for kick_fd when de-initialize vu-dev
[qemu.git] / contrib / libvhost-user / libvhost-user.c
1 /*
2 * Vhost User library
3 *
4 * Copyright IBM, Corp. 2007
5 * Copyright (c) 2016 Red Hat, Inc.
6 *
7 * Authors:
8 * Anthony Liguori <aliguori@us.ibm.com>
9 * Marc-André Lureau <mlureau@redhat.com>
10 * Victor Kaplansky <victork@redhat.com>
11 *
12 * This work is licensed under the terms of the GNU GPL, version 2 or
13 * later. See the COPYING file in the top-level directory.
14 */
15
16 /* this code avoids GLib dependency */
17 #include <stdlib.h>
18 #include <stdio.h>
19 #include <unistd.h>
20 #include <stdarg.h>
21 #include <errno.h>
22 #include <string.h>
23 #include <assert.h>
24 #include <inttypes.h>
25 #include <sys/types.h>
26 #include <sys/socket.h>
27 #include <sys/eventfd.h>
28 #include <sys/mman.h>
29 #include "qemu/compiler.h"
30
31 #if defined(__linux__)
32 #include <sys/syscall.h>
33 #include <fcntl.h>
34 #include <sys/ioctl.h>
35 #include <linux/vhost.h>
36
37 #ifdef __NR_userfaultfd
38 #include <linux/userfaultfd.h>
39 #endif
40
41 #endif
42
43 #include "qemu/atomic.h"
44 #include "qemu/osdep.h"
45 #include "qemu/bswap.h"
46 #include "qemu/memfd.h"
47
48 #include "libvhost-user.h"
49
50 /* usually provided by GLib */
51 #ifndef MIN
52 #define MIN(x, y) ({ \
53 typeof(x) _min1 = (x); \
54 typeof(y) _min2 = (y); \
55 (void) (&_min1 == &_min2); \
56 _min1 < _min2 ? _min1 : _min2; })
57 #endif
58
59 /* Round number down to multiple */
60 #define ALIGN_DOWN(n, m) ((n) / (m) * (m))
61
62 /* Round number up to multiple */
63 #define ALIGN_UP(n, m) ALIGN_DOWN((n) + (m) - 1, (m))
64
65 /* Align each region to cache line size in inflight buffer */
66 #define INFLIGHT_ALIGNMENT 64
67
68 /* The version of inflight buffer */
69 #define INFLIGHT_VERSION 1
70
71 /* The version of the protocol we support */
72 #define VHOST_USER_VERSION 1
73 #define LIBVHOST_USER_DEBUG 0
74
75 #define DPRINT(...) \
76 do { \
77 if (LIBVHOST_USER_DEBUG) { \
78 fprintf(stderr, __VA_ARGS__); \
79 } \
80 } while (0)
81
82 static inline
83 bool has_feature(uint64_t features, unsigned int fbit)
84 {
85 assert(fbit < 64);
86 return !!(features & (1ULL << fbit));
87 }
88
89 static inline
90 bool vu_has_feature(VuDev *dev,
91 unsigned int fbit)
92 {
93 return has_feature(dev->features, fbit);
94 }
95
96 static inline bool vu_has_protocol_feature(VuDev *dev, unsigned int fbit)
97 {
98 return has_feature(dev->protocol_features, fbit);
99 }
100
101 static const char *
102 vu_request_to_string(unsigned int req)
103 {
104 #define REQ(req) [req] = #req
105 static const char *vu_request_str[] = {
106 REQ(VHOST_USER_NONE),
107 REQ(VHOST_USER_GET_FEATURES),
108 REQ(VHOST_USER_SET_FEATURES),
109 REQ(VHOST_USER_SET_OWNER),
110 REQ(VHOST_USER_RESET_OWNER),
111 REQ(VHOST_USER_SET_MEM_TABLE),
112 REQ(VHOST_USER_SET_LOG_BASE),
113 REQ(VHOST_USER_SET_LOG_FD),
114 REQ(VHOST_USER_SET_VRING_NUM),
115 REQ(VHOST_USER_SET_VRING_ADDR),
116 REQ(VHOST_USER_SET_VRING_BASE),
117 REQ(VHOST_USER_GET_VRING_BASE),
118 REQ(VHOST_USER_SET_VRING_KICK),
119 REQ(VHOST_USER_SET_VRING_CALL),
120 REQ(VHOST_USER_SET_VRING_ERR),
121 REQ(VHOST_USER_GET_PROTOCOL_FEATURES),
122 REQ(VHOST_USER_SET_PROTOCOL_FEATURES),
123 REQ(VHOST_USER_GET_QUEUE_NUM),
124 REQ(VHOST_USER_SET_VRING_ENABLE),
125 REQ(VHOST_USER_SEND_RARP),
126 REQ(VHOST_USER_NET_SET_MTU),
127 REQ(VHOST_USER_SET_SLAVE_REQ_FD),
128 REQ(VHOST_USER_IOTLB_MSG),
129 REQ(VHOST_USER_SET_VRING_ENDIAN),
130 REQ(VHOST_USER_GET_CONFIG),
131 REQ(VHOST_USER_SET_CONFIG),
132 REQ(VHOST_USER_POSTCOPY_ADVISE),
133 REQ(VHOST_USER_POSTCOPY_LISTEN),
134 REQ(VHOST_USER_POSTCOPY_END),
135 REQ(VHOST_USER_GET_INFLIGHT_FD),
136 REQ(VHOST_USER_SET_INFLIGHT_FD),
137 REQ(VHOST_USER_GPU_SET_SOCKET),
138 REQ(VHOST_USER_VRING_KICK),
139 REQ(VHOST_USER_GET_MAX_MEM_SLOTS),
140 REQ(VHOST_USER_ADD_MEM_REG),
141 REQ(VHOST_USER_REM_MEM_REG),
142 REQ(VHOST_USER_MAX),
143 };
144 #undef REQ
145
146 if (req < VHOST_USER_MAX) {
147 return vu_request_str[req];
148 } else {
149 return "unknown";
150 }
151 }
152
153 static void
154 vu_panic(VuDev *dev, const char *msg, ...)
155 {
156 char *buf = NULL;
157 va_list ap;
158
159 va_start(ap, msg);
160 if (vasprintf(&buf, msg, ap) < 0) {
161 buf = NULL;
162 }
163 va_end(ap);
164
165 dev->broken = true;
166 dev->panic(dev, buf);
167 free(buf);
168
169 /*
170 * FIXME:
171 * find a way to call virtio_error, or perhaps close the connection?
172 */
173 }
174
175 /* Translate guest physical address to our virtual address. */
176 void *
177 vu_gpa_to_va(VuDev *dev, uint64_t *plen, uint64_t guest_addr)
178 {
179 int i;
180
181 if (*plen == 0) {
182 return NULL;
183 }
184
185 /* Find matching memory region. */
186 for (i = 0; i < dev->nregions; i++) {
187 VuDevRegion *r = &dev->regions[i];
188
189 if ((guest_addr >= r->gpa) && (guest_addr < (r->gpa + r->size))) {
190 if ((guest_addr + *plen) > (r->gpa + r->size)) {
191 *plen = r->gpa + r->size - guest_addr;
192 }
193 return (void *)(uintptr_t)
194 guest_addr - r->gpa + r->mmap_addr + r->mmap_offset;
195 }
196 }
197
198 return NULL;
199 }
200
201 /* Translate qemu virtual address to our virtual address. */
202 static void *
203 qva_to_va(VuDev *dev, uint64_t qemu_addr)
204 {
205 int i;
206
207 /* Find matching memory region. */
208 for (i = 0; i < dev->nregions; i++) {
209 VuDevRegion *r = &dev->regions[i];
210
211 if ((qemu_addr >= r->qva) && (qemu_addr < (r->qva + r->size))) {
212 return (void *)(uintptr_t)
213 qemu_addr - r->qva + r->mmap_addr + r->mmap_offset;
214 }
215 }
216
217 return NULL;
218 }
219
220 static void
221 vmsg_close_fds(VhostUserMsg *vmsg)
222 {
223 int i;
224
225 for (i = 0; i < vmsg->fd_num; i++) {
226 close(vmsg->fds[i]);
227 }
228 }
229
230 /* Set reply payload.u64 and clear request flags and fd_num */
231 static void vmsg_set_reply_u64(VhostUserMsg *vmsg, uint64_t val)
232 {
233 vmsg->flags = 0; /* defaults will be set by vu_send_reply() */
234 vmsg->size = sizeof(vmsg->payload.u64);
235 vmsg->payload.u64 = val;
236 vmsg->fd_num = 0;
237 }
238
239 /* A test to see if we have userfault available */
240 static bool
241 have_userfault(void)
242 {
243 #if defined(__linux__) && defined(__NR_userfaultfd) &&\
244 defined(UFFD_FEATURE_MISSING_SHMEM) &&\
245 defined(UFFD_FEATURE_MISSING_HUGETLBFS)
246 /* Now test the kernel we're running on really has the features */
247 int ufd = syscall(__NR_userfaultfd, O_CLOEXEC | O_NONBLOCK);
248 struct uffdio_api api_struct;
249 if (ufd < 0) {
250 return false;
251 }
252
253 api_struct.api = UFFD_API;
254 api_struct.features = UFFD_FEATURE_MISSING_SHMEM |
255 UFFD_FEATURE_MISSING_HUGETLBFS;
256 if (ioctl(ufd, UFFDIO_API, &api_struct)) {
257 close(ufd);
258 return false;
259 }
260 close(ufd);
261 return true;
262
263 #else
264 return false;
265 #endif
266 }
267
268 static bool
269 vu_message_read_default(VuDev *dev, int conn_fd, VhostUserMsg *vmsg)
270 {
271 char control[CMSG_SPACE(VHOST_MEMORY_BASELINE_NREGIONS * sizeof(int))] = {};
272 struct iovec iov = {
273 .iov_base = (char *)vmsg,
274 .iov_len = VHOST_USER_HDR_SIZE,
275 };
276 struct msghdr msg = {
277 .msg_iov = &iov,
278 .msg_iovlen = 1,
279 .msg_control = control,
280 .msg_controllen = sizeof(control),
281 };
282 size_t fd_size;
283 struct cmsghdr *cmsg;
284 int rc;
285
286 do {
287 rc = recvmsg(conn_fd, &msg, 0);
288 } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
289
290 if (rc < 0) {
291 vu_panic(dev, "Error while recvmsg: %s", strerror(errno));
292 return false;
293 }
294
295 vmsg->fd_num = 0;
296 for (cmsg = CMSG_FIRSTHDR(&msg);
297 cmsg != NULL;
298 cmsg = CMSG_NXTHDR(&msg, cmsg))
299 {
300 if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS) {
301 fd_size = cmsg->cmsg_len - CMSG_LEN(0);
302 vmsg->fd_num = fd_size / sizeof(int);
303 memcpy(vmsg->fds, CMSG_DATA(cmsg), fd_size);
304 break;
305 }
306 }
307
308 if (vmsg->size > sizeof(vmsg->payload)) {
309 vu_panic(dev,
310 "Error: too big message request: %d, size: vmsg->size: %u, "
311 "while sizeof(vmsg->payload) = %zu\n",
312 vmsg->request, vmsg->size, sizeof(vmsg->payload));
313 goto fail;
314 }
315
316 if (vmsg->size) {
317 do {
318 rc = read(conn_fd, &vmsg->payload, vmsg->size);
319 } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
320
321 if (rc <= 0) {
322 vu_panic(dev, "Error while reading: %s", strerror(errno));
323 goto fail;
324 }
325
326 assert(rc == vmsg->size);
327 }
328
329 return true;
330
331 fail:
332 vmsg_close_fds(vmsg);
333
334 return false;
335 }
336
337 static bool
338 vu_message_write(VuDev *dev, int conn_fd, VhostUserMsg *vmsg)
339 {
340 int rc;
341 uint8_t *p = (uint8_t *)vmsg;
342 char control[CMSG_SPACE(VHOST_MEMORY_BASELINE_NREGIONS * sizeof(int))] = {};
343 struct iovec iov = {
344 .iov_base = (char *)vmsg,
345 .iov_len = VHOST_USER_HDR_SIZE,
346 };
347 struct msghdr msg = {
348 .msg_iov = &iov,
349 .msg_iovlen = 1,
350 .msg_control = control,
351 };
352 struct cmsghdr *cmsg;
353
354 memset(control, 0, sizeof(control));
355 assert(vmsg->fd_num <= VHOST_MEMORY_BASELINE_NREGIONS);
356 if (vmsg->fd_num > 0) {
357 size_t fdsize = vmsg->fd_num * sizeof(int);
358 msg.msg_controllen = CMSG_SPACE(fdsize);
359 cmsg = CMSG_FIRSTHDR(&msg);
360 cmsg->cmsg_len = CMSG_LEN(fdsize);
361 cmsg->cmsg_level = SOL_SOCKET;
362 cmsg->cmsg_type = SCM_RIGHTS;
363 memcpy(CMSG_DATA(cmsg), vmsg->fds, fdsize);
364 } else {
365 msg.msg_controllen = 0;
366 }
367
368 do {
369 rc = sendmsg(conn_fd, &msg, 0);
370 } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
371
372 if (vmsg->size) {
373 do {
374 if (vmsg->data) {
375 rc = write(conn_fd, vmsg->data, vmsg->size);
376 } else {
377 rc = write(conn_fd, p + VHOST_USER_HDR_SIZE, vmsg->size);
378 }
379 } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
380 }
381
382 if (rc <= 0) {
383 vu_panic(dev, "Error while writing: %s", strerror(errno));
384 return false;
385 }
386
387 return true;
388 }
389
390 static bool
391 vu_send_reply(VuDev *dev, int conn_fd, VhostUserMsg *vmsg)
392 {
393 /* Set the version in the flags when sending the reply */
394 vmsg->flags &= ~VHOST_USER_VERSION_MASK;
395 vmsg->flags |= VHOST_USER_VERSION;
396 vmsg->flags |= VHOST_USER_REPLY_MASK;
397
398 return vu_message_write(dev, conn_fd, vmsg);
399 }
400
401 /*
402 * Processes a reply on the slave channel.
403 * Entered with slave_mutex held and releases it before exit.
404 * Returns true on success.
405 */
406 static bool
407 vu_process_message_reply(VuDev *dev, const VhostUserMsg *vmsg)
408 {
409 VhostUserMsg msg_reply;
410 bool result = false;
411
412 if ((vmsg->flags & VHOST_USER_NEED_REPLY_MASK) == 0) {
413 result = true;
414 goto out;
415 }
416
417 if (!vu_message_read_default(dev, dev->slave_fd, &msg_reply)) {
418 goto out;
419 }
420
421 if (msg_reply.request != vmsg->request) {
422 DPRINT("Received unexpected msg type. Expected %d received %d",
423 vmsg->request, msg_reply.request);
424 goto out;
425 }
426
427 result = msg_reply.payload.u64 == 0;
428
429 out:
430 pthread_mutex_unlock(&dev->slave_mutex);
431 return result;
432 }
433
434 /* Kick the log_call_fd if required. */
435 static void
436 vu_log_kick(VuDev *dev)
437 {
438 if (dev->log_call_fd != -1) {
439 DPRINT("Kicking the QEMU's log...\n");
440 if (eventfd_write(dev->log_call_fd, 1) < 0) {
441 vu_panic(dev, "Error writing eventfd: %s", strerror(errno));
442 }
443 }
444 }
445
446 static void
447 vu_log_page(uint8_t *log_table, uint64_t page)
448 {
449 DPRINT("Logged dirty guest page: %"PRId64"\n", page);
450 qatomic_or(&log_table[page / 8], 1 << (page % 8));
451 }
452
453 static void
454 vu_log_write(VuDev *dev, uint64_t address, uint64_t length)
455 {
456 uint64_t page;
457
458 if (!(dev->features & (1ULL << VHOST_F_LOG_ALL)) ||
459 !dev->log_table || !length) {
460 return;
461 }
462
463 assert(dev->log_size > ((address + length - 1) / VHOST_LOG_PAGE / 8));
464
465 page = address / VHOST_LOG_PAGE;
466 while (page * VHOST_LOG_PAGE < address + length) {
467 vu_log_page(dev->log_table, page);
468 page += 1;
469 }
470
471 vu_log_kick(dev);
472 }
473
474 static void
475 vu_kick_cb(VuDev *dev, int condition, void *data)
476 {
477 int index = (intptr_t)data;
478 VuVirtq *vq = &dev->vq[index];
479 int sock = vq->kick_fd;
480 eventfd_t kick_data;
481 ssize_t rc;
482
483 rc = eventfd_read(sock, &kick_data);
484 if (rc == -1) {
485 vu_panic(dev, "kick eventfd_read(): %s", strerror(errno));
486 dev->remove_watch(dev, dev->vq[index].kick_fd);
487 } else {
488 DPRINT("Got kick_data: %016"PRIx64" handler:%p idx:%d\n",
489 kick_data, vq->handler, index);
490 if (vq->handler) {
491 vq->handler(dev, index);
492 }
493 }
494 }
495
496 static bool
497 vu_get_features_exec(VuDev *dev, VhostUserMsg *vmsg)
498 {
499 vmsg->payload.u64 =
500 /*
501 * The following VIRTIO feature bits are supported by our virtqueue
502 * implementation:
503 */
504 1ULL << VIRTIO_F_NOTIFY_ON_EMPTY |
505 1ULL << VIRTIO_RING_F_INDIRECT_DESC |
506 1ULL << VIRTIO_RING_F_EVENT_IDX |
507 1ULL << VIRTIO_F_VERSION_1 |
508
509 /* vhost-user feature bits */
510 1ULL << VHOST_F_LOG_ALL |
511 1ULL << VHOST_USER_F_PROTOCOL_FEATURES;
512
513 if (dev->iface->get_features) {
514 vmsg->payload.u64 |= dev->iface->get_features(dev);
515 }
516
517 vmsg->size = sizeof(vmsg->payload.u64);
518 vmsg->fd_num = 0;
519
520 DPRINT("Sending back to guest u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
521
522 return true;
523 }
524
525 static void
526 vu_set_enable_all_rings(VuDev *dev, bool enabled)
527 {
528 uint16_t i;
529
530 for (i = 0; i < dev->max_queues; i++) {
531 dev->vq[i].enable = enabled;
532 }
533 }
534
535 static bool
536 vu_set_features_exec(VuDev *dev, VhostUserMsg *vmsg)
537 {
538 DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
539
540 dev->features = vmsg->payload.u64;
541 if (!vu_has_feature(dev, VIRTIO_F_VERSION_1)) {
542 /*
543 * We only support devices conforming to VIRTIO 1.0 or
544 * later
545 */
546 vu_panic(dev, "virtio legacy devices aren't supported by libvhost-user");
547 return false;
548 }
549
550 if (!(dev->features & VHOST_USER_F_PROTOCOL_FEATURES)) {
551 vu_set_enable_all_rings(dev, true);
552 }
553
554 if (dev->iface->set_features) {
555 dev->iface->set_features(dev, dev->features);
556 }
557
558 return false;
559 }
560
561 static bool
562 vu_set_owner_exec(VuDev *dev, VhostUserMsg *vmsg)
563 {
564 return false;
565 }
566
567 static void
568 vu_close_log(VuDev *dev)
569 {
570 if (dev->log_table) {
571 if (munmap(dev->log_table, dev->log_size) != 0) {
572 perror("close log munmap() error");
573 }
574
575 dev->log_table = NULL;
576 }
577 if (dev->log_call_fd != -1) {
578 close(dev->log_call_fd);
579 dev->log_call_fd = -1;
580 }
581 }
582
583 static bool
584 vu_reset_device_exec(VuDev *dev, VhostUserMsg *vmsg)
585 {
586 vu_set_enable_all_rings(dev, false);
587
588 return false;
589 }
590
591 static bool
592 map_ring(VuDev *dev, VuVirtq *vq)
593 {
594 vq->vring.desc = qva_to_va(dev, vq->vra.desc_user_addr);
595 vq->vring.used = qva_to_va(dev, vq->vra.used_user_addr);
596 vq->vring.avail = qva_to_va(dev, vq->vra.avail_user_addr);
597
598 DPRINT("Setting virtq addresses:\n");
599 DPRINT(" vring_desc at %p\n", vq->vring.desc);
600 DPRINT(" vring_used at %p\n", vq->vring.used);
601 DPRINT(" vring_avail at %p\n", vq->vring.avail);
602
603 return !(vq->vring.desc && vq->vring.used && vq->vring.avail);
604 }
605
606 static bool
607 generate_faults(VuDev *dev) {
608 int i;
609 for (i = 0; i < dev->nregions; i++) {
610 VuDevRegion *dev_region = &dev->regions[i];
611 int ret;
612 #ifdef UFFDIO_REGISTER
613 /*
614 * We should already have an open ufd. Mark each memory
615 * range as ufd.
616 * Discard any mapping we have here; note I can't use MADV_REMOVE
617 * or fallocate to make the hole since I don't want to lose
618 * data that's already arrived in the shared process.
619 * TODO: How to do hugepage
620 */
621 ret = madvise((void *)(uintptr_t)dev_region->mmap_addr,
622 dev_region->size + dev_region->mmap_offset,
623 MADV_DONTNEED);
624 if (ret) {
625 fprintf(stderr,
626 "%s: Failed to madvise(DONTNEED) region %d: %s\n",
627 __func__, i, strerror(errno));
628 }
629 /*
630 * Turn off transparent hugepages so we dont get lose wakeups
631 * in neighbouring pages.
632 * TODO: Turn this backon later.
633 */
634 ret = madvise((void *)(uintptr_t)dev_region->mmap_addr,
635 dev_region->size + dev_region->mmap_offset,
636 MADV_NOHUGEPAGE);
637 if (ret) {
638 /*
639 * Note: This can happen legally on kernels that are configured
640 * without madvise'able hugepages
641 */
642 fprintf(stderr,
643 "%s: Failed to madvise(NOHUGEPAGE) region %d: %s\n",
644 __func__, i, strerror(errno));
645 }
646 struct uffdio_register reg_struct;
647 reg_struct.range.start = (uintptr_t)dev_region->mmap_addr;
648 reg_struct.range.len = dev_region->size + dev_region->mmap_offset;
649 reg_struct.mode = UFFDIO_REGISTER_MODE_MISSING;
650
651 if (ioctl(dev->postcopy_ufd, UFFDIO_REGISTER, &reg_struct)) {
652 vu_panic(dev, "%s: Failed to userfault region %d "
653 "@%p + size:%zx offset: %zx: (ufd=%d)%s\n",
654 __func__, i,
655 dev_region->mmap_addr,
656 dev_region->size, dev_region->mmap_offset,
657 dev->postcopy_ufd, strerror(errno));
658 return false;
659 }
660 if (!(reg_struct.ioctls & ((__u64)1 << _UFFDIO_COPY))) {
661 vu_panic(dev, "%s Region (%d) doesn't support COPY",
662 __func__, i);
663 return false;
664 }
665 DPRINT("%s: region %d: Registered userfault for %"
666 PRIx64 " + %" PRIx64 "\n", __func__, i,
667 (uint64_t)reg_struct.range.start,
668 (uint64_t)reg_struct.range.len);
669 /* Now it's registered we can let the client at it */
670 if (mprotect((void *)(uintptr_t)dev_region->mmap_addr,
671 dev_region->size + dev_region->mmap_offset,
672 PROT_READ | PROT_WRITE)) {
673 vu_panic(dev, "failed to mprotect region %d for postcopy (%s)",
674 i, strerror(errno));
675 return false;
676 }
677 /* TODO: Stash 'zero' support flags somewhere */
678 #endif
679 }
680
681 return true;
682 }
683
684 static bool
685 vu_add_mem_reg(VuDev *dev, VhostUserMsg *vmsg) {
686 int i;
687 bool track_ramblocks = dev->postcopy_listening;
688 VhostUserMemoryRegion m = vmsg->payload.memreg.region, *msg_region = &m;
689 VuDevRegion *dev_region = &dev->regions[dev->nregions];
690 void *mmap_addr;
691
692 /*
693 * If we are in postcopy mode and we receive a u64 payload with a 0 value
694 * we know all the postcopy client bases have been received, and we
695 * should start generating faults.
696 */
697 if (track_ramblocks &&
698 vmsg->size == sizeof(vmsg->payload.u64) &&
699 vmsg->payload.u64 == 0) {
700 (void)generate_faults(dev);
701 return false;
702 }
703
704 DPRINT("Adding region: %d\n", dev->nregions);
705 DPRINT(" guest_phys_addr: 0x%016"PRIx64"\n",
706 msg_region->guest_phys_addr);
707 DPRINT(" memory_size: 0x%016"PRIx64"\n",
708 msg_region->memory_size);
709 DPRINT(" userspace_addr 0x%016"PRIx64"\n",
710 msg_region->userspace_addr);
711 DPRINT(" mmap_offset 0x%016"PRIx64"\n",
712 msg_region->mmap_offset);
713
714 dev_region->gpa = msg_region->guest_phys_addr;
715 dev_region->size = msg_region->memory_size;
716 dev_region->qva = msg_region->userspace_addr;
717 dev_region->mmap_offset = msg_region->mmap_offset;
718
719 /*
720 * We don't use offset argument of mmap() since the
721 * mapped address has to be page aligned, and we use huge
722 * pages.
723 */
724 if (track_ramblocks) {
725 /*
726 * In postcopy we're using PROT_NONE here to catch anyone
727 * accessing it before we userfault.
728 */
729 mmap_addr = mmap(0, dev_region->size + dev_region->mmap_offset,
730 PROT_NONE, MAP_SHARED,
731 vmsg->fds[0], 0);
732 } else {
733 mmap_addr = mmap(0, dev_region->size + dev_region->mmap_offset,
734 PROT_READ | PROT_WRITE, MAP_SHARED, vmsg->fds[0],
735 0);
736 }
737
738 if (mmap_addr == MAP_FAILED) {
739 vu_panic(dev, "region mmap error: %s", strerror(errno));
740 } else {
741 dev_region->mmap_addr = (uint64_t)(uintptr_t)mmap_addr;
742 DPRINT(" mmap_addr: 0x%016"PRIx64"\n",
743 dev_region->mmap_addr);
744 }
745
746 close(vmsg->fds[0]);
747
748 if (track_ramblocks) {
749 /*
750 * Return the address to QEMU so that it can translate the ufd
751 * fault addresses back.
752 */
753 msg_region->userspace_addr = (uintptr_t)(mmap_addr +
754 dev_region->mmap_offset);
755
756 /* Send the message back to qemu with the addresses filled in. */
757 vmsg->fd_num = 0;
758 if (!vu_send_reply(dev, dev->sock, vmsg)) {
759 vu_panic(dev, "failed to respond to add-mem-region for postcopy");
760 return false;
761 }
762
763 DPRINT("Successfully added new region in postcopy\n");
764 dev->nregions++;
765 return false;
766
767 } else {
768 for (i = 0; i < dev->max_queues; i++) {
769 if (dev->vq[i].vring.desc) {
770 if (map_ring(dev, &dev->vq[i])) {
771 vu_panic(dev, "remapping queue %d for new memory region",
772 i);
773 }
774 }
775 }
776
777 DPRINT("Successfully added new region\n");
778 dev->nregions++;
779 vmsg_set_reply_u64(vmsg, 0);
780 return true;
781 }
782 }
783
784 static inline bool reg_equal(VuDevRegion *vudev_reg,
785 VhostUserMemoryRegion *msg_reg)
786 {
787 if (vudev_reg->gpa == msg_reg->guest_phys_addr &&
788 vudev_reg->qva == msg_reg->userspace_addr &&
789 vudev_reg->size == msg_reg->memory_size) {
790 return true;
791 }
792
793 return false;
794 }
795
796 static bool
797 vu_rem_mem_reg(VuDev *dev, VhostUserMsg *vmsg) {
798 int i, j;
799 bool found = false;
800 VuDevRegion shadow_regions[VHOST_USER_MAX_RAM_SLOTS] = {};
801 VhostUserMemoryRegion m = vmsg->payload.memreg.region, *msg_region = &m;
802
803 DPRINT("Removing region:\n");
804 DPRINT(" guest_phys_addr: 0x%016"PRIx64"\n",
805 msg_region->guest_phys_addr);
806 DPRINT(" memory_size: 0x%016"PRIx64"\n",
807 msg_region->memory_size);
808 DPRINT(" userspace_addr 0x%016"PRIx64"\n",
809 msg_region->userspace_addr);
810 DPRINT(" mmap_offset 0x%016"PRIx64"\n",
811 msg_region->mmap_offset);
812
813 for (i = 0, j = 0; i < dev->nregions; i++) {
814 if (!reg_equal(&dev->regions[i], msg_region)) {
815 shadow_regions[j].gpa = dev->regions[i].gpa;
816 shadow_regions[j].size = dev->regions[i].size;
817 shadow_regions[j].qva = dev->regions[i].qva;
818 shadow_regions[j].mmap_offset = dev->regions[i].mmap_offset;
819 j++;
820 } else {
821 found = true;
822 VuDevRegion *r = &dev->regions[i];
823 void *m = (void *) (uintptr_t) r->mmap_addr;
824
825 if (m) {
826 munmap(m, r->size + r->mmap_offset);
827 }
828 }
829 }
830
831 if (found) {
832 memcpy(dev->regions, shadow_regions,
833 sizeof(VuDevRegion) * VHOST_USER_MAX_RAM_SLOTS);
834 DPRINT("Successfully removed a region\n");
835 dev->nregions--;
836 vmsg_set_reply_u64(vmsg, 0);
837 } else {
838 vu_panic(dev, "Specified region not found\n");
839 }
840
841 return true;
842 }
843
844 static bool
845 vu_set_mem_table_exec_postcopy(VuDev *dev, VhostUserMsg *vmsg)
846 {
847 int i;
848 VhostUserMemory m = vmsg->payload.memory, *memory = &m;
849 dev->nregions = memory->nregions;
850
851 DPRINT("Nregions: %d\n", memory->nregions);
852 for (i = 0; i < dev->nregions; i++) {
853 void *mmap_addr;
854 VhostUserMemoryRegion *msg_region = &memory->regions[i];
855 VuDevRegion *dev_region = &dev->regions[i];
856
857 DPRINT("Region %d\n", i);
858 DPRINT(" guest_phys_addr: 0x%016"PRIx64"\n",
859 msg_region->guest_phys_addr);
860 DPRINT(" memory_size: 0x%016"PRIx64"\n",
861 msg_region->memory_size);
862 DPRINT(" userspace_addr 0x%016"PRIx64"\n",
863 msg_region->userspace_addr);
864 DPRINT(" mmap_offset 0x%016"PRIx64"\n",
865 msg_region->mmap_offset);
866
867 dev_region->gpa = msg_region->guest_phys_addr;
868 dev_region->size = msg_region->memory_size;
869 dev_region->qva = msg_region->userspace_addr;
870 dev_region->mmap_offset = msg_region->mmap_offset;
871
872 /* We don't use offset argument of mmap() since the
873 * mapped address has to be page aligned, and we use huge
874 * pages.
875 * In postcopy we're using PROT_NONE here to catch anyone
876 * accessing it before we userfault
877 */
878 mmap_addr = mmap(0, dev_region->size + dev_region->mmap_offset,
879 PROT_NONE, MAP_SHARED,
880 vmsg->fds[i], 0);
881
882 if (mmap_addr == MAP_FAILED) {
883 vu_panic(dev, "region mmap error: %s", strerror(errno));
884 } else {
885 dev_region->mmap_addr = (uint64_t)(uintptr_t)mmap_addr;
886 DPRINT(" mmap_addr: 0x%016"PRIx64"\n",
887 dev_region->mmap_addr);
888 }
889
890 /* Return the address to QEMU so that it can translate the ufd
891 * fault addresses back.
892 */
893 msg_region->userspace_addr = (uintptr_t)(mmap_addr +
894 dev_region->mmap_offset);
895 close(vmsg->fds[i]);
896 }
897
898 /* Send the message back to qemu with the addresses filled in */
899 vmsg->fd_num = 0;
900 if (!vu_send_reply(dev, dev->sock, vmsg)) {
901 vu_panic(dev, "failed to respond to set-mem-table for postcopy");
902 return false;
903 }
904
905 /* Wait for QEMU to confirm that it's registered the handler for the
906 * faults.
907 */
908 if (!dev->read_msg(dev, dev->sock, vmsg) ||
909 vmsg->size != sizeof(vmsg->payload.u64) ||
910 vmsg->payload.u64 != 0) {
911 vu_panic(dev, "failed to receive valid ack for postcopy set-mem-table");
912 return false;
913 }
914
915 /* OK, now we can go and register the memory and generate faults */
916 (void)generate_faults(dev);
917
918 return false;
919 }
920
921 static bool
922 vu_set_mem_table_exec(VuDev *dev, VhostUserMsg *vmsg)
923 {
924 int i;
925 VhostUserMemory m = vmsg->payload.memory, *memory = &m;
926
927 for (i = 0; i < dev->nregions; i++) {
928 VuDevRegion *r = &dev->regions[i];
929 void *m = (void *) (uintptr_t) r->mmap_addr;
930
931 if (m) {
932 munmap(m, r->size + r->mmap_offset);
933 }
934 }
935 dev->nregions = memory->nregions;
936
937 if (dev->postcopy_listening) {
938 return vu_set_mem_table_exec_postcopy(dev, vmsg);
939 }
940
941 DPRINT("Nregions: %d\n", memory->nregions);
942 for (i = 0; i < dev->nregions; i++) {
943 void *mmap_addr;
944 VhostUserMemoryRegion *msg_region = &memory->regions[i];
945 VuDevRegion *dev_region = &dev->regions[i];
946
947 DPRINT("Region %d\n", i);
948 DPRINT(" guest_phys_addr: 0x%016"PRIx64"\n",
949 msg_region->guest_phys_addr);
950 DPRINT(" memory_size: 0x%016"PRIx64"\n",
951 msg_region->memory_size);
952 DPRINT(" userspace_addr 0x%016"PRIx64"\n",
953 msg_region->userspace_addr);
954 DPRINT(" mmap_offset 0x%016"PRIx64"\n",
955 msg_region->mmap_offset);
956
957 dev_region->gpa = msg_region->guest_phys_addr;
958 dev_region->size = msg_region->memory_size;
959 dev_region->qva = msg_region->userspace_addr;
960 dev_region->mmap_offset = msg_region->mmap_offset;
961
962 /* We don't use offset argument of mmap() since the
963 * mapped address has to be page aligned, and we use huge
964 * pages. */
965 mmap_addr = mmap(0, dev_region->size + dev_region->mmap_offset,
966 PROT_READ | PROT_WRITE, MAP_SHARED,
967 vmsg->fds[i], 0);
968
969 if (mmap_addr == MAP_FAILED) {
970 vu_panic(dev, "region mmap error: %s", strerror(errno));
971 } else {
972 dev_region->mmap_addr = (uint64_t)(uintptr_t)mmap_addr;
973 DPRINT(" mmap_addr: 0x%016"PRIx64"\n",
974 dev_region->mmap_addr);
975 }
976
977 close(vmsg->fds[i]);
978 }
979
980 for (i = 0; i < dev->max_queues; i++) {
981 if (dev->vq[i].vring.desc) {
982 if (map_ring(dev, &dev->vq[i])) {
983 vu_panic(dev, "remapping queue %d during setmemtable", i);
984 }
985 }
986 }
987
988 return false;
989 }
990
991 static bool
992 vu_set_log_base_exec(VuDev *dev, VhostUserMsg *vmsg)
993 {
994 int fd;
995 uint64_t log_mmap_size, log_mmap_offset;
996 void *rc;
997
998 if (vmsg->fd_num != 1 ||
999 vmsg->size != sizeof(vmsg->payload.log)) {
1000 vu_panic(dev, "Invalid log_base message");
1001 return true;
1002 }
1003
1004 fd = vmsg->fds[0];
1005 log_mmap_offset = vmsg->payload.log.mmap_offset;
1006 log_mmap_size = vmsg->payload.log.mmap_size;
1007 DPRINT("Log mmap_offset: %"PRId64"\n", log_mmap_offset);
1008 DPRINT("Log mmap_size: %"PRId64"\n", log_mmap_size);
1009
1010 rc = mmap(0, log_mmap_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
1011 log_mmap_offset);
1012 close(fd);
1013 if (rc == MAP_FAILED) {
1014 perror("log mmap error");
1015 }
1016
1017 if (dev->log_table) {
1018 munmap(dev->log_table, dev->log_size);
1019 }
1020 dev->log_table = rc;
1021 dev->log_size = log_mmap_size;
1022
1023 vmsg->size = sizeof(vmsg->payload.u64);
1024 vmsg->fd_num = 0;
1025
1026 return true;
1027 }
1028
1029 static bool
1030 vu_set_log_fd_exec(VuDev *dev, VhostUserMsg *vmsg)
1031 {
1032 if (vmsg->fd_num != 1) {
1033 vu_panic(dev, "Invalid log_fd message");
1034 return false;
1035 }
1036
1037 if (dev->log_call_fd != -1) {
1038 close(dev->log_call_fd);
1039 }
1040 dev->log_call_fd = vmsg->fds[0];
1041 DPRINT("Got log_call_fd: %d\n", vmsg->fds[0]);
1042
1043 return false;
1044 }
1045
1046 static bool
1047 vu_set_vring_num_exec(VuDev *dev, VhostUserMsg *vmsg)
1048 {
1049 unsigned int index = vmsg->payload.state.index;
1050 unsigned int num = vmsg->payload.state.num;
1051
1052 DPRINT("State.index: %d\n", index);
1053 DPRINT("State.num: %d\n", num);
1054 dev->vq[index].vring.num = num;
1055
1056 return false;
1057 }
1058
1059 static bool
1060 vu_set_vring_addr_exec(VuDev *dev, VhostUserMsg *vmsg)
1061 {
1062 struct vhost_vring_addr addr = vmsg->payload.addr, *vra = &addr;
1063 unsigned int index = vra->index;
1064 VuVirtq *vq = &dev->vq[index];
1065
1066 DPRINT("vhost_vring_addr:\n");
1067 DPRINT(" index: %d\n", vra->index);
1068 DPRINT(" flags: %d\n", vra->flags);
1069 DPRINT(" desc_user_addr: 0x%016" PRIx64 "\n", vra->desc_user_addr);
1070 DPRINT(" used_user_addr: 0x%016" PRIx64 "\n", vra->used_user_addr);
1071 DPRINT(" avail_user_addr: 0x%016" PRIx64 "\n", vra->avail_user_addr);
1072 DPRINT(" log_guest_addr: 0x%016" PRIx64 "\n", vra->log_guest_addr);
1073
1074 vq->vra = *vra;
1075 vq->vring.flags = vra->flags;
1076 vq->vring.log_guest_addr = vra->log_guest_addr;
1077
1078
1079 if (map_ring(dev, vq)) {
1080 vu_panic(dev, "Invalid vring_addr message");
1081 return false;
1082 }
1083
1084 vq->used_idx = lduw_le_p(&vq->vring.used->idx);
1085
1086 if (vq->last_avail_idx != vq->used_idx) {
1087 bool resume = dev->iface->queue_is_processed_in_order &&
1088 dev->iface->queue_is_processed_in_order(dev, index);
1089
1090 DPRINT("Last avail index != used index: %u != %u%s\n",
1091 vq->last_avail_idx, vq->used_idx,
1092 resume ? ", resuming" : "");
1093
1094 if (resume) {
1095 vq->shadow_avail_idx = vq->last_avail_idx = vq->used_idx;
1096 }
1097 }
1098
1099 return false;
1100 }
1101
1102 static bool
1103 vu_set_vring_base_exec(VuDev *dev, VhostUserMsg *vmsg)
1104 {
1105 unsigned int index = vmsg->payload.state.index;
1106 unsigned int num = vmsg->payload.state.num;
1107
1108 DPRINT("State.index: %d\n", index);
1109 DPRINT("State.num: %d\n", num);
1110 dev->vq[index].shadow_avail_idx = dev->vq[index].last_avail_idx = num;
1111
1112 return false;
1113 }
1114
1115 static bool
1116 vu_get_vring_base_exec(VuDev *dev, VhostUserMsg *vmsg)
1117 {
1118 unsigned int index = vmsg->payload.state.index;
1119
1120 DPRINT("State.index: %d\n", index);
1121 vmsg->payload.state.num = dev->vq[index].last_avail_idx;
1122 vmsg->size = sizeof(vmsg->payload.state);
1123
1124 dev->vq[index].started = false;
1125 if (dev->iface->queue_set_started) {
1126 dev->iface->queue_set_started(dev, index, false);
1127 }
1128
1129 if (dev->vq[index].call_fd != -1) {
1130 close(dev->vq[index].call_fd);
1131 dev->vq[index].call_fd = -1;
1132 }
1133 if (dev->vq[index].kick_fd != -1) {
1134 dev->remove_watch(dev, dev->vq[index].kick_fd);
1135 close(dev->vq[index].kick_fd);
1136 dev->vq[index].kick_fd = -1;
1137 }
1138
1139 return true;
1140 }
1141
1142 static bool
1143 vu_check_queue_msg_file(VuDev *dev, VhostUserMsg *vmsg)
1144 {
1145 int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
1146 bool nofd = vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK;
1147
1148 if (index >= dev->max_queues) {
1149 vmsg_close_fds(vmsg);
1150 vu_panic(dev, "Invalid queue index: %u", index);
1151 return false;
1152 }
1153
1154 if (nofd) {
1155 vmsg_close_fds(vmsg);
1156 return true;
1157 }
1158
1159 if (vmsg->fd_num != 1) {
1160 vmsg_close_fds(vmsg);
1161 vu_panic(dev, "Invalid fds in request: %d", vmsg->request);
1162 return false;
1163 }
1164
1165 return true;
1166 }
1167
1168 static int
1169 inflight_desc_compare(const void *a, const void *b)
1170 {
1171 VuVirtqInflightDesc *desc0 = (VuVirtqInflightDesc *)a,
1172 *desc1 = (VuVirtqInflightDesc *)b;
1173
1174 if (desc1->counter > desc0->counter &&
1175 (desc1->counter - desc0->counter) < VIRTQUEUE_MAX_SIZE * 2) {
1176 return 1;
1177 }
1178
1179 return -1;
1180 }
1181
1182 static int
1183 vu_check_queue_inflights(VuDev *dev, VuVirtq *vq)
1184 {
1185 int i = 0;
1186
1187 if (!vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_INFLIGHT_SHMFD)) {
1188 return 0;
1189 }
1190
1191 if (unlikely(!vq->inflight)) {
1192 return -1;
1193 }
1194
1195 if (unlikely(!vq->inflight->version)) {
1196 /* initialize the buffer */
1197 vq->inflight->version = INFLIGHT_VERSION;
1198 return 0;
1199 }
1200
1201 vq->used_idx = lduw_le_p(&vq->vring.used->idx);
1202 vq->resubmit_num = 0;
1203 vq->resubmit_list = NULL;
1204 vq->counter = 0;
1205
1206 if (unlikely(vq->inflight->used_idx != vq->used_idx)) {
1207 vq->inflight->desc[vq->inflight->last_batch_head].inflight = 0;
1208
1209 barrier();
1210
1211 vq->inflight->used_idx = vq->used_idx;
1212 }
1213
1214 for (i = 0; i < vq->inflight->desc_num; i++) {
1215 if (vq->inflight->desc[i].inflight == 1) {
1216 vq->inuse++;
1217 }
1218 }
1219
1220 vq->shadow_avail_idx = vq->last_avail_idx = vq->inuse + vq->used_idx;
1221
1222 if (vq->inuse) {
1223 vq->resubmit_list = calloc(vq->inuse, sizeof(VuVirtqInflightDesc));
1224 if (!vq->resubmit_list) {
1225 return -1;
1226 }
1227
1228 for (i = 0; i < vq->inflight->desc_num; i++) {
1229 if (vq->inflight->desc[i].inflight) {
1230 vq->resubmit_list[vq->resubmit_num].index = i;
1231 vq->resubmit_list[vq->resubmit_num].counter =
1232 vq->inflight->desc[i].counter;
1233 vq->resubmit_num++;
1234 }
1235 }
1236
1237 if (vq->resubmit_num > 1) {
1238 qsort(vq->resubmit_list, vq->resubmit_num,
1239 sizeof(VuVirtqInflightDesc), inflight_desc_compare);
1240 }
1241 vq->counter = vq->resubmit_list[0].counter + 1;
1242 }
1243
1244 /* in case of I/O hang after reconnecting */
1245 if (eventfd_write(vq->kick_fd, 1)) {
1246 return -1;
1247 }
1248
1249 return 0;
1250 }
1251
1252 static bool
1253 vu_set_vring_kick_exec(VuDev *dev, VhostUserMsg *vmsg)
1254 {
1255 int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
1256 bool nofd = vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK;
1257
1258 DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
1259
1260 if (!vu_check_queue_msg_file(dev, vmsg)) {
1261 return false;
1262 }
1263
1264 if (dev->vq[index].kick_fd != -1) {
1265 dev->remove_watch(dev, dev->vq[index].kick_fd);
1266 close(dev->vq[index].kick_fd);
1267 dev->vq[index].kick_fd = -1;
1268 }
1269
1270 dev->vq[index].kick_fd = nofd ? -1 : vmsg->fds[0];
1271 DPRINT("Got kick_fd: %d for vq: %d\n", dev->vq[index].kick_fd, index);
1272
1273 dev->vq[index].started = true;
1274 if (dev->iface->queue_set_started) {
1275 dev->iface->queue_set_started(dev, index, true);
1276 }
1277
1278 if (dev->vq[index].kick_fd != -1 && dev->vq[index].handler) {
1279 dev->set_watch(dev, dev->vq[index].kick_fd, VU_WATCH_IN,
1280 vu_kick_cb, (void *)(long)index);
1281
1282 DPRINT("Waiting for kicks on fd: %d for vq: %d\n",
1283 dev->vq[index].kick_fd, index);
1284 }
1285
1286 if (vu_check_queue_inflights(dev, &dev->vq[index])) {
1287 vu_panic(dev, "Failed to check inflights for vq: %d\n", index);
1288 }
1289
1290 return false;
1291 }
1292
1293 void vu_set_queue_handler(VuDev *dev, VuVirtq *vq,
1294 vu_queue_handler_cb handler)
1295 {
1296 int qidx = vq - dev->vq;
1297
1298 vq->handler = handler;
1299 if (vq->kick_fd >= 0) {
1300 if (handler) {
1301 dev->set_watch(dev, vq->kick_fd, VU_WATCH_IN,
1302 vu_kick_cb, (void *)(long)qidx);
1303 } else {
1304 dev->remove_watch(dev, vq->kick_fd);
1305 }
1306 }
1307 }
1308
1309 bool vu_set_queue_host_notifier(VuDev *dev, VuVirtq *vq, int fd,
1310 int size, int offset)
1311 {
1312 int qidx = vq - dev->vq;
1313 int fd_num = 0;
1314 VhostUserMsg vmsg = {
1315 .request = VHOST_USER_SLAVE_VRING_HOST_NOTIFIER_MSG,
1316 .flags = VHOST_USER_VERSION | VHOST_USER_NEED_REPLY_MASK,
1317 .size = sizeof(vmsg.payload.area),
1318 .payload.area = {
1319 .u64 = qidx & VHOST_USER_VRING_IDX_MASK,
1320 .size = size,
1321 .offset = offset,
1322 },
1323 };
1324
1325 if (fd == -1) {
1326 vmsg.payload.area.u64 |= VHOST_USER_VRING_NOFD_MASK;
1327 } else {
1328 vmsg.fds[fd_num++] = fd;
1329 }
1330
1331 vmsg.fd_num = fd_num;
1332
1333 if (!vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_SLAVE_SEND_FD)) {
1334 return false;
1335 }
1336
1337 pthread_mutex_lock(&dev->slave_mutex);
1338 if (!vu_message_write(dev, dev->slave_fd, &vmsg)) {
1339 pthread_mutex_unlock(&dev->slave_mutex);
1340 return false;
1341 }
1342
1343 /* Also unlocks the slave_mutex */
1344 return vu_process_message_reply(dev, &vmsg);
1345 }
1346
1347 static bool
1348 vu_set_vring_call_exec(VuDev *dev, VhostUserMsg *vmsg)
1349 {
1350 int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
1351 bool nofd = vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK;
1352
1353 DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
1354
1355 if (!vu_check_queue_msg_file(dev, vmsg)) {
1356 return false;
1357 }
1358
1359 if (dev->vq[index].call_fd != -1) {
1360 close(dev->vq[index].call_fd);
1361 dev->vq[index].call_fd = -1;
1362 }
1363
1364 dev->vq[index].call_fd = nofd ? -1 : vmsg->fds[0];
1365
1366 /* in case of I/O hang after reconnecting */
1367 if (dev->vq[index].call_fd != -1 && eventfd_write(vmsg->fds[0], 1)) {
1368 return -1;
1369 }
1370
1371 DPRINT("Got call_fd: %d for vq: %d\n", dev->vq[index].call_fd, index);
1372
1373 return false;
1374 }
1375
1376 static bool
1377 vu_set_vring_err_exec(VuDev *dev, VhostUserMsg *vmsg)
1378 {
1379 int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
1380 bool nofd = vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK;
1381
1382 DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
1383
1384 if (!vu_check_queue_msg_file(dev, vmsg)) {
1385 return false;
1386 }
1387
1388 if (dev->vq[index].err_fd != -1) {
1389 close(dev->vq[index].err_fd);
1390 dev->vq[index].err_fd = -1;
1391 }
1392
1393 dev->vq[index].err_fd = nofd ? -1 : vmsg->fds[0];
1394
1395 return false;
1396 }
1397
1398 static bool
1399 vu_get_protocol_features_exec(VuDev *dev, VhostUserMsg *vmsg)
1400 {
1401 /*
1402 * Note that we support, but intentionally do not set,
1403 * VHOST_USER_PROTOCOL_F_INBAND_NOTIFICATIONS. This means that
1404 * a device implementation can return it in its callback
1405 * (get_protocol_features) if it wants to use this for
1406 * simulation, but it is otherwise not desirable (if even
1407 * implemented by the master.)
1408 */
1409 uint64_t features = 1ULL << VHOST_USER_PROTOCOL_F_MQ |
1410 1ULL << VHOST_USER_PROTOCOL_F_LOG_SHMFD |
1411 1ULL << VHOST_USER_PROTOCOL_F_SLAVE_REQ |
1412 1ULL << VHOST_USER_PROTOCOL_F_HOST_NOTIFIER |
1413 1ULL << VHOST_USER_PROTOCOL_F_SLAVE_SEND_FD |
1414 1ULL << VHOST_USER_PROTOCOL_F_REPLY_ACK |
1415 1ULL << VHOST_USER_PROTOCOL_F_CONFIGURE_MEM_SLOTS;
1416
1417 if (have_userfault()) {
1418 features |= 1ULL << VHOST_USER_PROTOCOL_F_PAGEFAULT;
1419 }
1420
1421 if (dev->iface->get_config && dev->iface->set_config) {
1422 features |= 1ULL << VHOST_USER_PROTOCOL_F_CONFIG;
1423 }
1424
1425 if (dev->iface->get_protocol_features) {
1426 features |= dev->iface->get_protocol_features(dev);
1427 }
1428
1429 vmsg_set_reply_u64(vmsg, features);
1430 return true;
1431 }
1432
1433 static bool
1434 vu_set_protocol_features_exec(VuDev *dev, VhostUserMsg *vmsg)
1435 {
1436 uint64_t features = vmsg->payload.u64;
1437
1438 DPRINT("u64: 0x%016"PRIx64"\n", features);
1439
1440 dev->protocol_features = vmsg->payload.u64;
1441
1442 if (vu_has_protocol_feature(dev,
1443 VHOST_USER_PROTOCOL_F_INBAND_NOTIFICATIONS) &&
1444 (!vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_SLAVE_REQ) ||
1445 !vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_REPLY_ACK))) {
1446 /*
1447 * The use case for using messages for kick/call is simulation, to make
1448 * the kick and call synchronous. To actually get that behaviour, both
1449 * of the other features are required.
1450 * Theoretically, one could use only kick messages, or do them without
1451 * having F_REPLY_ACK, but too many (possibly pending) messages on the
1452 * socket will eventually cause the master to hang, to avoid this in
1453 * scenarios where not desired enforce that the settings are in a way
1454 * that actually enables the simulation case.
1455 */
1456 vu_panic(dev,
1457 "F_IN_BAND_NOTIFICATIONS requires F_SLAVE_REQ && F_REPLY_ACK");
1458 return false;
1459 }
1460
1461 if (dev->iface->set_protocol_features) {
1462 dev->iface->set_protocol_features(dev, features);
1463 }
1464
1465 return false;
1466 }
1467
1468 static bool
1469 vu_get_queue_num_exec(VuDev *dev, VhostUserMsg *vmsg)
1470 {
1471 vmsg_set_reply_u64(vmsg, dev->max_queues);
1472 return true;
1473 }
1474
1475 static bool
1476 vu_set_vring_enable_exec(VuDev *dev, VhostUserMsg *vmsg)
1477 {
1478 unsigned int index = vmsg->payload.state.index;
1479 unsigned int enable = vmsg->payload.state.num;
1480
1481 DPRINT("State.index: %d\n", index);
1482 DPRINT("State.enable: %d\n", enable);
1483
1484 if (index >= dev->max_queues) {
1485 vu_panic(dev, "Invalid vring_enable index: %u", index);
1486 return false;
1487 }
1488
1489 dev->vq[index].enable = enable;
1490 return false;
1491 }
1492
1493 static bool
1494 vu_set_slave_req_fd(VuDev *dev, VhostUserMsg *vmsg)
1495 {
1496 if (vmsg->fd_num != 1) {
1497 vu_panic(dev, "Invalid slave_req_fd message (%d fd's)", vmsg->fd_num);
1498 return false;
1499 }
1500
1501 if (dev->slave_fd != -1) {
1502 close(dev->slave_fd);
1503 }
1504 dev->slave_fd = vmsg->fds[0];
1505 DPRINT("Got slave_fd: %d\n", vmsg->fds[0]);
1506
1507 return false;
1508 }
1509
1510 static bool
1511 vu_get_config(VuDev *dev, VhostUserMsg *vmsg)
1512 {
1513 int ret = -1;
1514
1515 if (dev->iface->get_config) {
1516 ret = dev->iface->get_config(dev, vmsg->payload.config.region,
1517 vmsg->payload.config.size);
1518 }
1519
1520 if (ret) {
1521 /* resize to zero to indicate an error to master */
1522 vmsg->size = 0;
1523 }
1524
1525 return true;
1526 }
1527
1528 static bool
1529 vu_set_config(VuDev *dev, VhostUserMsg *vmsg)
1530 {
1531 int ret = -1;
1532
1533 if (dev->iface->set_config) {
1534 ret = dev->iface->set_config(dev, vmsg->payload.config.region,
1535 vmsg->payload.config.offset,
1536 vmsg->payload.config.size,
1537 vmsg->payload.config.flags);
1538 if (ret) {
1539 vu_panic(dev, "Set virtio configuration space failed");
1540 }
1541 }
1542
1543 return false;
1544 }
1545
1546 static bool
1547 vu_set_postcopy_advise(VuDev *dev, VhostUserMsg *vmsg)
1548 {
1549 dev->postcopy_ufd = -1;
1550 #ifdef UFFDIO_API
1551 struct uffdio_api api_struct;
1552
1553 dev->postcopy_ufd = syscall(__NR_userfaultfd, O_CLOEXEC | O_NONBLOCK);
1554 vmsg->size = 0;
1555 #endif
1556
1557 if (dev->postcopy_ufd == -1) {
1558 vu_panic(dev, "Userfaultfd not available: %s", strerror(errno));
1559 goto out;
1560 }
1561
1562 #ifdef UFFDIO_API
1563 api_struct.api = UFFD_API;
1564 api_struct.features = 0;
1565 if (ioctl(dev->postcopy_ufd, UFFDIO_API, &api_struct)) {
1566 vu_panic(dev, "Failed UFFDIO_API: %s", strerror(errno));
1567 close(dev->postcopy_ufd);
1568 dev->postcopy_ufd = -1;
1569 goto out;
1570 }
1571 /* TODO: Stash feature flags somewhere */
1572 #endif
1573
1574 out:
1575 /* Return a ufd to the QEMU */
1576 vmsg->fd_num = 1;
1577 vmsg->fds[0] = dev->postcopy_ufd;
1578 return true; /* = send a reply */
1579 }
1580
1581 static bool
1582 vu_set_postcopy_listen(VuDev *dev, VhostUserMsg *vmsg)
1583 {
1584 if (dev->nregions) {
1585 vu_panic(dev, "Regions already registered at postcopy-listen");
1586 vmsg_set_reply_u64(vmsg, -1);
1587 return true;
1588 }
1589 dev->postcopy_listening = true;
1590
1591 vmsg_set_reply_u64(vmsg, 0);
1592 return true;
1593 }
1594
1595 static bool
1596 vu_set_postcopy_end(VuDev *dev, VhostUserMsg *vmsg)
1597 {
1598 DPRINT("%s: Entry\n", __func__);
1599 dev->postcopy_listening = false;
1600 if (dev->postcopy_ufd > 0) {
1601 close(dev->postcopy_ufd);
1602 dev->postcopy_ufd = -1;
1603 DPRINT("%s: Done close\n", __func__);
1604 }
1605
1606 vmsg_set_reply_u64(vmsg, 0);
1607 DPRINT("%s: exit\n", __func__);
1608 return true;
1609 }
1610
1611 static inline uint64_t
1612 vu_inflight_queue_size(uint16_t queue_size)
1613 {
1614 return ALIGN_UP(sizeof(VuDescStateSplit) * queue_size +
1615 sizeof(uint16_t), INFLIGHT_ALIGNMENT);
1616 }
1617
1618 static bool
1619 vu_get_inflight_fd(VuDev *dev, VhostUserMsg *vmsg)
1620 {
1621 int fd;
1622 void *addr;
1623 uint64_t mmap_size;
1624 uint16_t num_queues, queue_size;
1625
1626 if (vmsg->size != sizeof(vmsg->payload.inflight)) {
1627 vu_panic(dev, "Invalid get_inflight_fd message:%d", vmsg->size);
1628 vmsg->payload.inflight.mmap_size = 0;
1629 return true;
1630 }
1631
1632 num_queues = vmsg->payload.inflight.num_queues;
1633 queue_size = vmsg->payload.inflight.queue_size;
1634
1635 DPRINT("set_inflight_fd num_queues: %"PRId16"\n", num_queues);
1636 DPRINT("set_inflight_fd queue_size: %"PRId16"\n", queue_size);
1637
1638 mmap_size = vu_inflight_queue_size(queue_size) * num_queues;
1639
1640 addr = qemu_memfd_alloc("vhost-inflight", mmap_size,
1641 F_SEAL_GROW | F_SEAL_SHRINK | F_SEAL_SEAL,
1642 &fd, NULL);
1643
1644 if (!addr) {
1645 vu_panic(dev, "Failed to alloc vhost inflight area");
1646 vmsg->payload.inflight.mmap_size = 0;
1647 return true;
1648 }
1649
1650 memset(addr, 0, mmap_size);
1651
1652 dev->inflight_info.addr = addr;
1653 dev->inflight_info.size = vmsg->payload.inflight.mmap_size = mmap_size;
1654 dev->inflight_info.fd = vmsg->fds[0] = fd;
1655 vmsg->fd_num = 1;
1656 vmsg->payload.inflight.mmap_offset = 0;
1657
1658 DPRINT("send inflight mmap_size: %"PRId64"\n",
1659 vmsg->payload.inflight.mmap_size);
1660 DPRINT("send inflight mmap offset: %"PRId64"\n",
1661 vmsg->payload.inflight.mmap_offset);
1662
1663 return true;
1664 }
1665
1666 static bool
1667 vu_set_inflight_fd(VuDev *dev, VhostUserMsg *vmsg)
1668 {
1669 int fd, i;
1670 uint64_t mmap_size, mmap_offset;
1671 uint16_t num_queues, queue_size;
1672 void *rc;
1673
1674 if (vmsg->fd_num != 1 ||
1675 vmsg->size != sizeof(vmsg->payload.inflight)) {
1676 vu_panic(dev, "Invalid set_inflight_fd message size:%d fds:%d",
1677 vmsg->size, vmsg->fd_num);
1678 return false;
1679 }
1680
1681 fd = vmsg->fds[0];
1682 mmap_size = vmsg->payload.inflight.mmap_size;
1683 mmap_offset = vmsg->payload.inflight.mmap_offset;
1684 num_queues = vmsg->payload.inflight.num_queues;
1685 queue_size = vmsg->payload.inflight.queue_size;
1686
1687 DPRINT("set_inflight_fd mmap_size: %"PRId64"\n", mmap_size);
1688 DPRINT("set_inflight_fd mmap_offset: %"PRId64"\n", mmap_offset);
1689 DPRINT("set_inflight_fd num_queues: %"PRId16"\n", num_queues);
1690 DPRINT("set_inflight_fd queue_size: %"PRId16"\n", queue_size);
1691
1692 rc = mmap(0, mmap_size, PROT_READ | PROT_WRITE, MAP_SHARED,
1693 fd, mmap_offset);
1694
1695 if (rc == MAP_FAILED) {
1696 vu_panic(dev, "set_inflight_fd mmap error: %s", strerror(errno));
1697 return false;
1698 }
1699
1700 if (dev->inflight_info.fd) {
1701 close(dev->inflight_info.fd);
1702 }
1703
1704 if (dev->inflight_info.addr) {
1705 munmap(dev->inflight_info.addr, dev->inflight_info.size);
1706 }
1707
1708 dev->inflight_info.fd = fd;
1709 dev->inflight_info.addr = rc;
1710 dev->inflight_info.size = mmap_size;
1711
1712 for (i = 0; i < num_queues; i++) {
1713 dev->vq[i].inflight = (VuVirtqInflight *)rc;
1714 dev->vq[i].inflight->desc_num = queue_size;
1715 rc = (void *)((char *)rc + vu_inflight_queue_size(queue_size));
1716 }
1717
1718 return false;
1719 }
1720
1721 static bool
1722 vu_handle_vring_kick(VuDev *dev, VhostUserMsg *vmsg)
1723 {
1724 unsigned int index = vmsg->payload.state.index;
1725
1726 if (index >= dev->max_queues) {
1727 vu_panic(dev, "Invalid queue index: %u", index);
1728 return false;
1729 }
1730
1731 DPRINT("Got kick message: handler:%p idx:%d\n",
1732 dev->vq[index].handler, index);
1733
1734 if (!dev->vq[index].started) {
1735 dev->vq[index].started = true;
1736
1737 if (dev->iface->queue_set_started) {
1738 dev->iface->queue_set_started(dev, index, true);
1739 }
1740 }
1741
1742 if (dev->vq[index].handler) {
1743 dev->vq[index].handler(dev, index);
1744 }
1745
1746 return false;
1747 }
1748
1749 static bool vu_handle_get_max_memslots(VuDev *dev, VhostUserMsg *vmsg)
1750 {
1751 vmsg->flags = VHOST_USER_REPLY_MASK | VHOST_USER_VERSION;
1752 vmsg->size = sizeof(vmsg->payload.u64);
1753 vmsg->payload.u64 = VHOST_USER_MAX_RAM_SLOTS;
1754 vmsg->fd_num = 0;
1755
1756 if (!vu_message_write(dev, dev->sock, vmsg)) {
1757 vu_panic(dev, "Failed to send max ram slots: %s\n", strerror(errno));
1758 }
1759
1760 DPRINT("u64: 0x%016"PRIx64"\n", (uint64_t) VHOST_USER_MAX_RAM_SLOTS);
1761
1762 return false;
1763 }
1764
1765 static bool
1766 vu_process_message(VuDev *dev, VhostUserMsg *vmsg)
1767 {
1768 int do_reply = 0;
1769
1770 /* Print out generic part of the request. */
1771 DPRINT("================ Vhost user message ================\n");
1772 DPRINT("Request: %s (%d)\n", vu_request_to_string(vmsg->request),
1773 vmsg->request);
1774 DPRINT("Flags: 0x%x\n", vmsg->flags);
1775 DPRINT("Size: %d\n", vmsg->size);
1776
1777 if (vmsg->fd_num) {
1778 int i;
1779 DPRINT("Fds:");
1780 for (i = 0; i < vmsg->fd_num; i++) {
1781 DPRINT(" %d", vmsg->fds[i]);
1782 }
1783 DPRINT("\n");
1784 }
1785
1786 if (dev->iface->process_msg &&
1787 dev->iface->process_msg(dev, vmsg, &do_reply)) {
1788 return do_reply;
1789 }
1790
1791 switch (vmsg->request) {
1792 case VHOST_USER_GET_FEATURES:
1793 return vu_get_features_exec(dev, vmsg);
1794 case VHOST_USER_SET_FEATURES:
1795 return vu_set_features_exec(dev, vmsg);
1796 case VHOST_USER_GET_PROTOCOL_FEATURES:
1797 return vu_get_protocol_features_exec(dev, vmsg);
1798 case VHOST_USER_SET_PROTOCOL_FEATURES:
1799 return vu_set_protocol_features_exec(dev, vmsg);
1800 case VHOST_USER_SET_OWNER:
1801 return vu_set_owner_exec(dev, vmsg);
1802 case VHOST_USER_RESET_OWNER:
1803 return vu_reset_device_exec(dev, vmsg);
1804 case VHOST_USER_SET_MEM_TABLE:
1805 return vu_set_mem_table_exec(dev, vmsg);
1806 case VHOST_USER_SET_LOG_BASE:
1807 return vu_set_log_base_exec(dev, vmsg);
1808 case VHOST_USER_SET_LOG_FD:
1809 return vu_set_log_fd_exec(dev, vmsg);
1810 case VHOST_USER_SET_VRING_NUM:
1811 return vu_set_vring_num_exec(dev, vmsg);
1812 case VHOST_USER_SET_VRING_ADDR:
1813 return vu_set_vring_addr_exec(dev, vmsg);
1814 case VHOST_USER_SET_VRING_BASE:
1815 return vu_set_vring_base_exec(dev, vmsg);
1816 case VHOST_USER_GET_VRING_BASE:
1817 return vu_get_vring_base_exec(dev, vmsg);
1818 case VHOST_USER_SET_VRING_KICK:
1819 return vu_set_vring_kick_exec(dev, vmsg);
1820 case VHOST_USER_SET_VRING_CALL:
1821 return vu_set_vring_call_exec(dev, vmsg);
1822 case VHOST_USER_SET_VRING_ERR:
1823 return vu_set_vring_err_exec(dev, vmsg);
1824 case VHOST_USER_GET_QUEUE_NUM:
1825 return vu_get_queue_num_exec(dev, vmsg);
1826 case VHOST_USER_SET_VRING_ENABLE:
1827 return vu_set_vring_enable_exec(dev, vmsg);
1828 case VHOST_USER_SET_SLAVE_REQ_FD:
1829 return vu_set_slave_req_fd(dev, vmsg);
1830 case VHOST_USER_GET_CONFIG:
1831 return vu_get_config(dev, vmsg);
1832 case VHOST_USER_SET_CONFIG:
1833 return vu_set_config(dev, vmsg);
1834 case VHOST_USER_NONE:
1835 /* if you need processing before exit, override iface->process_msg */
1836 exit(0);
1837 case VHOST_USER_POSTCOPY_ADVISE:
1838 return vu_set_postcopy_advise(dev, vmsg);
1839 case VHOST_USER_POSTCOPY_LISTEN:
1840 return vu_set_postcopy_listen(dev, vmsg);
1841 case VHOST_USER_POSTCOPY_END:
1842 return vu_set_postcopy_end(dev, vmsg);
1843 case VHOST_USER_GET_INFLIGHT_FD:
1844 return vu_get_inflight_fd(dev, vmsg);
1845 case VHOST_USER_SET_INFLIGHT_FD:
1846 return vu_set_inflight_fd(dev, vmsg);
1847 case VHOST_USER_VRING_KICK:
1848 return vu_handle_vring_kick(dev, vmsg);
1849 case VHOST_USER_GET_MAX_MEM_SLOTS:
1850 return vu_handle_get_max_memslots(dev, vmsg);
1851 case VHOST_USER_ADD_MEM_REG:
1852 return vu_add_mem_reg(dev, vmsg);
1853 case VHOST_USER_REM_MEM_REG:
1854 return vu_rem_mem_reg(dev, vmsg);
1855 default:
1856 vmsg_close_fds(vmsg);
1857 vu_panic(dev, "Unhandled request: %d", vmsg->request);
1858 }
1859
1860 return false;
1861 }
1862
1863 bool
1864 vu_dispatch(VuDev *dev)
1865 {
1866 VhostUserMsg vmsg = { 0, };
1867 int reply_requested;
1868 bool need_reply, success = false;
1869
1870 if (!dev->read_msg(dev, dev->sock, &vmsg)) {
1871 goto end;
1872 }
1873
1874 need_reply = vmsg.flags & VHOST_USER_NEED_REPLY_MASK;
1875
1876 reply_requested = vu_process_message(dev, &vmsg);
1877 if (!reply_requested && need_reply) {
1878 vmsg_set_reply_u64(&vmsg, 0);
1879 reply_requested = 1;
1880 }
1881
1882 if (!reply_requested) {
1883 success = true;
1884 goto end;
1885 }
1886
1887 if (!vu_send_reply(dev, dev->sock, &vmsg)) {
1888 goto end;
1889 }
1890
1891 success = true;
1892
1893 end:
1894 free(vmsg.data);
1895 return success;
1896 }
1897
1898 void
1899 vu_deinit(VuDev *dev)
1900 {
1901 int i;
1902
1903 for (i = 0; i < dev->nregions; i++) {
1904 VuDevRegion *r = &dev->regions[i];
1905 void *m = (void *) (uintptr_t) r->mmap_addr;
1906 if (m != MAP_FAILED) {
1907 munmap(m, r->size + r->mmap_offset);
1908 }
1909 }
1910 dev->nregions = 0;
1911
1912 for (i = 0; i < dev->max_queues; i++) {
1913 VuVirtq *vq = &dev->vq[i];
1914
1915 if (vq->call_fd != -1) {
1916 close(vq->call_fd);
1917 vq->call_fd = -1;
1918 }
1919
1920 if (vq->kick_fd != -1) {
1921 dev->remove_watch(dev, vq->kick_fd);
1922 close(vq->kick_fd);
1923 vq->kick_fd = -1;
1924 }
1925
1926 if (vq->err_fd != -1) {
1927 close(vq->err_fd);
1928 vq->err_fd = -1;
1929 }
1930
1931 if (vq->resubmit_list) {
1932 free(vq->resubmit_list);
1933 vq->resubmit_list = NULL;
1934 }
1935
1936 vq->inflight = NULL;
1937 }
1938
1939 if (dev->inflight_info.addr) {
1940 munmap(dev->inflight_info.addr, dev->inflight_info.size);
1941 dev->inflight_info.addr = NULL;
1942 }
1943
1944 if (dev->inflight_info.fd > 0) {
1945 close(dev->inflight_info.fd);
1946 dev->inflight_info.fd = -1;
1947 }
1948
1949 vu_close_log(dev);
1950 if (dev->slave_fd != -1) {
1951 close(dev->slave_fd);
1952 dev->slave_fd = -1;
1953 }
1954 pthread_mutex_destroy(&dev->slave_mutex);
1955
1956 if (dev->sock != -1) {
1957 close(dev->sock);
1958 }
1959
1960 free(dev->vq);
1961 dev->vq = NULL;
1962 }
1963
1964 bool
1965 vu_init(VuDev *dev,
1966 uint16_t max_queues,
1967 int socket,
1968 vu_panic_cb panic,
1969 vu_read_msg_cb read_msg,
1970 vu_set_watch_cb set_watch,
1971 vu_remove_watch_cb remove_watch,
1972 const VuDevIface *iface)
1973 {
1974 uint16_t i;
1975
1976 assert(max_queues > 0);
1977 assert(socket >= 0);
1978 assert(set_watch);
1979 assert(remove_watch);
1980 assert(iface);
1981 assert(panic);
1982
1983 memset(dev, 0, sizeof(*dev));
1984
1985 dev->sock = socket;
1986 dev->panic = panic;
1987 dev->read_msg = read_msg ? read_msg : vu_message_read_default;
1988 dev->set_watch = set_watch;
1989 dev->remove_watch = remove_watch;
1990 dev->iface = iface;
1991 dev->log_call_fd = -1;
1992 pthread_mutex_init(&dev->slave_mutex, NULL);
1993 dev->slave_fd = -1;
1994 dev->max_queues = max_queues;
1995
1996 dev->vq = malloc(max_queues * sizeof(dev->vq[0]));
1997 if (!dev->vq) {
1998 DPRINT("%s: failed to malloc virtqueues\n", __func__);
1999 return false;
2000 }
2001
2002 for (i = 0; i < max_queues; i++) {
2003 dev->vq[i] = (VuVirtq) {
2004 .call_fd = -1, .kick_fd = -1, .err_fd = -1,
2005 .notification = true,
2006 };
2007 }
2008
2009 return true;
2010 }
2011
2012 VuVirtq *
2013 vu_get_queue(VuDev *dev, int qidx)
2014 {
2015 assert(qidx < dev->max_queues);
2016 return &dev->vq[qidx];
2017 }
2018
2019 bool
2020 vu_queue_enabled(VuDev *dev, VuVirtq *vq)
2021 {
2022 return vq->enable;
2023 }
2024
2025 bool
2026 vu_queue_started(const VuDev *dev, const VuVirtq *vq)
2027 {
2028 return vq->started;
2029 }
2030
2031 static inline uint16_t
2032 vring_avail_flags(VuVirtq *vq)
2033 {
2034 return lduw_le_p(&vq->vring.avail->flags);
2035 }
2036
2037 static inline uint16_t
2038 vring_avail_idx(VuVirtq *vq)
2039 {
2040 vq->shadow_avail_idx = lduw_le_p(&vq->vring.avail->idx);
2041
2042 return vq->shadow_avail_idx;
2043 }
2044
2045 static inline uint16_t
2046 vring_avail_ring(VuVirtq *vq, int i)
2047 {
2048 return lduw_le_p(&vq->vring.avail->ring[i]);
2049 }
2050
2051 static inline uint16_t
2052 vring_get_used_event(VuVirtq *vq)
2053 {
2054 return vring_avail_ring(vq, vq->vring.num);
2055 }
2056
2057 static int
2058 virtqueue_num_heads(VuDev *dev, VuVirtq *vq, unsigned int idx)
2059 {
2060 uint16_t num_heads = vring_avail_idx(vq) - idx;
2061
2062 /* Check it isn't doing very strange things with descriptor numbers. */
2063 if (num_heads > vq->vring.num) {
2064 vu_panic(dev, "Guest moved used index from %u to %u",
2065 idx, vq->shadow_avail_idx);
2066 return -1;
2067 }
2068 if (num_heads) {
2069 /* On success, callers read a descriptor at vq->last_avail_idx.
2070 * Make sure descriptor read does not bypass avail index read. */
2071 smp_rmb();
2072 }
2073
2074 return num_heads;
2075 }
2076
2077 static bool
2078 virtqueue_get_head(VuDev *dev, VuVirtq *vq,
2079 unsigned int idx, unsigned int *head)
2080 {
2081 /* Grab the next descriptor number they're advertising, and increment
2082 * the index we've seen. */
2083 *head = vring_avail_ring(vq, idx % vq->vring.num);
2084
2085 /* If their number is silly, that's a fatal mistake. */
2086 if (*head >= vq->vring.num) {
2087 vu_panic(dev, "Guest says index %u is available", *head);
2088 return false;
2089 }
2090
2091 return true;
2092 }
2093
2094 static int
2095 virtqueue_read_indirect_desc(VuDev *dev, struct vring_desc *desc,
2096 uint64_t addr, size_t len)
2097 {
2098 struct vring_desc *ori_desc;
2099 uint64_t read_len;
2100
2101 if (len > (VIRTQUEUE_MAX_SIZE * sizeof(struct vring_desc))) {
2102 return -1;
2103 }
2104
2105 if (len == 0) {
2106 return -1;
2107 }
2108
2109 while (len) {
2110 read_len = len;
2111 ori_desc = vu_gpa_to_va(dev, &read_len, addr);
2112 if (!ori_desc) {
2113 return -1;
2114 }
2115
2116 memcpy(desc, ori_desc, read_len);
2117 len -= read_len;
2118 addr += read_len;
2119 desc += read_len;
2120 }
2121
2122 return 0;
2123 }
2124
2125 enum {
2126 VIRTQUEUE_READ_DESC_ERROR = -1,
2127 VIRTQUEUE_READ_DESC_DONE = 0, /* end of chain */
2128 VIRTQUEUE_READ_DESC_MORE = 1, /* more buffers in chain */
2129 };
2130
2131 static int
2132 virtqueue_read_next_desc(VuDev *dev, struct vring_desc *desc,
2133 int i, unsigned int max, unsigned int *next)
2134 {
2135 /* If this descriptor says it doesn't chain, we're done. */
2136 if (!(lduw_le_p(&desc[i].flags) & VRING_DESC_F_NEXT)) {
2137 return VIRTQUEUE_READ_DESC_DONE;
2138 }
2139
2140 /* Check they're not leading us off end of descriptors. */
2141 *next = lduw_le_p(&desc[i].next);
2142 /* Make sure compiler knows to grab that: we don't want it changing! */
2143 smp_wmb();
2144
2145 if (*next >= max) {
2146 vu_panic(dev, "Desc next is %u", *next);
2147 return VIRTQUEUE_READ_DESC_ERROR;
2148 }
2149
2150 return VIRTQUEUE_READ_DESC_MORE;
2151 }
2152
2153 void
2154 vu_queue_get_avail_bytes(VuDev *dev, VuVirtq *vq, unsigned int *in_bytes,
2155 unsigned int *out_bytes,
2156 unsigned max_in_bytes, unsigned max_out_bytes)
2157 {
2158 unsigned int idx;
2159 unsigned int total_bufs, in_total, out_total;
2160 int rc;
2161
2162 idx = vq->last_avail_idx;
2163
2164 total_bufs = in_total = out_total = 0;
2165 if (unlikely(dev->broken) ||
2166 unlikely(!vq->vring.avail)) {
2167 goto done;
2168 }
2169
2170 while ((rc = virtqueue_num_heads(dev, vq, idx)) > 0) {
2171 unsigned int max, desc_len, num_bufs, indirect = 0;
2172 uint64_t desc_addr, read_len;
2173 struct vring_desc *desc;
2174 struct vring_desc desc_buf[VIRTQUEUE_MAX_SIZE];
2175 unsigned int i;
2176
2177 max = vq->vring.num;
2178 num_bufs = total_bufs;
2179 if (!virtqueue_get_head(dev, vq, idx++, &i)) {
2180 goto err;
2181 }
2182 desc = vq->vring.desc;
2183
2184 if (lduw_le_p(&desc[i].flags) & VRING_DESC_F_INDIRECT) {
2185 if (ldl_le_p(&desc[i].len) % sizeof(struct vring_desc)) {
2186 vu_panic(dev, "Invalid size for indirect buffer table");
2187 goto err;
2188 }
2189
2190 /* If we've got too many, that implies a descriptor loop. */
2191 if (num_bufs >= max) {
2192 vu_panic(dev, "Looped descriptor");
2193 goto err;
2194 }
2195
2196 /* loop over the indirect descriptor table */
2197 indirect = 1;
2198 desc_addr = ldq_le_p(&desc[i].addr);
2199 desc_len = ldl_le_p(&desc[i].len);
2200 max = desc_len / sizeof(struct vring_desc);
2201 read_len = desc_len;
2202 desc = vu_gpa_to_va(dev, &read_len, desc_addr);
2203 if (unlikely(desc && read_len != desc_len)) {
2204 /* Failed to use zero copy */
2205 desc = NULL;
2206 if (!virtqueue_read_indirect_desc(dev, desc_buf,
2207 desc_addr,
2208 desc_len)) {
2209 desc = desc_buf;
2210 }
2211 }
2212 if (!desc) {
2213 vu_panic(dev, "Invalid indirect buffer table");
2214 goto err;
2215 }
2216 num_bufs = i = 0;
2217 }
2218
2219 do {
2220 /* If we've got too many, that implies a descriptor loop. */
2221 if (++num_bufs > max) {
2222 vu_panic(dev, "Looped descriptor");
2223 goto err;
2224 }
2225
2226 if (lduw_le_p(&desc[i].flags) & VRING_DESC_F_WRITE) {
2227 in_total += ldl_le_p(&desc[i].len);
2228 } else {
2229 out_total += ldl_le_p(&desc[i].len);
2230 }
2231 if (in_total >= max_in_bytes && out_total >= max_out_bytes) {
2232 goto done;
2233 }
2234 rc = virtqueue_read_next_desc(dev, desc, i, max, &i);
2235 } while (rc == VIRTQUEUE_READ_DESC_MORE);
2236
2237 if (rc == VIRTQUEUE_READ_DESC_ERROR) {
2238 goto err;
2239 }
2240
2241 if (!indirect) {
2242 total_bufs = num_bufs;
2243 } else {
2244 total_bufs++;
2245 }
2246 }
2247 if (rc < 0) {
2248 goto err;
2249 }
2250 done:
2251 if (in_bytes) {
2252 *in_bytes = in_total;
2253 }
2254 if (out_bytes) {
2255 *out_bytes = out_total;
2256 }
2257 return;
2258
2259 err:
2260 in_total = out_total = 0;
2261 goto done;
2262 }
2263
2264 bool
2265 vu_queue_avail_bytes(VuDev *dev, VuVirtq *vq, unsigned int in_bytes,
2266 unsigned int out_bytes)
2267 {
2268 unsigned int in_total, out_total;
2269
2270 vu_queue_get_avail_bytes(dev, vq, &in_total, &out_total,
2271 in_bytes, out_bytes);
2272
2273 return in_bytes <= in_total && out_bytes <= out_total;
2274 }
2275
2276 /* Fetch avail_idx from VQ memory only when we really need to know if
2277 * guest has added some buffers. */
2278 bool
2279 vu_queue_empty(VuDev *dev, VuVirtq *vq)
2280 {
2281 if (unlikely(dev->broken) ||
2282 unlikely(!vq->vring.avail)) {
2283 return true;
2284 }
2285
2286 if (vq->shadow_avail_idx != vq->last_avail_idx) {
2287 return false;
2288 }
2289
2290 return vring_avail_idx(vq) == vq->last_avail_idx;
2291 }
2292
2293 static bool
2294 vring_notify(VuDev *dev, VuVirtq *vq)
2295 {
2296 uint16_t old, new;
2297 bool v;
2298
2299 /* We need to expose used array entries before checking used event. */
2300 smp_mb();
2301
2302 /* Always notify when queue is empty (when feature acknowledge) */
2303 if (vu_has_feature(dev, VIRTIO_F_NOTIFY_ON_EMPTY) &&
2304 !vq->inuse && vu_queue_empty(dev, vq)) {
2305 return true;
2306 }
2307
2308 if (!vu_has_feature(dev, VIRTIO_RING_F_EVENT_IDX)) {
2309 return !(vring_avail_flags(vq) & VRING_AVAIL_F_NO_INTERRUPT);
2310 }
2311
2312 v = vq->signalled_used_valid;
2313 vq->signalled_used_valid = true;
2314 old = vq->signalled_used;
2315 new = vq->signalled_used = vq->used_idx;
2316 return !v || vring_need_event(vring_get_used_event(vq), new, old);
2317 }
2318
2319 static void _vu_queue_notify(VuDev *dev, VuVirtq *vq, bool sync)
2320 {
2321 if (unlikely(dev->broken) ||
2322 unlikely(!vq->vring.avail)) {
2323 return;
2324 }
2325
2326 if (!vring_notify(dev, vq)) {
2327 DPRINT("skipped notify...\n");
2328 return;
2329 }
2330
2331 if (vq->call_fd < 0 &&
2332 vu_has_protocol_feature(dev,
2333 VHOST_USER_PROTOCOL_F_INBAND_NOTIFICATIONS) &&
2334 vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_SLAVE_REQ)) {
2335 VhostUserMsg vmsg = {
2336 .request = VHOST_USER_SLAVE_VRING_CALL,
2337 .flags = VHOST_USER_VERSION,
2338 .size = sizeof(vmsg.payload.state),
2339 .payload.state = {
2340 .index = vq - dev->vq,
2341 },
2342 };
2343 bool ack = sync &&
2344 vu_has_protocol_feature(dev,
2345 VHOST_USER_PROTOCOL_F_REPLY_ACK);
2346
2347 if (ack) {
2348 vmsg.flags |= VHOST_USER_NEED_REPLY_MASK;
2349 }
2350
2351 vu_message_write(dev, dev->slave_fd, &vmsg);
2352 if (ack) {
2353 vu_message_read_default(dev, dev->slave_fd, &vmsg);
2354 }
2355 return;
2356 }
2357
2358 if (eventfd_write(vq->call_fd, 1) < 0) {
2359 vu_panic(dev, "Error writing eventfd: %s", strerror(errno));
2360 }
2361 }
2362
2363 void vu_queue_notify(VuDev *dev, VuVirtq *vq)
2364 {
2365 _vu_queue_notify(dev, vq, false);
2366 }
2367
2368 void vu_queue_notify_sync(VuDev *dev, VuVirtq *vq)
2369 {
2370 _vu_queue_notify(dev, vq, true);
2371 }
2372
2373 static inline void
2374 vring_used_flags_set_bit(VuVirtq *vq, int mask)
2375 {
2376 uint16_t *flags;
2377
2378 flags = (uint16_t *)((char*)vq->vring.used +
2379 offsetof(struct vring_used, flags));
2380 stw_le_p(flags, lduw_le_p(flags) | mask);
2381 }
2382
2383 static inline void
2384 vring_used_flags_unset_bit(VuVirtq *vq, int mask)
2385 {
2386 uint16_t *flags;
2387
2388 flags = (uint16_t *)((char*)vq->vring.used +
2389 offsetof(struct vring_used, flags));
2390 stw_le_p(flags, lduw_le_p(flags) & ~mask);
2391 }
2392
2393 static inline void
2394 vring_set_avail_event(VuVirtq *vq, uint16_t val)
2395 {
2396 if (!vq->notification) {
2397 return;
2398 }
2399
2400 stw_le_p(&vq->vring.used->ring[vq->vring.num], val);
2401 }
2402
2403 void
2404 vu_queue_set_notification(VuDev *dev, VuVirtq *vq, int enable)
2405 {
2406 vq->notification = enable;
2407 if (vu_has_feature(dev, VIRTIO_RING_F_EVENT_IDX)) {
2408 vring_set_avail_event(vq, vring_avail_idx(vq));
2409 } else if (enable) {
2410 vring_used_flags_unset_bit(vq, VRING_USED_F_NO_NOTIFY);
2411 } else {
2412 vring_used_flags_set_bit(vq, VRING_USED_F_NO_NOTIFY);
2413 }
2414 if (enable) {
2415 /* Expose avail event/used flags before caller checks the avail idx. */
2416 smp_mb();
2417 }
2418 }
2419
2420 static bool
2421 virtqueue_map_desc(VuDev *dev,
2422 unsigned int *p_num_sg, struct iovec *iov,
2423 unsigned int max_num_sg, bool is_write,
2424 uint64_t pa, size_t sz)
2425 {
2426 unsigned num_sg = *p_num_sg;
2427
2428 assert(num_sg <= max_num_sg);
2429
2430 if (!sz) {
2431 vu_panic(dev, "virtio: zero sized buffers are not allowed");
2432 return false;
2433 }
2434
2435 while (sz) {
2436 uint64_t len = sz;
2437
2438 if (num_sg == max_num_sg) {
2439 vu_panic(dev, "virtio: too many descriptors in indirect table");
2440 return false;
2441 }
2442
2443 iov[num_sg].iov_base = vu_gpa_to_va(dev, &len, pa);
2444 if (iov[num_sg].iov_base == NULL) {
2445 vu_panic(dev, "virtio: invalid address for buffers");
2446 return false;
2447 }
2448 iov[num_sg].iov_len = len;
2449 num_sg++;
2450 sz -= len;
2451 pa += len;
2452 }
2453
2454 *p_num_sg = num_sg;
2455 return true;
2456 }
2457
2458 static void *
2459 virtqueue_alloc_element(size_t sz,
2460 unsigned out_num, unsigned in_num)
2461 {
2462 VuVirtqElement *elem;
2463 size_t in_sg_ofs = ALIGN_UP(sz, __alignof__(elem->in_sg[0]));
2464 size_t out_sg_ofs = in_sg_ofs + in_num * sizeof(elem->in_sg[0]);
2465 size_t out_sg_end = out_sg_ofs + out_num * sizeof(elem->out_sg[0]);
2466
2467 assert(sz >= sizeof(VuVirtqElement));
2468 elem = malloc(out_sg_end);
2469 elem->out_num = out_num;
2470 elem->in_num = in_num;
2471 elem->in_sg = (void *)elem + in_sg_ofs;
2472 elem->out_sg = (void *)elem + out_sg_ofs;
2473 return elem;
2474 }
2475
2476 static void *
2477 vu_queue_map_desc(VuDev *dev, VuVirtq *vq, unsigned int idx, size_t sz)
2478 {
2479 struct vring_desc *desc = vq->vring.desc;
2480 uint64_t desc_addr, read_len;
2481 unsigned int desc_len;
2482 unsigned int max = vq->vring.num;
2483 unsigned int i = idx;
2484 VuVirtqElement *elem;
2485 unsigned int out_num = 0, in_num = 0;
2486 struct iovec iov[VIRTQUEUE_MAX_SIZE];
2487 struct vring_desc desc_buf[VIRTQUEUE_MAX_SIZE];
2488 int rc;
2489
2490 if (lduw_le_p(&desc[i].flags) & VRING_DESC_F_INDIRECT) {
2491 if (ldl_le_p(&desc[i].len) % sizeof(struct vring_desc)) {
2492 vu_panic(dev, "Invalid size for indirect buffer table");
2493 return NULL;
2494 }
2495
2496 /* loop over the indirect descriptor table */
2497 desc_addr = ldq_le_p(&desc[i].addr);
2498 desc_len = ldl_le_p(&desc[i].len);
2499 max = desc_len / sizeof(struct vring_desc);
2500 read_len = desc_len;
2501 desc = vu_gpa_to_va(dev, &read_len, desc_addr);
2502 if (unlikely(desc && read_len != desc_len)) {
2503 /* Failed to use zero copy */
2504 desc = NULL;
2505 if (!virtqueue_read_indirect_desc(dev, desc_buf,
2506 desc_addr,
2507 desc_len)) {
2508 desc = desc_buf;
2509 }
2510 }
2511 if (!desc) {
2512 vu_panic(dev, "Invalid indirect buffer table");
2513 return NULL;
2514 }
2515 i = 0;
2516 }
2517
2518 /* Collect all the descriptors */
2519 do {
2520 if (lduw_le_p(&desc[i].flags) & VRING_DESC_F_WRITE) {
2521 if (!virtqueue_map_desc(dev, &in_num, iov + out_num,
2522 VIRTQUEUE_MAX_SIZE - out_num, true,
2523 ldq_le_p(&desc[i].addr),
2524 ldl_le_p(&desc[i].len))) {
2525 return NULL;
2526 }
2527 } else {
2528 if (in_num) {
2529 vu_panic(dev, "Incorrect order for descriptors");
2530 return NULL;
2531 }
2532 if (!virtqueue_map_desc(dev, &out_num, iov,
2533 VIRTQUEUE_MAX_SIZE, false,
2534 ldq_le_p(&desc[i].addr),
2535 ldl_le_p(&desc[i].len))) {
2536 return NULL;
2537 }
2538 }
2539
2540 /* If we've got too many, that implies a descriptor loop. */
2541 if ((in_num + out_num) > max) {
2542 vu_panic(dev, "Looped descriptor");
2543 return NULL;
2544 }
2545 rc = virtqueue_read_next_desc(dev, desc, i, max, &i);
2546 } while (rc == VIRTQUEUE_READ_DESC_MORE);
2547
2548 if (rc == VIRTQUEUE_READ_DESC_ERROR) {
2549 vu_panic(dev, "read descriptor error");
2550 return NULL;
2551 }
2552
2553 /* Now copy what we have collected and mapped */
2554 elem = virtqueue_alloc_element(sz, out_num, in_num);
2555 elem->index = idx;
2556 for (i = 0; i < out_num; i++) {
2557 elem->out_sg[i] = iov[i];
2558 }
2559 for (i = 0; i < in_num; i++) {
2560 elem->in_sg[i] = iov[out_num + i];
2561 }
2562
2563 return elem;
2564 }
2565
2566 static int
2567 vu_queue_inflight_get(VuDev *dev, VuVirtq *vq, int desc_idx)
2568 {
2569 if (!vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_INFLIGHT_SHMFD)) {
2570 return 0;
2571 }
2572
2573 if (unlikely(!vq->inflight)) {
2574 return -1;
2575 }
2576
2577 vq->inflight->desc[desc_idx].counter = vq->counter++;
2578 vq->inflight->desc[desc_idx].inflight = 1;
2579
2580 return 0;
2581 }
2582
2583 static int
2584 vu_queue_inflight_pre_put(VuDev *dev, VuVirtq *vq, int desc_idx)
2585 {
2586 if (!vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_INFLIGHT_SHMFD)) {
2587 return 0;
2588 }
2589
2590 if (unlikely(!vq->inflight)) {
2591 return -1;
2592 }
2593
2594 vq->inflight->last_batch_head = desc_idx;
2595
2596 return 0;
2597 }
2598
2599 static int
2600 vu_queue_inflight_post_put(VuDev *dev, VuVirtq *vq, int desc_idx)
2601 {
2602 if (!vu_has_protocol_feature(dev, VHOST_USER_PROTOCOL_F_INFLIGHT_SHMFD)) {
2603 return 0;
2604 }
2605
2606 if (unlikely(!vq->inflight)) {
2607 return -1;
2608 }
2609
2610 barrier();
2611
2612 vq->inflight->desc[desc_idx].inflight = 0;
2613
2614 barrier();
2615
2616 vq->inflight->used_idx = vq->used_idx;
2617
2618 return 0;
2619 }
2620
2621 void *
2622 vu_queue_pop(VuDev *dev, VuVirtq *vq, size_t sz)
2623 {
2624 int i;
2625 unsigned int head;
2626 VuVirtqElement *elem;
2627
2628 if (unlikely(dev->broken) ||
2629 unlikely(!vq->vring.avail)) {
2630 return NULL;
2631 }
2632
2633 if (unlikely(vq->resubmit_list && vq->resubmit_num > 0)) {
2634 i = (--vq->resubmit_num);
2635 elem = vu_queue_map_desc(dev, vq, vq->resubmit_list[i].index, sz);
2636
2637 if (!vq->resubmit_num) {
2638 free(vq->resubmit_list);
2639 vq->resubmit_list = NULL;
2640 }
2641
2642 return elem;
2643 }
2644
2645 if (vu_queue_empty(dev, vq)) {
2646 return NULL;
2647 }
2648 /*
2649 * Needed after virtio_queue_empty(), see comment in
2650 * virtqueue_num_heads().
2651 */
2652 smp_rmb();
2653
2654 if (vq->inuse >= vq->vring.num) {
2655 vu_panic(dev, "Virtqueue size exceeded");
2656 return NULL;
2657 }
2658
2659 if (!virtqueue_get_head(dev, vq, vq->last_avail_idx++, &head)) {
2660 return NULL;
2661 }
2662
2663 if (vu_has_feature(dev, VIRTIO_RING_F_EVENT_IDX)) {
2664 vring_set_avail_event(vq, vq->last_avail_idx);
2665 }
2666
2667 elem = vu_queue_map_desc(dev, vq, head, sz);
2668
2669 if (!elem) {
2670 return NULL;
2671 }
2672
2673 vq->inuse++;
2674
2675 vu_queue_inflight_get(dev, vq, head);
2676
2677 return elem;
2678 }
2679
2680 static void
2681 vu_queue_detach_element(VuDev *dev, VuVirtq *vq, VuVirtqElement *elem,
2682 size_t len)
2683 {
2684 vq->inuse--;
2685 /* unmap, when DMA support is added */
2686 }
2687
2688 void
2689 vu_queue_unpop(VuDev *dev, VuVirtq *vq, VuVirtqElement *elem,
2690 size_t len)
2691 {
2692 vq->last_avail_idx--;
2693 vu_queue_detach_element(dev, vq, elem, len);
2694 }
2695
2696 bool
2697 vu_queue_rewind(VuDev *dev, VuVirtq *vq, unsigned int num)
2698 {
2699 if (num > vq->inuse) {
2700 return false;
2701 }
2702 vq->last_avail_idx -= num;
2703 vq->inuse -= num;
2704 return true;
2705 }
2706
2707 static inline
2708 void vring_used_write(VuDev *dev, VuVirtq *vq,
2709 struct vring_used_elem *uelem, int i)
2710 {
2711 struct vring_used *used = vq->vring.used;
2712
2713 used->ring[i] = *uelem;
2714 vu_log_write(dev, vq->vring.log_guest_addr +
2715 offsetof(struct vring_used, ring[i]),
2716 sizeof(used->ring[i]));
2717 }
2718
2719
2720 static void
2721 vu_log_queue_fill(VuDev *dev, VuVirtq *vq,
2722 const VuVirtqElement *elem,
2723 unsigned int len)
2724 {
2725 struct vring_desc *desc = vq->vring.desc;
2726 unsigned int i, max, min, desc_len;
2727 uint64_t desc_addr, read_len;
2728 struct vring_desc desc_buf[VIRTQUEUE_MAX_SIZE];
2729 unsigned num_bufs = 0;
2730
2731 max = vq->vring.num;
2732 i = elem->index;
2733
2734 if (lduw_le_p(&desc[i].flags) & VRING_DESC_F_INDIRECT) {
2735 if (ldl_le_p(&desc[i].len) % sizeof(struct vring_desc)) {
2736 vu_panic(dev, "Invalid size for indirect buffer table");
2737 return;
2738 }
2739
2740 /* loop over the indirect descriptor table */
2741 desc_addr = ldq_le_p(&desc[i].addr);
2742 desc_len = ldl_le_p(&desc[i].len);
2743 max = desc_len / sizeof(struct vring_desc);
2744 read_len = desc_len;
2745 desc = vu_gpa_to_va(dev, &read_len, desc_addr);
2746 if (unlikely(desc && read_len != desc_len)) {
2747 /* Failed to use zero copy */
2748 desc = NULL;
2749 if (!virtqueue_read_indirect_desc(dev, desc_buf,
2750 desc_addr,
2751 desc_len)) {
2752 desc = desc_buf;
2753 }
2754 }
2755 if (!desc) {
2756 vu_panic(dev, "Invalid indirect buffer table");
2757 return;
2758 }
2759 i = 0;
2760 }
2761
2762 do {
2763 if (++num_bufs > max) {
2764 vu_panic(dev, "Looped descriptor");
2765 return;
2766 }
2767
2768 if (lduw_le_p(&desc[i].flags) & VRING_DESC_F_WRITE) {
2769 min = MIN(ldl_le_p(&desc[i].len), len);
2770 vu_log_write(dev, ldq_le_p(&desc[i].addr), min);
2771 len -= min;
2772 }
2773
2774 } while (len > 0 &&
2775 (virtqueue_read_next_desc(dev, desc, i, max, &i)
2776 == VIRTQUEUE_READ_DESC_MORE));
2777 }
2778
2779 void
2780 vu_queue_fill(VuDev *dev, VuVirtq *vq,
2781 const VuVirtqElement *elem,
2782 unsigned int len, unsigned int idx)
2783 {
2784 struct vring_used_elem uelem;
2785
2786 if (unlikely(dev->broken) ||
2787 unlikely(!vq->vring.avail)) {
2788 return;
2789 }
2790
2791 vu_log_queue_fill(dev, vq, elem, len);
2792
2793 idx = (idx + vq->used_idx) % vq->vring.num;
2794
2795 stl_le_p(&uelem.id, elem->index);
2796 stl_le_p(&uelem.len, len);
2797 vring_used_write(dev, vq, &uelem, idx);
2798 }
2799
2800 static inline
2801 void vring_used_idx_set(VuDev *dev, VuVirtq *vq, uint16_t val)
2802 {
2803 stw_le_p(&vq->vring.used->idx, val);
2804 vu_log_write(dev,
2805 vq->vring.log_guest_addr + offsetof(struct vring_used, idx),
2806 sizeof(vq->vring.used->idx));
2807
2808 vq->used_idx = val;
2809 }
2810
2811 void
2812 vu_queue_flush(VuDev *dev, VuVirtq *vq, unsigned int count)
2813 {
2814 uint16_t old, new;
2815
2816 if (unlikely(dev->broken) ||
2817 unlikely(!vq->vring.avail)) {
2818 return;
2819 }
2820
2821 /* Make sure buffer is written before we update index. */
2822 smp_wmb();
2823
2824 old = vq->used_idx;
2825 new = old + count;
2826 vring_used_idx_set(dev, vq, new);
2827 vq->inuse -= count;
2828 if (unlikely((int16_t)(new - vq->signalled_used) < (uint16_t)(new - old))) {
2829 vq->signalled_used_valid = false;
2830 }
2831 }
2832
2833 void
2834 vu_queue_push(VuDev *dev, VuVirtq *vq,
2835 const VuVirtqElement *elem, unsigned int len)
2836 {
2837 vu_queue_fill(dev, vq, elem, len, 0);
2838 vu_queue_inflight_pre_put(dev, vq, elem->index);
2839 vu_queue_flush(dev, vq, 1);
2840 vu_queue_inflight_post_put(dev, vq, elem->index);
2841 }