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