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 #include <qemu/osdep.h>
  17 #include <sys/eventfd.h>
  18 #include <linux/vhost.h>
  19
  20 #include "qemu/atomic.h"
  21
  22 #include "libvhost-user.h"
  23
  24 #define VHOST_USER_HDR_SIZE offsetof(VhostUserMsg, payload.u64)
  25
  26 /* The version of the protocol we support */
  27 #define VHOST_USER_VERSION 1
  28 #define LIBVHOST_USER_DEBUG 0
  29
  30 #define DPRINT(...)                             \
  31     do {                                        \
  32         if (LIBVHOST_USER_DEBUG) {              \
  33             fprintf(stderr, __VA_ARGS__);        \
  34         }                                       \
  35     } while (0)
  36
  37 static const char *
  38 vu_request_to_string(int req)
  39 {
  40 #define REQ(req) [req] = #req
  41     static const char *vu_request_str[] = {
  42         REQ(VHOST_USER_NONE),
  43         REQ(VHOST_USER_GET_FEATURES),
  44         REQ(VHOST_USER_SET_FEATURES),
  45         REQ(VHOST_USER_NONE),
  46         REQ(VHOST_USER_GET_FEATURES),
  47         REQ(VHOST_USER_SET_FEATURES),
  48         REQ(VHOST_USER_SET_OWNER),
  49         REQ(VHOST_USER_RESET_OWNER),
  50         REQ(VHOST_USER_SET_MEM_TABLE),
  51         REQ(VHOST_USER_SET_LOG_BASE),
  52         REQ(VHOST_USER_SET_LOG_FD),
  53         REQ(VHOST_USER_SET_VRING_NUM),
  54         REQ(VHOST_USER_SET_VRING_ADDR),
  55         REQ(VHOST_USER_SET_VRING_BASE),
  56         REQ(VHOST_USER_GET_VRING_BASE),
  57         REQ(VHOST_USER_SET_VRING_KICK),
  58         REQ(VHOST_USER_SET_VRING_CALL),
  59         REQ(VHOST_USER_SET_VRING_ERR),
  60         REQ(VHOST_USER_GET_PROTOCOL_FEATURES),
  61         REQ(VHOST_USER_SET_PROTOCOL_FEATURES),
  62         REQ(VHOST_USER_GET_QUEUE_NUM),
  63         REQ(VHOST_USER_SET_VRING_ENABLE),
  64         REQ(VHOST_USER_SEND_RARP),
  65         REQ(VHOST_USER_INPUT_GET_CONFIG),
  66         REQ(VHOST_USER_MAX),
  67     };
  68 #undef REQ
  69
  70     if (req < VHOST_USER_MAX) {
  71         return vu_request_str[req];
  72     } else {
  73         return "unknown";
  74     }
  75 }
  76
  77 static void
  78 vu_panic(VuDev *dev, const char *msg, ...)
  79 {
  80     char *buf = NULL;
  81     va_list ap;
  82
  83     va_start(ap, msg);
  84     (void)vasprintf(&buf, msg, ap);
  85     va_end(ap);
  86
  87     dev->broken = true;
  88     dev->panic(dev, buf);
  89     free(buf);
  90
  91     /* FIXME: find a way to call virtio_error? */
  92 }
  93
  94 /* Translate guest physical address to our virtual address.  */
  95 void *
  96 vu_gpa_to_va(VuDev *dev, uint64_t guest_addr)
  97 {
  98     int i;
  99
 100     /* Find matching memory region.  */
 101     for (i = 0; i < dev->nregions; i++) {
 102         VuDevRegion *r = &dev->regions[i];
 103
 104         if ((guest_addr >= r->gpa) && (guest_addr < (r->gpa + r->size))) {
 105             return (void *)(uintptr_t)
 106                 guest_addr - r->gpa + r->mmap_addr + r->mmap_offset;
 107         }
 108     }
 109
 110     return NULL;
 111 }
 112
 113 /* Translate qemu virtual address to our virtual address.  */
 114 static void *
 115 qva_to_va(VuDev *dev, uint64_t qemu_addr)
 116 {
 117     int i;
 118
 119     /* Find matching memory region.  */
 120     for (i = 0; i < dev->nregions; i++) {
 121         VuDevRegion *r = &dev->regions[i];
 122
 123         if ((qemu_addr >= r->qva) && (qemu_addr < (r->qva + r->size))) {
 124             return (void *)(uintptr_t)
 125                 qemu_addr - r->qva + r->mmap_addr + r->mmap_offset;
 126         }
 127     }
 128
 129     return NULL;
 130 }
 131
 132 static void
 133 vmsg_close_fds(VhostUserMsg *vmsg)
 134 {
 135     int i;
 136
 137     for (i = 0; i < vmsg->fd_num; i++) {
 138         close(vmsg->fds[i]);
 139     }
 140 }
 141
 142 static bool
 143 vu_message_read(VuDev *dev, int conn_fd, VhostUserMsg *vmsg)
 144 {
 145     char control[CMSG_SPACE(VHOST_MEMORY_MAX_NREGIONS * sizeof(int))] = { };
 146     struct iovec iov = {
 147         .iov_base = (char *)vmsg,
 148         .iov_len = VHOST_USER_HDR_SIZE,
 149     };
 150     struct msghdr msg = {
 151         .msg_iov = &iov,
 152         .msg_iovlen = 1,
 153         .msg_control = control,
 154         .msg_controllen = sizeof(control),
 155     };
 156     size_t fd_size;
 157     struct cmsghdr *cmsg;
 158     int rc;
 159
 160     do {
 161         rc = recvmsg(conn_fd, &msg, 0);
 162     } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
 163
 164     if (rc <= 0) {
 165         vu_panic(dev, "Error while recvmsg: %s", strerror(errno));
 166         return false;
 167     }
 168
 169     vmsg->fd_num = 0;
 170     for (cmsg = CMSG_FIRSTHDR(&msg);
 171          cmsg != NULL;
 172          cmsg = CMSG_NXTHDR(&msg, cmsg))
 173     {
 174         if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS) {
 175             fd_size = cmsg->cmsg_len - CMSG_LEN(0);
 176             vmsg->fd_num = fd_size / sizeof(int);
 177             memcpy(vmsg->fds, CMSG_DATA(cmsg), fd_size);
 178             break;
 179         }
 180     }
 181
 182     if (vmsg->size > sizeof(vmsg->payload)) {
 183         vu_panic(dev,
 184                  "Error: too big message request: %d, size: vmsg->size: %u, "
 185                  "while sizeof(vmsg->payload) = %zu\n",
 186                  vmsg->request, vmsg->size, sizeof(vmsg->payload));
 187         goto fail;
 188     }
 189
 190     if (vmsg->size) {
 191         do {
 192             rc = read(conn_fd, &vmsg->payload, vmsg->size);
 193         } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
 194
 195         if (rc <= 0) {
 196             vu_panic(dev, "Error while reading: %s", strerror(errno));
 197             goto fail;
 198         }
 199
 200         assert(rc == vmsg->size);
 201     }
 202
 203     return true;
 204
 205 fail:
 206     vmsg_close_fds(vmsg);
 207
 208     return false;
 209 }
 210
 211 static bool
 212 vu_message_write(VuDev *dev, int conn_fd, VhostUserMsg *vmsg)
 213 {
 214     int rc;
 215     uint8_t *p = (uint8_t *)vmsg;
 216
 217     /* Set the version in the flags when sending the reply */
 218     vmsg->flags &= ~VHOST_USER_VERSION_MASK;
 219     vmsg->flags |= VHOST_USER_VERSION;
 220     vmsg->flags |= VHOST_USER_REPLY_MASK;
 221
 222     do {
 223         rc = write(conn_fd, p, VHOST_USER_HDR_SIZE);
 224     } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
 225
 226     do {
 227         if (vmsg->data) {
 228             rc = write(conn_fd, vmsg->data, vmsg->size);
 229         } else {
 230             rc = write(conn_fd, p + VHOST_USER_HDR_SIZE, vmsg->size);
 231         }
 232     } while (rc < 0 && (errno == EINTR || errno == EAGAIN));
 233
 234     if (rc <= 0) {
 235         vu_panic(dev, "Error while writing: %s", strerror(errno));
 236         return false;
 237     }
 238
 239     return true;
 240 }
 241
 242 /* Kick the log_call_fd if required. */
 243 static void
 244 vu_log_kick(VuDev *dev)
 245 {
 246     if (dev->log_call_fd != -1) {
 247         DPRINT("Kicking the QEMU's log...\n");
 248         if (eventfd_write(dev->log_call_fd, 1) < 0) {
 249             vu_panic(dev, "Error writing eventfd: %s", strerror(errno));
 250         }
 251     }
 252 }
 253
 254 static void
 255 vu_log_page(uint8_t *log_table, uint64_t page)
 256 {
 257     DPRINT("Logged dirty guest page: %"PRId64"\n", page);
 258     atomic_or(&log_table[page / 8], 1 << (page % 8));
 259 }
 260
 261 static void
 262 vu_log_write(VuDev *dev, uint64_t address, uint64_t length)
 263 {
 264     uint64_t page;
 265
 266     if (!(dev->features & (1ULL << VHOST_F_LOG_ALL)) ||
 267         !dev->log_table || !length) {
 268         return;
 269     }
 270
 271     assert(dev->log_size > ((address + length - 1) / VHOST_LOG_PAGE / 8));
 272
 273     page = address / VHOST_LOG_PAGE;
 274     while (page * VHOST_LOG_PAGE < address + length) {
 275         vu_log_page(dev->log_table, page);
 276         page += VHOST_LOG_PAGE;
 277     }
 278
 279     vu_log_kick(dev);
 280 }
 281
 282 static void
 283 vu_kick_cb(VuDev *dev, int condition, void *data)
 284 {
 285     int index = (intptr_t)data;
 286     VuVirtq *vq = &dev->vq[index];
 287     int sock = vq->kick_fd;
 288     eventfd_t kick_data;
 289     ssize_t rc;
 290
 291     rc = eventfd_read(sock, &kick_data);
 292     if (rc == -1) {
 293         vu_panic(dev, "kick eventfd_read(): %s", strerror(errno));
 294         dev->remove_watch(dev, dev->vq[index].kick_fd);
 295     } else {
 296         DPRINT("Got kick_data: %016"PRIx64" handler:%p idx:%d\n",
 297                kick_data, vq->handler, index);
 298         if (vq->handler) {
 299             vq->handler(dev, index);
 300         }
 301     }
 302 }
 303
 304 static bool
 305 vu_get_features_exec(VuDev *dev, VhostUserMsg *vmsg)
 306 {
 307     vmsg->payload.u64 =
 308         1ULL << VHOST_F_LOG_ALL |
 309         1ULL << VHOST_USER_F_PROTOCOL_FEATURES;
 310
 311     if (dev->iface->get_features) {
 312         vmsg->payload.u64 |= dev->iface->get_features(dev);
 313     }
 314
 315     vmsg->size = sizeof(vmsg->payload.u64);
 316
 317     DPRINT("Sending back to guest u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
 318
 319     return true;
 320 }
 321
 322 static void
 323 vu_set_enable_all_rings(VuDev *dev, bool enabled)
 324 {
 325     int i;
 326
 327     for (i = 0; i < VHOST_MAX_NR_VIRTQUEUE; i++) {
 328         dev->vq[i].enable = enabled;
 329     }
 330 }
 331
 332 static bool
 333 vu_set_features_exec(VuDev *dev, VhostUserMsg *vmsg)
 334 {
 335     DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
 336
 337     dev->features = vmsg->payload.u64;
 338
 339     if (!(dev->features & VHOST_USER_F_PROTOCOL_FEATURES)) {
 340         vu_set_enable_all_rings(dev, true);
 341     }
 342
 343     if (dev->iface->set_features) {
 344         dev->iface->set_features(dev, dev->features);
 345     }
 346
 347     return false;
 348 }
 349
 350 static bool
 351 vu_set_owner_exec(VuDev *dev, VhostUserMsg *vmsg)
 352 {
 353     return false;
 354 }
 355
 356 static void
 357 vu_close_log(VuDev *dev)
 358 {
 359     if (dev->log_table) {
 360         if (munmap(dev->log_table, dev->log_size) != 0) {
 361             perror("close log munmap() error");
 362         }
 363
 364         dev->log_table = NULL;
 365     }
 366     if (dev->log_call_fd != -1) {
 367         close(dev->log_call_fd);
 368         dev->log_call_fd = -1;
 369     }
 370 }
 371
 372 static bool
 373 vu_reset_device_exec(VuDev *dev, VhostUserMsg *vmsg)
 374 {
 375     vu_set_enable_all_rings(dev, false);
 376
 377     return false;
 378 }
 379
 380 static bool
 381 vu_set_mem_table_exec(VuDev *dev, VhostUserMsg *vmsg)
 382 {
 383     int i;
 384     VhostUserMemory *memory = &vmsg->payload.memory;
 385     dev->nregions = memory->nregions;
 386
 387     DPRINT("Nregions: %d\n", memory->nregions);
 388     for (i = 0; i < dev->nregions; i++) {
 389         void *mmap_addr;
 390         VhostUserMemoryRegion *msg_region = &memory->regions[i];
 391         VuDevRegion *dev_region = &dev->regions[i];
 392
 393         DPRINT("Region %d\n", i);
 394         DPRINT("    guest_phys_addr: 0x%016"PRIx64"\n",
 395                msg_region->guest_phys_addr);
 396         DPRINT("    memory_size:     0x%016"PRIx64"\n",
 397                msg_region->memory_size);
 398         DPRINT("    userspace_addr   0x%016"PRIx64"\n",
 399                msg_region->userspace_addr);
 400         DPRINT("    mmap_offset      0x%016"PRIx64"\n",
 401                msg_region->mmap_offset);
 402
 403         dev_region->gpa = msg_region->guest_phys_addr;
 404         dev_region->size = msg_region->memory_size;
 405         dev_region->qva = msg_region->userspace_addr;
 406         dev_region->mmap_offset = msg_region->mmap_offset;
 407
 408         /* We don't use offset argument of mmap() since the
 409          * mapped address has to be page aligned, and we use huge
 410          * pages.  */
 411         mmap_addr = mmap(0, dev_region->size + dev_region->mmap_offset,
 412                          PROT_READ | PROT_WRITE, MAP_SHARED,
 413                          vmsg->fds[i], 0);
 414
 415         if (mmap_addr == MAP_FAILED) {
 416             vu_panic(dev, "region mmap error: %s", strerror(errno));
 417         } else {
 418             dev_region->mmap_addr = (uint64_t)(uintptr_t)mmap_addr;
 419             DPRINT("    mmap_addr:       0x%016"PRIx64"\n",
 420                    dev_region->mmap_addr);
 421         }
 422
 423         close(vmsg->fds[i]);
 424     }
 425
 426     return false;
 427 }
 428
 429 static bool
 430 vu_set_log_base_exec(VuDev *dev, VhostUserMsg *vmsg)
 431 {
 432     int fd;
 433     uint64_t log_mmap_size, log_mmap_offset;
 434     void *rc;
 435
 436     if (vmsg->fd_num != 1 ||
 437         vmsg->size != sizeof(vmsg->payload.log)) {
 438         vu_panic(dev, "Invalid log_base message");
 439         return true;
 440     }
 441
 442     fd = vmsg->fds[0];
 443     log_mmap_offset = vmsg->payload.log.mmap_offset;
 444     log_mmap_size = vmsg->payload.log.mmap_size;
 445     DPRINT("Log mmap_offset: %"PRId64"\n", log_mmap_offset);
 446     DPRINT("Log mmap_size:   %"PRId64"\n", log_mmap_size);
 447
 448     rc = mmap(0, log_mmap_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
 449               log_mmap_offset);
 450     if (rc == MAP_FAILED) {
 451         perror("log mmap error");
 452     }
 453     dev->log_table = rc;
 454     dev->log_size = log_mmap_size;
 455
 456     vmsg->size = sizeof(vmsg->payload.u64);
 457
 458     return true;
 459 }
 460
 461 static bool
 462 vu_set_log_fd_exec(VuDev *dev, VhostUserMsg *vmsg)
 463 {
 464     if (vmsg->fd_num != 1) {
 465         vu_panic(dev, "Invalid log_fd message");
 466         return false;
 467     }
 468
 469     if (dev->log_call_fd != -1) {
 470         close(dev->log_call_fd);
 471     }
 472     dev->log_call_fd = vmsg->fds[0];
 473     DPRINT("Got log_call_fd: %d\n", vmsg->fds[0]);
 474
 475     return false;
 476 }
 477
 478 static bool
 479 vu_set_vring_num_exec(VuDev *dev, VhostUserMsg *vmsg)
 480 {
 481     unsigned int index = vmsg->payload.state.index;
 482     unsigned int num = vmsg->payload.state.num;
 483
 484     DPRINT("State.index: %d\n", index);
 485     DPRINT("State.num:   %d\n", num);
 486     dev->vq[index].vring.num = num;
 487
 488     return false;
 489 }
 490
 491 static bool
 492 vu_set_vring_addr_exec(VuDev *dev, VhostUserMsg *vmsg)
 493 {
 494     struct vhost_vring_addr *vra = &vmsg->payload.addr;
 495     unsigned int index = vra->index;
 496     VuVirtq *vq = &dev->vq[index];
 497
 498     DPRINT("vhost_vring_addr:\n");
 499     DPRINT("    index:  %d\n", vra->index);
 500     DPRINT("    flags:  %d\n", vra->flags);
 501     DPRINT("    desc_user_addr:   0x%016llx\n", vra->desc_user_addr);
 502     DPRINT("    used_user_addr:   0x%016llx\n", vra->used_user_addr);
 503     DPRINT("    avail_user_addr:  0x%016llx\n", vra->avail_user_addr);
 504     DPRINT("    log_guest_addr:   0x%016llx\n", vra->log_guest_addr);
 505
 506     vq->vring.flags = vra->flags;
 507     vq->vring.desc = qva_to_va(dev, vra->desc_user_addr);
 508     vq->vring.used = qva_to_va(dev, vra->used_user_addr);
 509     vq->vring.avail = qva_to_va(dev, vra->avail_user_addr);
 510     vq->vring.log_guest_addr = vra->log_guest_addr;
 511
 512     DPRINT("Setting virtq addresses:\n");
 513     DPRINT("    vring_desc  at %p\n", vq->vring.desc);
 514     DPRINT("    vring_used  at %p\n", vq->vring.used);
 515     DPRINT("    vring_avail at %p\n", vq->vring.avail);
 516
 517     if (!(vq->vring.desc && vq->vring.used && vq->vring.avail)) {
 518         vu_panic(dev, "Invalid vring_addr message");
 519         return false;
 520     }
 521
 522     vq->used_idx = vq->vring.used->idx;
 523
 524     return false;
 525 }
 526
 527 static bool
 528 vu_set_vring_base_exec(VuDev *dev, VhostUserMsg *vmsg)
 529 {
 530     unsigned int index = vmsg->payload.state.index;
 531     unsigned int num = vmsg->payload.state.num;
 532
 533     DPRINT("State.index: %d\n", index);
 534     DPRINT("State.num:   %d\n", num);
 535     dev->vq[index].shadow_avail_idx = dev->vq[index].last_avail_idx = num;
 536
 537     return false;
 538 }
 539
 540 static bool
 541 vu_get_vring_base_exec(VuDev *dev, VhostUserMsg *vmsg)
 542 {
 543     unsigned int index = vmsg->payload.state.index;
 544
 545     DPRINT("State.index: %d\n", index);
 546     vmsg->payload.state.num = dev->vq[index].last_avail_idx;
 547     vmsg->size = sizeof(vmsg->payload.state);
 548
 549     dev->vq[index].started = false;
 550     if (dev->iface->queue_set_started) {
 551         dev->iface->queue_set_started(dev, index, false);
 552     }
 553
 554     if (dev->vq[index].call_fd != -1) {
 555         close(dev->vq[index].call_fd);
 556         dev->vq[index].call_fd = -1;
 557     }
 558     if (dev->vq[index].kick_fd != -1) {
 559         dev->remove_watch(dev, dev->vq[index].kick_fd);
 560         close(dev->vq[index].kick_fd);
 561         dev->vq[index].kick_fd = -1;
 562     }
 563
 564     return true;
 565 }
 566
 567 static bool
 568 vu_check_queue_msg_file(VuDev *dev, VhostUserMsg *vmsg)
 569 {
 570     int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
 571
 572     if (index >= VHOST_MAX_NR_VIRTQUEUE) {
 573         vmsg_close_fds(vmsg);
 574         vu_panic(dev, "Invalid queue index: %u", index);
 575         return false;
 576     }
 577
 578     if (vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK ||
 579         vmsg->fd_num != 1) {
 580         vmsg_close_fds(vmsg);
 581         vu_panic(dev, "Invalid fds in request: %d", vmsg->request);
 582         return false;
 583     }
 584
 585     return true;
 586 }
 587
 588 static bool
 589 vu_set_vring_kick_exec(VuDev *dev, VhostUserMsg *vmsg)
 590 {
 591     int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
 592
 593     DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
 594
 595     if (!vu_check_queue_msg_file(dev, vmsg)) {
 596         return false;
 597     }
 598
 599     if (dev->vq[index].kick_fd != -1) {
 600         dev->remove_watch(dev, dev->vq[index].kick_fd);
 601         close(dev->vq[index].kick_fd);
 602         dev->vq[index].kick_fd = -1;
 603     }
 604
 605     if (!(vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK)) {
 606         dev->vq[index].kick_fd = vmsg->fds[0];
 607         DPRINT("Got kick_fd: %d for vq: %d\n", vmsg->fds[0], index);
 608     }
 609
 610     dev->vq[index].started = true;
 611     if (dev->iface->queue_set_started) {
 612         dev->iface->queue_set_started(dev, index, true);
 613     }
 614
 615     if (dev->vq[index].kick_fd != -1 && dev->vq[index].handler) {
 616         dev->set_watch(dev, dev->vq[index].kick_fd, VU_WATCH_IN,
 617                        vu_kick_cb, (void *)(long)index);
 618
 619         DPRINT("Waiting for kicks on fd: %d for vq: %d\n",
 620                dev->vq[index].kick_fd, index);
 621     }
 622
 623     return false;
 624 }
 625
 626 void vu_set_queue_handler(VuDev *dev, VuVirtq *vq,
 627                           vu_queue_handler_cb handler)
 628 {
 629     int qidx = vq - dev->vq;
 630
 631     vq->handler = handler;
 632     if (vq->kick_fd >= 0) {
 633         if (handler) {
 634             dev->set_watch(dev, vq->kick_fd, VU_WATCH_IN,
 635                            vu_kick_cb, (void *)(long)qidx);
 636         } else {
 637             dev->remove_watch(dev, vq->kick_fd);
 638         }
 639     }
 640 }
 641
 642 static bool
 643 vu_set_vring_call_exec(VuDev *dev, VhostUserMsg *vmsg)
 644 {
 645     int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
 646
 647     DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
 648
 649     if (!vu_check_queue_msg_file(dev, vmsg)) {
 650         return false;
 651     }
 652
 653     if (dev->vq[index].call_fd != -1) {
 654         close(dev->vq[index].call_fd);
 655         dev->vq[index].call_fd = -1;
 656     }
 657
 658     if (!(vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK)) {
 659         dev->vq[index].call_fd = vmsg->fds[0];
 660     }
 661
 662     DPRINT("Got call_fd: %d for vq: %d\n", vmsg->fds[0], index);
 663
 664     return false;
 665 }
 666
 667 static bool
 668 vu_set_vring_err_exec(VuDev *dev, VhostUserMsg *vmsg)
 669 {
 670     int index = vmsg->payload.u64 & VHOST_USER_VRING_IDX_MASK;
 671
 672     DPRINT("u64: 0x%016"PRIx64"\n", vmsg->payload.u64);
 673
 674     if (!vu_check_queue_msg_file(dev, vmsg)) {
 675         return false;
 676     }
 677
 678     if (dev->vq[index].err_fd != -1) {
 679         close(dev->vq[index].err_fd);
 680         dev->vq[index].err_fd = -1;
 681     }
 682
 683     if (!(vmsg->payload.u64 & VHOST_USER_VRING_NOFD_MASK)) {
 684         dev->vq[index].err_fd = vmsg->fds[0];
 685     }
 686
 687     return false;
 688 }
 689
 690 static bool
 691 vu_get_protocol_features_exec(VuDev *dev, VhostUserMsg *vmsg)
 692 {
 693     uint64_t features = 1ULL << VHOST_USER_PROTOCOL_F_LOG_SHMFD;
 694
 695     if (dev->iface->get_protocol_features) {
 696         features |= dev->iface->get_protocol_features(dev);
 697     }
 698
 699     vmsg->payload.u64 = features;
 700     vmsg->size = sizeof(vmsg->payload.u64);
 701
 702     return true;
 703 }
 704
 705 static bool
 706 vu_set_protocol_features_exec(VuDev *dev, VhostUserMsg *vmsg)
 707 {
 708     uint64_t features = vmsg->payload.u64;
 709
 710     DPRINT("u64: 0x%016"PRIx64"\n", features);
 711
 712     dev->protocol_features = vmsg->payload.u64;
 713
 714     if (dev->iface->set_protocol_features) {
 715         dev->iface->set_protocol_features(dev, features);
 716     }
 717
 718     return false;
 719 }
 720
 721 static bool
 722 vu_get_queue_num_exec(VuDev *dev, VhostUserMsg *vmsg)
 723 {
 724     DPRINT("Function %s() not implemented yet.\n", __func__);
 725     return false;
 726 }
 727
 728 static bool
 729 vu_set_vring_enable_exec(VuDev *dev, VhostUserMsg *vmsg)
 730 {
 731     unsigned int index = vmsg->payload.state.index;
 732     unsigned int enable = vmsg->payload.state.num;
 733
 734     DPRINT("State.index: %d\n", index);
 735     DPRINT("State.enable:   %d\n", enable);
 736
 737     if (index >= VHOST_MAX_NR_VIRTQUEUE) {
 738         vu_panic(dev, "Invalid vring_enable index: %u", index);
 739         return false;
 740     }
 741
 742     dev->vq[index].enable = enable;
 743     return false;
 744 }
 745
 746 static bool
 747 vu_process_message(VuDev *dev, VhostUserMsg *vmsg)
 748 {
 749     int do_reply = 0;
 750
 751     /* Print out generic part of the request. */
 752     DPRINT("================ Vhost user message ================\n");
 753     DPRINT("Request: %s (%d)\n", vu_request_to_string(vmsg->request),
 754            vmsg->request);
 755     DPRINT("Flags:   0x%x\n", vmsg->flags);
 756     DPRINT("Size:    %d\n", vmsg->size);
 757
 758     if (vmsg->fd_num) {
 759         int i;
 760         DPRINT("Fds:");
 761         for (i = 0; i < vmsg->fd_num; i++) {
 762             DPRINT(" %d", vmsg->fds[i]);
 763         }
 764         DPRINT("\n");
 765     }
 766
 767     if (dev->iface->process_msg &&
 768         dev->iface->process_msg(dev, vmsg, &do_reply)) {
 769         return do_reply;
 770     }
 771
 772     switch (vmsg->request) {
 773     case VHOST_USER_GET_FEATURES:
 774         return vu_get_features_exec(dev, vmsg);
 775     case VHOST_USER_SET_FEATURES:
 776         return vu_set_features_exec(dev, vmsg);
 777     case VHOST_USER_GET_PROTOCOL_FEATURES:
 778         return vu_get_protocol_features_exec(dev, vmsg);
 779     case VHOST_USER_SET_PROTOCOL_FEATURES:
 780         return vu_set_protocol_features_exec(dev, vmsg);
 781     case VHOST_USER_SET_OWNER:
 782         return vu_set_owner_exec(dev, vmsg);
 783     case VHOST_USER_RESET_OWNER:
 784         return vu_reset_device_exec(dev, vmsg);
 785     case VHOST_USER_SET_MEM_TABLE:
 786         return vu_set_mem_table_exec(dev, vmsg);
 787     case VHOST_USER_SET_LOG_BASE:
 788         return vu_set_log_base_exec(dev, vmsg);
 789     case VHOST_USER_SET_LOG_FD:
 790         return vu_set_log_fd_exec(dev, vmsg);
 791     case VHOST_USER_SET_VRING_NUM:
 792         return vu_set_vring_num_exec(dev, vmsg);
 793     case VHOST_USER_SET_VRING_ADDR:
 794         return vu_set_vring_addr_exec(dev, vmsg);
 795     case VHOST_USER_SET_VRING_BASE:
 796         return vu_set_vring_base_exec(dev, vmsg);
 797     case VHOST_USER_GET_VRING_BASE:
 798         return vu_get_vring_base_exec(dev, vmsg);
 799     case VHOST_USER_SET_VRING_KICK:
 800         return vu_set_vring_kick_exec(dev, vmsg);
 801     case VHOST_USER_SET_VRING_CALL:
 802         return vu_set_vring_call_exec(dev, vmsg);
 803     case VHOST_USER_SET_VRING_ERR:
 804         return vu_set_vring_err_exec(dev, vmsg);
 805     case VHOST_USER_GET_QUEUE_NUM:
 806         return vu_get_queue_num_exec(dev, vmsg);
 807     case VHOST_USER_SET_VRING_ENABLE:
 808         return vu_set_vring_enable_exec(dev, vmsg);
 809     default:
 810         vmsg_close_fds(vmsg);
 811         vu_panic(dev, "Unhandled request: %d", vmsg->request);
 812     }
 813
 814     return false;
 815 }
 816
 817 bool
 818 vu_dispatch(VuDev *dev)
 819 {
 820     VhostUserMsg vmsg = { 0, };
 821     int reply_requested;
 822     bool success = false;
 823
 824     if (!vu_message_read(dev, dev->sock, &vmsg)) {
 825         goto end;
 826     }
 827
 828     reply_requested = vu_process_message(dev, &vmsg);
 829     if (!reply_requested) {
 830         success = true;
 831         goto end;
 832     }
 833
 834     if (!vu_message_write(dev, dev->sock, &vmsg)) {
 835         goto end;
 836     }
 837
 838     success = true;
 839
 840 end:
 841     g_free(vmsg.data);
 842     return success;
 843 }
 844
 845 void
 846 vu_deinit(VuDev *dev)
 847 {
 848     int i;
 849
 850     for (i = 0; i < dev->nregions; i++) {
 851         VuDevRegion *r = &dev->regions[i];
 852         void *m = (void *) (uintptr_t) r->mmap_addr;
 853         if (m != MAP_FAILED) {
 854             munmap(m, r->size + r->mmap_offset);
 855         }
 856     }
 857     dev->nregions = 0;
 858
 859     for (i = 0; i < VHOST_MAX_NR_VIRTQUEUE; i++) {
 860         VuVirtq *vq = &dev->vq[i];
 861
 862         if (vq->call_fd != -1) {
 863             close(vq->call_fd);
 864             vq->call_fd = -1;
 865         }
 866
 867         if (vq->kick_fd != -1) {
 868             close(vq->kick_fd);
 869             vq->kick_fd = -1;
 870         }
 871
 872         if (vq->err_fd != -1) {
 873             close(vq->err_fd);
 874             vq->err_fd = -1;
 875         }
 876     }
 877
 878
 879     vu_close_log(dev);
 880
 881     if (dev->sock != -1) {
 882         close(dev->sock);
 883     }
 884 }
 885
 886 void
 887 vu_init(VuDev *dev,
 888         int socket,
 889         vu_panic_cb panic,
 890         vu_set_watch_cb set_watch,
 891         vu_remove_watch_cb remove_watch,
 892         const VuDevIface *iface)
 893 {
 894     int i;
 895
 896     assert(socket >= 0);
 897     assert(set_watch);
 898     assert(remove_watch);
 899     assert(iface);
 900     assert(panic);
 901
 902     memset(dev, 0, sizeof(*dev));
 903
 904     dev->sock = socket;
 905     dev->panic = panic;
 906     dev->set_watch = set_watch;
 907     dev->remove_watch = remove_watch;
 908     dev->iface = iface;
 909     dev->log_call_fd = -1;
 910     for (i = 0; i < VHOST_MAX_NR_VIRTQUEUE; i++) {
 911         dev->vq[i] = (VuVirtq) {
 912             .call_fd = -1, .kick_fd = -1, .err_fd = -1,
 913             .notification = true,
 914         };
 915     }
 916 }
 917
 918 VuVirtq *
 919 vu_get_queue(VuDev *dev, int qidx)
 920 {
 921     assert(qidx < VHOST_MAX_NR_VIRTQUEUE);
 922     return &dev->vq[qidx];
 923 }
 924
 925 bool
 926 vu_queue_enabled(VuDev *dev, VuVirtq *vq)
 927 {
 928     return vq->enable;
 929 }
 930
 931 static inline uint16_t
 932 vring_avail_flags(VuVirtq *vq)
 933 {
 934     return vq->vring.avail->flags;
 935 }
 936
 937 static inline uint16_t
 938 vring_avail_idx(VuVirtq *vq)
 939 {
 940     vq->shadow_avail_idx = vq->vring.avail->idx;
 941
 942     return vq->shadow_avail_idx;
 943 }
 944
 945 static inline uint16_t
 946 vring_avail_ring(VuVirtq *vq, int i)
 947 {
 948     return vq->vring.avail->ring[i];
 949 }
 950
 951 static inline uint16_t
 952 vring_get_used_event(VuVirtq *vq)
 953 {
 954     return vring_avail_ring(vq, vq->vring.num);
 955 }
 956
 957 static int
 958 virtqueue_num_heads(VuDev *dev, VuVirtq *vq, unsigned int idx)
 959 {
 960     uint16_t num_heads = vring_avail_idx(vq) - idx;
 961
 962     /* Check it isn't doing very strange things with descriptor numbers. */
 963     if (num_heads > vq->vring.num) {
 964         vu_panic(dev, "Guest moved used index from %u to %u",
 965                  idx, vq->shadow_avail_idx);
 966         return -1;
 967     }
 968     if (num_heads) {
 969         /* On success, callers read a descriptor at vq->last_avail_idx.
 970          * Make sure descriptor read does not bypass avail index read. */
 971         smp_rmb();
 972     }
 973
 974     return num_heads;
 975 }
 976
 977 static bool
 978 virtqueue_get_head(VuDev *dev, VuVirtq *vq,
 979                    unsigned int idx, unsigned int *head)
 980 {
 981     /* Grab the next descriptor number they're advertising, and increment
 982      * the index we've seen. */
 983     *head = vring_avail_ring(vq, idx % vq->vring.num);
 984
 985     /* If their number is silly, that's a fatal mistake. */
 986     if (*head >= vq->vring.num) {
 987         vu_panic(dev, "Guest says index %u is available", head);
 988         return false;
 989     }
 990
 991     return true;
 992 }
 993
 994 enum {
 995     VIRTQUEUE_READ_DESC_ERROR = -1,
 996     VIRTQUEUE_READ_DESC_DONE = 0,   /* end of chain */
 997     VIRTQUEUE_READ_DESC_MORE = 1,   /* more buffers in chain */
 998 };
 999
1000 static int
1001 virtqueue_read_next_desc(VuDev *dev, struct vring_desc *desc,
1002                          int i, unsigned int max, unsigned int *next)
1003 {
1004     /* If this descriptor says it doesn't chain, we're done. */
1005     if (!(desc[i].flags & VRING_DESC_F_NEXT)) {
1006         return VIRTQUEUE_READ_DESC_DONE;
1007     }
1008
1009     /* Check they're not leading us off end of descriptors. */
1010     *next = desc[i].next;
1011     /* Make sure compiler knows to grab that: we don't want it changing! */
1012     smp_wmb();
1013
1014     if (*next >= max) {
1015         vu_panic(dev, "Desc next is %u", next);
1016         return VIRTQUEUE_READ_DESC_ERROR;
1017     }
1018
1019     return VIRTQUEUE_READ_DESC_MORE;
1020 }
1021
1022 void
1023 vu_queue_get_avail_bytes(VuDev *dev, VuVirtq *vq, unsigned int *in_bytes,
1024                          unsigned int *out_bytes,
1025                          unsigned max_in_bytes, unsigned max_out_bytes)
1026 {
1027     unsigned int idx;
1028     unsigned int total_bufs, in_total, out_total;
1029     int rc;
1030
1031     idx = vq->last_avail_idx;
1032
1033     total_bufs = in_total = out_total = 0;
1034     while ((rc = virtqueue_num_heads(dev, vq, idx)) > 0) {
1035         unsigned int max, num_bufs, indirect = 0;
1036         struct vring_desc *desc;
1037         unsigned int i;
1038
1039         max = vq->vring.num;
1040         num_bufs = total_bufs;
1041         if (!virtqueue_get_head(dev, vq, idx++, &i)) {
1042             goto err;
1043         }
1044         desc = vq->vring.desc;
1045
1046         if (desc[i].flags & VRING_DESC_F_INDIRECT) {
1047             if (desc[i].len % sizeof(struct vring_desc)) {
1048                 vu_panic(dev, "Invalid size for indirect buffer table");
1049                 goto err;
1050             }
1051
1052             /* If we've got too many, that implies a descriptor loop. */
1053             if (num_bufs >= max) {
1054                 vu_panic(dev, "Looped descriptor");
1055                 goto err;
1056             }
1057
1058             /* loop over the indirect descriptor table */
1059             indirect = 1;
1060             max = desc[i].len / sizeof(struct vring_desc);
1061             desc = vu_gpa_to_va(dev, desc[i].addr);
1062             num_bufs = i = 0;
1063         }
1064
1065         do {
1066             /* If we've got too many, that implies a descriptor loop. */
1067             if (++num_bufs > max) {
1068                 vu_panic(dev, "Looped descriptor");
1069                 goto err;
1070             }
1071
1072             if (desc[i].flags & VRING_DESC_F_WRITE) {
1073                 in_total += desc[i].len;
1074             } else {
1075                 out_total += desc[i].len;
1076             }
1077             if (in_total >= max_in_bytes && out_total >= max_out_bytes) {
1078                 goto done;
1079             }
1080             rc = virtqueue_read_next_desc(dev, desc, i, max, &i);
1081         } while (rc == VIRTQUEUE_READ_DESC_MORE);
1082
1083         if (rc == VIRTQUEUE_READ_DESC_ERROR) {
1084             goto err;
1085         }
1086
1087         if (!indirect) {
1088             total_bufs = num_bufs;
1089         } else {
1090             total_bufs++;
1091         }
1092     }
1093     if (rc < 0) {
1094         goto err;
1095     }
1096 done:
1097     if (in_bytes) {
1098         *in_bytes = in_total;
1099     }
1100     if (out_bytes) {
1101         *out_bytes = out_total;
1102     }
1103     return;
1104
1105 err:
1106     in_total = out_total = 0;
1107     goto done;
1108 }
1109
1110 bool
1111 vu_queue_avail_bytes(VuDev *dev, VuVirtq *vq, unsigned int in_bytes,
1112                      unsigned int out_bytes)
1113 {
1114     unsigned int in_total, out_total;
1115
1116     vu_queue_get_avail_bytes(dev, vq, &in_total, &out_total,
1117                              in_bytes, out_bytes);
1118
1119     return in_bytes <= in_total && out_bytes <= out_total;
1120 }
1121
1122 /* Fetch avail_idx from VQ memory only when we really need to know if
1123  * guest has added some buffers. */
1124 int
1125 vu_queue_empty(VuDev *dev, VuVirtq *vq)
1126 {
1127     if (vq->shadow_avail_idx != vq->last_avail_idx) {
1128         return 0;
1129     }
1130
1131     return vring_avail_idx(vq) == vq->last_avail_idx;
1132 }
1133
1134 static inline
1135 bool has_feature(uint64_t features, unsigned int fbit)
1136 {
1137     assert(fbit < 64);
1138     return !!(features & (1ULL << fbit));
1139 }
1140
1141 static inline
1142 bool vu_has_feature(VuDev *dev,
1143                     unsigned int fbit)
1144 {
1145     return has_feature(dev->features, fbit);
1146 }
1147
1148 static bool
1149 vring_notify(VuDev *dev, VuVirtq *vq)
1150 {
1151     uint16_t old, new;
1152     bool v;
1153
1154     /* We need to expose used array entries before checking used event. */
1155     smp_mb();
1156
1157     /* Always notify when queue is empty (when feature acknowledge) */
1158     if (vu_has_feature(dev, VIRTIO_F_NOTIFY_ON_EMPTY) &&
1159         !vq->inuse && vu_queue_empty(dev, vq)) {
1160         return true;
1161     }
1162
1163     if (!vu_has_feature(dev, VIRTIO_RING_F_EVENT_IDX)) {
1164         return !(vring_avail_flags(vq) & VRING_AVAIL_F_NO_INTERRUPT);
1165     }
1166
1167     v = vq->signalled_used_valid;
1168     vq->signalled_used_valid = true;
1169     old = vq->signalled_used;
1170     new = vq->signalled_used = vq->used_idx;
1171     return !v || vring_need_event(vring_get_used_event(vq), new, old);
1172 }
1173
1174 void
1175 vu_queue_notify(VuDev *dev, VuVirtq *vq)
1176 {
1177     if (unlikely(dev->broken)) {
1178         return;
1179     }
1180
1181     if (!vring_notify(dev, vq)) {
1182         DPRINT("skipped notify...\n");
1183         return;
1184     }
1185
1186     if (eventfd_write(vq->call_fd, 1) < 0) {
1187         vu_panic(dev, "Error writing eventfd: %s", strerror(errno));
1188     }
1189 }
1190
1191 static inline void
1192 vring_used_flags_set_bit(VuVirtq *vq, int mask)
1193 {
1194     uint16_t *flags;
1195
1196     flags = (uint16_t *)((char*)vq->vring.used +
1197                          offsetof(struct vring_used, flags));
1198     *flags |= mask;
1199 }
1200
1201 static inline void
1202 vring_used_flags_unset_bit(VuVirtq *vq, int mask)
1203 {
1204     uint16_t *flags;
1205
1206     flags = (uint16_t *)((char*)vq->vring.used +
1207                          offsetof(struct vring_used, flags));
1208     *flags &= ~mask;
1209 }
1210
1211 static inline void
1212 vring_set_avail_event(VuVirtq *vq, uint16_t val)
1213 {
1214     if (!vq->notification) {
1215         return;
1216     }
1217
1218     *((uint16_t *) &vq->vring.used->ring[vq->vring.num]) = val;
1219 }
1220
1221 void
1222 vu_queue_set_notification(VuDev *dev, VuVirtq *vq, int enable)
1223 {
1224     vq->notification = enable;
1225     if (vu_has_feature(dev, VIRTIO_RING_F_EVENT_IDX)) {
1226         vring_set_avail_event(vq, vring_avail_idx(vq));
1227     } else if (enable) {
1228         vring_used_flags_unset_bit(vq, VRING_USED_F_NO_NOTIFY);
1229     } else {
1230         vring_used_flags_set_bit(vq, VRING_USED_F_NO_NOTIFY);
1231     }
1232     if (enable) {
1233         /* Expose avail event/used flags before caller checks the avail idx. */
1234         smp_mb();
1235     }
1236 }
1237
1238 static void
1239 virtqueue_map_desc(VuDev *dev,
1240                    unsigned int *p_num_sg, struct iovec *iov,
1241                    unsigned int max_num_sg, bool is_write,
1242                    uint64_t pa, size_t sz)
1243 {
1244     unsigned num_sg = *p_num_sg;
1245
1246     assert(num_sg <= max_num_sg);
1247
1248     if (!sz) {
1249         vu_panic(dev, "virtio: zero sized buffers are not allowed");
1250         return;
1251     }
1252
1253     iov[num_sg].iov_base = vu_gpa_to_va(dev, pa);
1254     iov[num_sg].iov_len = sz;
1255     num_sg++;
1256
1257     *p_num_sg = num_sg;
1258 }
1259
1260 /* Round number down to multiple */
1261 #define ALIGN_DOWN(n, m) ((n) / (m) * (m))
1262
1263 /* Round number up to multiple */
1264 #define ALIGN_UP(n, m) ALIGN_DOWN((n) + (m) - 1, (m))
1265
1266 static void *
1267 virtqueue_alloc_element(size_t sz,
1268                                      unsigned out_num, unsigned in_num)
1269 {
1270     VuVirtqElement *elem;
1271     size_t in_sg_ofs = ALIGN_UP(sz, __alignof__(elem->in_sg[0]));
1272     size_t out_sg_ofs = in_sg_ofs + in_num * sizeof(elem->in_sg[0]);
1273     size_t out_sg_end = out_sg_ofs + out_num * sizeof(elem->out_sg[0]);
1274
1275     assert(sz >= sizeof(VuVirtqElement));
1276     elem = malloc(out_sg_end);
1277     elem->out_num = out_num;
1278     elem->in_num = in_num;
1279     elem->in_sg = (void *)elem + in_sg_ofs;
1280     elem->out_sg = (void *)elem + out_sg_ofs;
1281     return elem;
1282 }
1283
1284 void *
1285 vu_queue_pop(VuDev *dev, VuVirtq *vq, size_t sz)
1286 {
1287     unsigned int i, head, max;
1288     VuVirtqElement *elem;
1289     unsigned out_num, in_num;
1290     struct iovec iov[VIRTQUEUE_MAX_SIZE];
1291     struct vring_desc *desc;
1292     int rc;
1293
1294     if (unlikely(dev->broken)) {
1295         return NULL;
1296     }
1297
1298     if (vu_queue_empty(dev, vq)) {
1299         return NULL;
1300     }
1301     /* Needed after virtio_queue_empty(), see comment in
1302      * virtqueue_num_heads(). */
1303     smp_rmb();
1304
1305     /* When we start there are none of either input nor output. */
1306     out_num = in_num = 0;
1307
1308     max = vq->vring.num;
1309     if (vq->inuse >= vq->vring.num) {
1310         vu_panic(dev, "Virtqueue size exceeded");
1311         return NULL;
1312     }
1313
1314     if (!virtqueue_get_head(dev, vq, vq->last_avail_idx++, &head)) {
1315         return NULL;
1316     }
1317
1318     if (vu_has_feature(dev, VIRTIO_RING_F_EVENT_IDX)) {
1319         vring_set_avail_event(vq, vq->last_avail_idx);
1320     }
1321
1322     i = head;
1323     desc = vq->vring.desc;
1324     if (desc[i].flags & VRING_DESC_F_INDIRECT) {
1325         if (desc[i].len % sizeof(struct vring_desc)) {
1326             vu_panic(dev, "Invalid size for indirect buffer table");
1327         }
1328
1329         /* loop over the indirect descriptor table */
1330         max = desc[i].len / sizeof(struct vring_desc);
1331         desc = vu_gpa_to_va(dev, desc[i].addr);
1332         i = 0;
1333     }
1334
1335     /* Collect all the descriptors */
1336     do {
1337         if (desc[i].flags & VRING_DESC_F_WRITE) {
1338             virtqueue_map_desc(dev, &in_num, iov + out_num,
1339                                VIRTQUEUE_MAX_SIZE - out_num, true,
1340                                desc[i].addr, desc[i].len);
1341         } else {
1342             if (in_num) {
1343                 vu_panic(dev, "Incorrect order for descriptors");
1344                 return NULL;
1345             }
1346             virtqueue_map_desc(dev, &out_num, iov,
1347                                VIRTQUEUE_MAX_SIZE, false,
1348                                desc[i].addr, desc[i].len);
1349         }
1350
1351         /* If we've got too many, that implies a descriptor loop. */
1352         if ((in_num + out_num) > max) {
1353             vu_panic(dev, "Looped descriptor");
1354         }
1355         rc = virtqueue_read_next_desc(dev, desc, i, max, &i);
1356     } while (rc == VIRTQUEUE_READ_DESC_MORE);
1357
1358     if (rc == VIRTQUEUE_READ_DESC_ERROR) {
1359         return NULL;
1360     }
1361
1362     /* Now copy what we have collected and mapped */
1363     elem = virtqueue_alloc_element(sz, out_num, in_num);
1364     elem->index = head;
1365     for (i = 0; i < out_num; i++) {
1366         elem->out_sg[i] = iov[i];
1367     }
1368     for (i = 0; i < in_num; i++) {
1369         elem->in_sg[i] = iov[out_num + i];
1370     }
1371
1372     vq->inuse++;
1373
1374     return elem;
1375 }
1376
1377 bool
1378 vu_queue_rewind(VuDev *dev, VuVirtq *vq, unsigned int num)
1379 {
1380     if (num > vq->inuse) {
1381         return false;
1382     }
1383     vq->last_avail_idx -= num;
1384     vq->inuse -= num;
1385     return true;
1386 }
1387
1388 static inline
1389 void vring_used_write(VuDev *dev, VuVirtq *vq,
1390                       struct vring_used_elem *uelem, int i)
1391 {
1392     struct vring_used *used = vq->vring.used;
1393
1394     used->ring[i] = *uelem;
1395     vu_log_write(dev, vq->vring.log_guest_addr +
1396                  offsetof(struct vring_used, ring[i]),
1397                  sizeof(used->ring[i]));
1398 }
1399
1400
1401 static void
1402 vu_log_queue_fill(VuDev *dev, VuVirtq *vq,
1403                   const VuVirtqElement *elem,
1404                   unsigned int len)
1405 {
1406     struct vring_desc *desc = vq->vring.desc;
1407     unsigned int i, max, min;
1408     unsigned num_bufs = 0;
1409
1410     max = vq->vring.num;
1411     i = elem->index;
1412
1413     if (desc[i].flags & VRING_DESC_F_INDIRECT) {
1414         if (desc[i].len % sizeof(struct vring_desc)) {
1415             vu_panic(dev, "Invalid size for indirect buffer table");
1416         }
1417
1418         /* loop over the indirect descriptor table */
1419         max = desc[i].len / sizeof(struct vring_desc);
1420         desc = vu_gpa_to_va(dev, desc[i].addr);
1421         i = 0;
1422     }
1423
1424     do {
1425         if (++num_bufs > max) {
1426             vu_panic(dev, "Looped descriptor");
1427             return;
1428         }
1429
1430         if (desc[i].flags & VRING_DESC_F_WRITE) {
1431             min = MIN(desc[i].len, len);
1432             vu_log_write(dev, desc[i].addr, min);
1433             len -= min;
1434         }
1435
1436     } while (len > 0 &&
1437              (virtqueue_read_next_desc(dev, desc, i, max, &i)
1438               == VIRTQUEUE_READ_DESC_MORE));
1439 }
1440
1441 void
1442 vu_queue_fill(VuDev *dev, VuVirtq *vq,
1443               const VuVirtqElement *elem,
1444               unsigned int len, unsigned int idx)
1445 {
1446     struct vring_used_elem uelem;
1447
1448     if (unlikely(dev->broken)) {
1449         return;
1450     }
1451
1452     vu_log_queue_fill(dev, vq, elem, len);
1453
1454     idx = (idx + vq->used_idx) % vq->vring.num;
1455
1456     uelem.id = elem->index;
1457     uelem.len = len;
1458     vring_used_write(dev, vq, &uelem, idx);
1459 }
1460
1461 static inline
1462 void vring_used_idx_set(VuDev *dev, VuVirtq *vq, uint16_t val)
1463 {
1464     vq->vring.used->idx = val;
1465     vu_log_write(dev,
1466                  vq->vring.log_guest_addr + offsetof(struct vring_used, idx),
1467                  sizeof(vq->vring.used->idx));
1468
1469     vq->used_idx = val;
1470 }
1471
1472 void
1473 vu_queue_flush(VuDev *dev, VuVirtq *vq, unsigned int count)
1474 {
1475     uint16_t old, new;
1476
1477     if (unlikely(dev->broken)) {
1478         return;
1479     }
1480
1481     /* Make sure buffer is written before we update index. */
1482     smp_wmb();
1483
1484     old = vq->used_idx;
1485     new = old + count;
1486     vring_used_idx_set(dev, vq, new);
1487     vq->inuse -= count;
1488     if (unlikely((int16_t)(new - vq->signalled_used) < (uint16_t)(new - old))) {
1489         vq->signalled_used_valid = false;
1490     }
1491 }
1492
1493 void
1494 vu_queue_push(VuDev *dev, VuVirtq *vq,
1495               const VuVirtqElement *elem, unsigned int len)
1496 {
1497     vu_queue_fill(dev, vq, elem, len, 0);
1498     vu_queue_flush(dev, vq, 1);
1499 }