cachepc-qemu

Fork of AMDESE/qemu with changes for cachepc side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-qemu
Log | Files | Refs | Submodules | LICENSE | sfeed.txt

main.c (11033B)

      1/*
      2 * This work is licensed under the terms of the GNU GPL, version 2 or
      3 * (at your option) any later version.  See the COPYING file in the
      4 * top-level directory.
      5 */
      6
      7#include "qemu/osdep.h"
      8
      9#include <sys/ioctl.h>
     10
     11#include "qemu/iov.h"
     12#include "qemu/bswap.h"
     13#include "qemu/sockets.h"
     14#include "libvhost-user-glib.h"
     15#include "standard-headers/linux/input.h"
     16#include "standard-headers/linux/virtio_input.h"
     17#include "qapi/error.h"
     18
     19enum {
     20    VHOST_USER_INPUT_MAX_QUEUES = 2,
     21};
     22
     23typedef struct virtio_input_event virtio_input_event;
     24typedef struct virtio_input_config virtio_input_config;
     25
     26typedef struct VuInput {
     27    VugDev dev;
     28    GSource *evsrc;
     29    int evdevfd;
     30    GArray *config;
     31    virtio_input_config *sel_config;
     32    struct {
     33        virtio_input_event event;
     34        VuVirtqElement *elem;
     35    } *queue;
     36    uint32_t qindex, qsize;
     37} VuInput;
     38
     39static void vi_input_send(VuInput *vi, struct virtio_input_event *event)
     40{
     41    VuDev *dev = &vi->dev.parent;
     42    VuVirtq *vq = vu_get_queue(dev, 0);
     43    VuVirtqElement *elem;
     44    int i, len;
     45
     46    /* queue up events ... */
     47    if (vi->qindex == vi->qsize) {
     48        vi->qsize++;
     49        vi->queue = g_realloc_n(vi->queue, vi->qsize, sizeof(vi->queue[0]));
     50    }
     51    vi->queue[vi->qindex++].event = *event;
     52
     53    /* ... until we see a report sync ... */
     54    if (event->type != htole16(EV_SYN) ||
     55        event->code != htole16(SYN_REPORT)) {
     56        return;
     57    }
     58
     59    /* ... then check available space ... */
     60    for (i = 0; i < vi->qindex; i++) {
     61        elem = vu_queue_pop(dev, vq, sizeof(VuVirtqElement));
     62        if (!elem) {
     63            while (--i >= 0) {
     64                vu_queue_unpop(dev, vq, vi->queue[i].elem, 0);
     65            }
     66            vi->qindex = 0;
     67            g_warning("virtio-input queue full");
     68            return;
     69        }
     70        vi->queue[i].elem = elem;
     71    }
     72
     73    /* ... and finally pass them to the guest */
     74    for (i = 0; i < vi->qindex; i++) {
     75        elem = vi->queue[i].elem;
     76        len = iov_from_buf(elem->in_sg, elem->in_num,
     77                           0, &vi->queue[i].event, sizeof(virtio_input_event));
     78        vu_queue_push(dev, vq, elem, len);
     79        free(elem);
     80    }
     81
     82    vu_queue_notify(&vi->dev.parent, vq);
     83    vi->qindex = 0;
     84}
     85
     86static void
     87vi_evdev_watch(VuDev *dev, int condition, void *data)
     88{
     89    VuInput *vi = data;
     90    int fd = vi->evdevfd;
     91
     92    g_debug("Got evdev condition %x", condition);
     93
     94    struct virtio_input_event virtio;
     95    struct input_event evdev;
     96    int rc;
     97
     98    for (;;) {
     99        rc = read(fd, &evdev, sizeof(evdev));
    100        if (rc != sizeof(evdev)) {
    101            break;
    102        }
    103
    104        g_debug("input %d %d %d", evdev.type, evdev.code, evdev.value);
    105
    106        virtio.type  = htole16(evdev.type);
    107        virtio.code  = htole16(evdev.code);
    108        virtio.value = htole32(evdev.value);
    109        vi_input_send(vi, &virtio);
    110    }
    111}
    112
    113
    114static void vi_handle_status(VuInput *vi, virtio_input_event *event)
    115{
    116    struct input_event evdev;
    117    struct timeval tval;
    118    int rc;
    119
    120    if (gettimeofday(&tval, NULL)) {
    121        perror("vi_handle_status: gettimeofday");
    122        return;
    123    }
    124
    125    evdev.input_event_sec = tval.tv_sec;
    126    evdev.input_event_usec = tval.tv_usec;
    127    evdev.type = le16toh(event->type);
    128    evdev.code = le16toh(event->code);
    129    evdev.value = le32toh(event->value);
    130
    131    rc = write(vi->evdevfd, &evdev, sizeof(evdev));
    132    if (rc == -1) {
    133        perror("vi_host_handle_status: write");
    134    }
    135}
    136
    137static void vi_handle_sts(VuDev *dev, int qidx)
    138{
    139    VuInput *vi = container_of(dev, VuInput, dev.parent);
    140    VuVirtq *vq = vu_get_queue(dev, qidx);
    141    virtio_input_event event;
    142    VuVirtqElement *elem;
    143    int len;
    144
    145    g_debug("%s", G_STRFUNC);
    146
    147    for (;;) {
    148        elem = vu_queue_pop(dev, vq, sizeof(VuVirtqElement));
    149        if (!elem) {
    150            break;
    151        }
    152
    153        memset(&event, 0, sizeof(event));
    154        len = iov_to_buf(elem->out_sg, elem->out_num,
    155                         0, &event, sizeof(event));
    156        vi_handle_status(vi, &event);
    157        vu_queue_push(dev, vq, elem, len);
    158        free(elem);
    159    }
    160
    161    vu_queue_notify(&vi->dev.parent, vq);
    162}
    163
    164static void
    165vi_panic(VuDev *dev, const char *msg)
    166{
    167    g_critical("%s\n", msg);
    168    exit(EXIT_FAILURE);
    169}
    170
    171static void
    172vi_queue_set_started(VuDev *dev, int qidx, bool started)
    173{
    174    VuInput *vi = container_of(dev, VuInput, dev.parent);
    175    VuVirtq *vq = vu_get_queue(dev, qidx);
    176
    177    g_debug("queue started %d:%d", qidx, started);
    178
    179    if (qidx == 1) {
    180        vu_set_queue_handler(dev, vq, started ? vi_handle_sts : NULL);
    181    }
    182
    183    started = vu_queue_started(dev, vu_get_queue(dev, 0)) &&
    184        vu_queue_started(dev, vu_get_queue(dev, 1));
    185
    186    if (started && !vi->evsrc) {
    187        vi->evsrc = vug_source_new(&vi->dev, vi->evdevfd,
    188                                   G_IO_IN, vi_evdev_watch, vi);
    189    }
    190
    191    if (!started && vi->evsrc) {
    192        vug_source_destroy(vi->evsrc);
    193        vi->evsrc = NULL;
    194    }
    195}
    196
    197static virtio_input_config *
    198vi_find_config(VuInput *vi, uint8_t select, uint8_t subsel)
    199{
    200    virtio_input_config *cfg;
    201    int i;
    202
    203    for (i = 0; i < vi->config->len; i++) {
    204        cfg = &g_array_index(vi->config, virtio_input_config, i);
    205        if (select == cfg->select && subsel == cfg->subsel) {
    206            return cfg;
    207        }
    208    }
    209
    210    return NULL;
    211}
    212
    213static int vi_get_config(VuDev *dev, uint8_t *config, uint32_t len)
    214{
    215    VuInput *vi = container_of(dev, VuInput, dev.parent);
    216
    217    if (len > sizeof(*vi->sel_config)) {
    218        return -1;
    219    }
    220
    221    if (vi->sel_config) {
    222        memcpy(config, vi->sel_config, len);
    223    } else {
    224        memset(config, 0, len);
    225    }
    226
    227    return 0;
    228}
    229
    230static int vi_set_config(VuDev *dev, const uint8_t *data,
    231                         uint32_t offset, uint32_t size,
    232                         uint32_t flags)
    233{
    234    VuInput *vi = container_of(dev, VuInput, dev.parent);
    235    virtio_input_config *config = (virtio_input_config *)data;
    236
    237    vi->sel_config = vi_find_config(vi, config->select, config->subsel);
    238
    239    return 0;
    240}
    241
    242static const VuDevIface vuiface = {
    243    .queue_set_started = vi_queue_set_started,
    244    .get_config = vi_get_config,
    245    .set_config = vi_set_config,
    246};
    247
    248static void
    249vi_bits_config(VuInput *vi, int type, int count)
    250{
    251    virtio_input_config bits;
    252    int rc, i, size = 0;
    253
    254    memset(&bits, 0, sizeof(bits));
    255    rc = ioctl(vi->evdevfd, EVIOCGBIT(type, count / 8), bits.u.bitmap);
    256    if (rc < 0) {
    257        return;
    258    }
    259
    260    for (i = 0; i < count / 8; i++) {
    261        if (bits.u.bitmap[i]) {
    262            size = i + 1;
    263        }
    264    }
    265    if (size == 0) {
    266        return;
    267    }
    268
    269    bits.select = VIRTIO_INPUT_CFG_EV_BITS;
    270    bits.subsel = type;
    271    bits.size   = size;
    272    g_array_append_val(vi->config, bits);
    273}
    274
    275static char *opt_evdev;
    276static int opt_fdnum = -1;
    277static char *opt_socket_path;
    278static gboolean opt_nograb;
    279static gboolean opt_print_caps;
    280
    281static GOptionEntry entries[] = {
    282    { "print-capabilities", 'c', 0, G_OPTION_ARG_NONE, &opt_print_caps,
    283      "Print capabilities", NULL },
    284    { "no-grab", 'n', 0, G_OPTION_ARG_NONE, &opt_nograb,
    285      "Don't grab device", NULL },
    286    { "fd", 'f', 0, G_OPTION_ARG_INT, &opt_fdnum,
    287      "Use inherited fd socket", "FDNUM" },
    288    { "socket-path", 's', 0, G_OPTION_ARG_FILENAME, &opt_socket_path,
    289      "Use UNIX socket path", "PATH" },
    290    { "evdev-path", 'p', 0, G_OPTION_ARG_FILENAME, &opt_evdev,
    291      "evdev input device path", "PATH" },
    292    { NULL, }
    293};
    294
    295int
    296main(int argc, char *argv[])
    297{
    298    GMainLoop *loop = NULL;
    299    VuInput vi = { 0, };
    300    int rc, ver, fd;
    301    virtio_input_config id;
    302    struct input_id ids;
    303    GError *error = NULL;
    304    GOptionContext *context;
    305
    306    context = g_option_context_new(NULL);
    307    g_option_context_add_main_entries(context, entries, NULL);
    308    if (!g_option_context_parse(context, &argc, &argv, &error)) {
    309        g_printerr("Option parsing failed: %s\n", error->message);
    310        exit(EXIT_FAILURE);
    311    }
    312    if (opt_print_caps) {
    313        g_print("{\n");
    314        g_print("  \"type\": \"input\",\n");
    315        g_print("  \"features\": [\n");
    316        g_print("    \"evdev-path\",\n");
    317        g_print("    \"no-grab\"\n");
    318        g_print("  ]\n");
    319        g_print("}\n");
    320        exit(EXIT_SUCCESS);
    321    }
    322    if (!opt_evdev) {
    323        g_printerr("Please specify an evdev path\n");
    324        exit(EXIT_FAILURE);
    325    }
    326    if ((!!opt_socket_path + (opt_fdnum != -1)) != 1) {
    327        g_printerr("Please specify either --fd or --socket-path\n");
    328        exit(EXIT_FAILURE);
    329    }
    330
    331    vi.evdevfd = open(opt_evdev, O_RDWR);
    332    if (vi.evdevfd < 0) {
    333        g_printerr("Failed to open evdev: %s\n", g_strerror(errno));
    334        exit(EXIT_FAILURE);
    335    }
    336
    337    rc = ioctl(vi.evdevfd, EVIOCGVERSION, &ver);
    338    if (rc < 0) {
    339        g_printerr("%s: is not an evdev device\n", argv[1]);
    340        exit(EXIT_FAILURE);
    341    }
    342
    343    if (!opt_nograb) {
    344        rc = ioctl(vi.evdevfd, EVIOCGRAB, 1);
    345        if (rc < 0) {
    346            g_printerr("Failed to grab device\n");
    347            exit(EXIT_FAILURE);
    348        }
    349    }
    350
    351    vi.config = g_array_new(false, false, sizeof(virtio_input_config));
    352    memset(&id, 0, sizeof(id));
    353    if (ioctl(vi.evdevfd, EVIOCGNAME(sizeof(id.u.string) - 1),
    354              id.u.string) < 0) {
    355        g_printerr("Failed to get evdev name: %s\n", g_strerror(errno));
    356        exit(EXIT_FAILURE);
    357    }
    358    id.select = VIRTIO_INPUT_CFG_ID_NAME;
    359    id.size = strlen(id.u.string);
    360    g_array_append_val(vi.config, id);
    361
    362    if (ioctl(vi.evdevfd, EVIOCGID, &ids) == 0) {
    363        memset(&id, 0, sizeof(id));
    364        id.select = VIRTIO_INPUT_CFG_ID_DEVIDS;
    365        id.size = sizeof(struct virtio_input_devids);
    366        id.u.ids.bustype = cpu_to_le16(ids.bustype);
    367        id.u.ids.vendor  = cpu_to_le16(ids.vendor);
    368        id.u.ids.product = cpu_to_le16(ids.product);
    369        id.u.ids.version = cpu_to_le16(ids.version);
    370        g_array_append_val(vi.config, id);
    371    }
    372
    373    vi_bits_config(&vi, EV_KEY, KEY_CNT);
    374    vi_bits_config(&vi, EV_REL, REL_CNT);
    375    vi_bits_config(&vi, EV_ABS, ABS_CNT);
    376    vi_bits_config(&vi, EV_MSC, MSC_CNT);
    377    vi_bits_config(&vi, EV_SW,  SW_CNT);
    378    g_debug("config length: %u", vi.config->len);
    379
    380    if (opt_socket_path) {
    381        int lsock = unix_listen(opt_socket_path, &error_fatal);
    382        if (lsock < 0) {
    383            g_printerr("Failed to listen on %s.\n", opt_socket_path);
    384            exit(EXIT_FAILURE);
    385        }
    386        fd = accept(lsock, NULL, NULL);
    387        close(lsock);
    388    } else {
    389        fd = opt_fdnum;
    390    }
    391    if (fd == -1) {
    392        g_printerr("Invalid vhost-user socket.\n");
    393        exit(EXIT_FAILURE);
    394    }
    395
    396    if (!vug_init(&vi.dev, VHOST_USER_INPUT_MAX_QUEUES, fd, vi_panic,
    397                  &vuiface)) {
    398        g_printerr("Failed to initialize libvhost-user-glib.\n");
    399        exit(EXIT_FAILURE);
    400    }
    401
    402    loop = g_main_loop_new(NULL, FALSE);
    403    g_main_loop_run(loop);
    404    g_main_loop_unref(loop);
    405
    406    vug_deinit(&vi.dev);
    407
    408    vug_source_destroy(vi.evsrc);
    409    g_array_free(vi.config, TRUE);
    410    g_free(vi.queue);
    411    return 0;
    412}