path: root/tools/vhost-user-scmi/main.c

/*
 * VIRTIO SCMI Bridge via vhost-user
 *
 * Copyright (c) 2021 Vincent Guittot <vincent.guittot@linaro.org>
 *
 * SPDX-License-Identifier: GPL-2.0-or-later
 */

#define G_LOG_DOMAIN "vhost-user-scmi"
#define G_LOG_USE_STRUCTURED 1

#include <glib.h>
#include <gio/gunixsocketaddress.h>
#include <glib-unix.h>
#include <stdio.h>
#include <stdbool.h>
#include <inttypes.h>

#include "subprojects/libvhost-user/libvhost-user-glib.h"
#include "subprojects/libvhost-user/libvhost-user.h"

/* Device implements some SCMI notifications, or delayed responses. */
#define VIRTIO_SCMI_F_P2A_CHANNELS 0
/* Definitions from virtio-scmi specifications */
#define VHOST_USER_SCMI_MAX_QUEUES       2

/* Status */
#define VIRTIO_SCMI_MSG_OK               0
#define VIRTIO_SCMI_MSG_ERR              1

/**
 * struct virtio_scmi_request - the virtio scmi message request header
 * @hdr: the controlled device's address
 * @data: used to pad to full dword
 */
struct virtio_scmi_request {
    __virtio32 hdr;
    __u8 data[];
};

struct virtio_scmi_response {
    __virtio32 hdr;
    __virtio32 status;
    __u8 data[];
};

struct virtio_scmi_notification {
    __virtio32 hdr;
    __u8 data[];
};

/* vhost-user-scmi definitions */

#ifndef container_of
#define container_of(ptr, type, member) ({                      \
        const typeof(((type *) 0)->member) *__mptr = (ptr);     \
        (type *) ((char *) __mptr - offsetof(type, member));})
#endif

typedef struct {
    VugDev dev;
    gchar *path;
    GSocket *socket ;
    GThread *thread;
    VuVirtqElement *elem[VHOST_USER_SCMI_MAX_QUEUES];
    struct vhost_vring_addr vring[VHOST_USER_SCMI_MAX_QUEUES];
    int vring_num[VHOST_USER_SCMI_MAX_QUEUES];
    bool guest;
    bool started;
} VuChnl;

typedef struct {
    VuChnl vm_dev;
    VuChnl be_dev;
    GMainLoop *loop;
} VuScmi;

static gboolean print_cap, notification, verbose, debug;
static gchar *socket_path_vm;
static gchar *socket_path_be;

static GOptionEntry options[] = {
    { "socket-path-vm", 'v', 0, G_OPTION_ARG_FILENAME, &socket_path_vm,
      "Location of vhost-user Unix domain socket for guest vm",
      "PATH" },
    { "socket-path-be", 'g', 0, G_OPTION_ARG_FILENAME, &socket_path_be,
      "Location of vhost-user Unix domain socket for server vm",
      "PATH" },
    { "notif-only", 'n', 0, G_OPTION_ARG_NONE, &notification,
      "Only forward virtqueue notification", NULL},
    { "print-capabilities", 'c', 0, G_OPTION_ARG_NONE, &print_cap,
      "Output to stdout the backend capabilities in JSON format and exit",
      NULL},
    { "verbose", 'v', 0, G_OPTION_ARG_NONE, &verbose,
      "Be more verbose in output", NULL},
    { "debug", 'v', 0, G_OPTION_ARG_NONE, &debug,
      "Enable debug output", NULL},
    { NULL }
};

/* Debug helpers */
static void fmt_bytes(GString *s, uint8_t *bytes, int len)
{
    int32_t i;
    for (i = 0; i < len; i++) {
        if (i && i % 16 == 0) {
            g_string_append_c(s, '\n');
        }
        g_string_append_printf(s, "%02x ", bytes[i]);
    }
}

static void vscmi_dump_msg(struct virtio_scmi_request *msg, size_t len)
{
    g_autoptr(GString) s = g_string_new("\n");

    g_string_append_printf(s, "hdr: %x\n", msg->hdr);

    fmt_bytes(s, (uint8_t *)msg->data, len-sizeof(msg->hdr));

    g_info("%s: %s", __func__, s->str);
}

/*
 * vscmi_queue_set_started: set vq handler
 *
 */
static void vscmi_copy_req(VuVirtqElement *src, VuVirtqElement *dst)
{
    memcpy(dst->in_sg[0].iov_base, src->out_sg[0].iov_base, src->out_sg[0].iov_len);
    dst->in_sg[0].iov_len = src->out_sg[0].iov_len;

    dst->out_sg[0].iov_len = src->in_sg[0].iov_len;

    return;
}

static int vscmi_notify_dst(int qidx, VuChnl *src_chnl, VuChnl *dst_chnl)
{
    VuVirtq *dst_vq;

    g_info("%s: path %s idx %d", __func__, src_chnl->path, qidx);

    if (qidx >= src_chnl->dev.parent.max_queues) {
        g_info("%s: path %s not started yet", __func__, src_chnl->path);
        return 0;
    }

    if (!dst_chnl->started)
        return 0;

    if (qidx >= dst_chnl->dev.parent.max_queues) {
        g_info("%s: path %s not started yet", __func__, dst_chnl->path);
        return 0;
    }
    dst_vq = vu_get_queue(&dst_chnl->dev.parent, qidx);

    g_info("%s: notify path %s idx %d", __func__, dst_chnl->path, qidx);
    vu_queue_notify(&dst_chnl->dev.parent, dst_vq);

    return 1;
}


static int vscmi_forward_buffer(int qidx, VuChnl *src_chnl, VuChnl *dst_chnl)
{
    VuVirtq *dst_vq, *src_vq;
    struct virtio_scmi_request *out_hdr;
    struct virtio_scmi_response *in_hdr;
    size_t out_hdr_len, in_hdr_len;
    VuVirtqElement *elem;


//    g_info("%s: path %s idx %d", __func__, src_chnl->path, qidx);

    if (qidx >= src_chnl->dev.parent.max_queues) {
//        g_info("%s: path %s not started yet", __func__, src_chnl->path);
        return 0;
    }

    /* a request is already pending */
    if (src_chnl->elem[qidx]) {
//        g_info("%s: path %s element pending", __func__, src_chnl->path);
        return 1;
    }

    src_vq = vu_get_queue(&src_chnl->dev.parent, qidx);
    elem = vu_queue_pop(&src_chnl->dev.parent, src_vq, sizeof(VuVirtqElement));

    /* No element available */
    if (!elem) {
//        g_info("%s: path %s no new elements", __func__, src_chnl->path);
    return 0;
    }

    g_info("%s: path %s:%d got elements (in %d, out %d)", __func__,
            src_chnl->path, qidx, elem->in_num, elem->out_num);

    src_chnl->elem[qidx] = elem;

    /* destination not ready */
    if (!dst_chnl->elem[qidx]) {
//        g_info("%s: path %s no available elements", __func__, dst_chnl->path);
        return 1;
    }

    if (elem->out_num) {
        out_hdr = elem->out_sg[0].iov_base;
        out_hdr_len =  elem->out_sg[0].iov_len;
        g_info("%s: path %s sent OUT size %lu @ %p", __func__, src_chnl->path, out_hdr_len, out_hdr);
	if (debug)
            vscmi_dump_msg(out_hdr, out_hdr_len);
        vscmi_copy_req(src_chnl->elem[qidx], dst_chnl->elem[qidx]);
//        g_info("%s: path %s copied IN elem size %lu", __func__, dst_chnl->path, dst_chnl->elem[qidx]->in_sg[0].iov_len);
    } else
        out_hdr_len = 0;

    if (elem->in_num) {
        in_hdr = elem->in_sg[0].iov_base;
        in_hdr_len =  elem->out_sg[0].iov_len;
        g_info("%s: path %s sent IN size %lu @ %p", __func__, src_chnl->path, in_hdr_len, in_hdr);
    } else
        in_hdr_len = 0;

    dst_vq = vu_get_queue(&dst_chnl->dev.parent, qidx);
    vu_queue_push(&dst_chnl->dev.parent, dst_vq, dst_chnl->elem[qidx], out_hdr_len);

    dst_chnl->elem[qidx] = NULL;

    vu_queue_notify(&dst_chnl->dev.parent, dst_vq);

    return 1;
}

static void vscmi_handle_ctrl(VuDev *dev, int qidx)
{
    VuChnl *dst_chnl, *src_chnl = container_of(dev, VuChnl, dev.parent);
    VuScmi *scmi;

    if (src_chnl->guest) {
        scmi = container_of(src_chnl, VuScmi, vm_dev);
        dst_chnl =  &scmi->be_dev;
    } else {
        scmi = container_of(src_chnl, VuScmi, be_dev);
        dst_chnl =  &scmi->vm_dev;
    }

    g_info("%s: path %s idx %d", __func__, src_chnl->path, qidx);

    if (notification)
        vscmi_notify_dst(qidx, src_chnl, dst_chnl);
    else {
        for (;;) {
            if (!vscmi_forward_buffer(qidx, src_chnl, dst_chnl))
                break;

            if (!vscmi_forward_buffer(qidx, dst_chnl, src_chnl))
                break;
        }
    }
}

/* Virtio helpers */

/*
 * vscmi_get_features: return device features
 *
 */
static uint64_t vscmi_get_features(VuDev *dev)
{
    uint64_t features = 1ull << VIRTIO_SCMI_F_P2A_CHANNELS;
    features |= 1ull << VIRTIO_F_ACCESS_PLATFORM;
    VuChnl *chnl = container_of(dev, VuChnl, dev.parent);
    g_autoptr(GString) s = g_string_new(" ");

    g_string_append_printf(s, " 0x%" PRIx64 "", features);
    g_info("%s: path %s features: %s", __func__, chnl->path, s->str);

    return features;
}

/*
 * vscmi_set_features: features set by driver
 *
 */
static void vscmi_set_features(VuDev *dev, uint64_t features)
{
    if (verbose && features) {
        VuChnl *chnl = container_of(dev, VuChnl, dev.parent);
        g_autoptr(GString) s = g_string_new(" ");

        g_string_append_printf(s, " 0x%" PRIx64 "", features);
        g_info("%s: path %s features: %s", __func__, chnl->path, s->str);
    }
}

/*
 * vscmi_queue_set_started: set vq handler
 *
 */
static void
vscmi_queue_set_started(VuDev *dev, int qidx, bool started)
{
    VuChnl *chnl = container_of(dev, VuChnl, dev.parent);
    VuVirtq *vq = vu_get_queue(dev, qidx);

    g_info("%s: path %s idx %d:%d", __func__, chnl->path, qidx, started);

    chnl->started = started;

    if (notification && chnl->guest) {
        /*
	 * In notification only mode, backend is a device that need to be setup
	 * with guest vring information.
	 *
	 * Set backend vring information when guest starts.
	 */
        if(started) {
            /* Guest starts, set backend/device addresses */
            VuScmi *scmi = container_of(chnl, VuScmi, vm_dev);
            VuChnl *be_chnl = &scmi->be_dev;

            /* update vring hw address of backend */
            vu_set_queue_host_num(&be_chnl->dev.parent, qidx, -1, chnl->vring_num[qidx]);
            vu_set_queue_host_addr(&be_chnl->dev.parent, qidx, -1, &chnl->vring[qidx]);
            vu_set_queue_host_state(&be_chnl->dev.parent, qidx, -1, 1);
            vu_queue_notify(&be_chnl->dev.parent, vu_get_queue(&be_chnl->dev.parent, qidx));
        } else {
	    /* Guest stops, clear server/device status */
            VuScmi *scmi = container_of(chnl, VuScmi, vm_dev);
            VuChnl *be_chnl = &scmi->be_dev;

            /* clear ready and status bits */
            vu_set_queue_host_state(&be_chnl->dev.parent, qidx, -1, 0);
            vu_queue_notify(&be_chnl->dev.parent, vu_get_queue(&be_chnl->dev.parent, qidx));
        }
    }

    vu_set_queue_handler(dev, vq, started ? vscmi_handle_ctrl : NULL);
}

/*
 * vscmi_process_msg: process messages of vhost-user interface
 *
 */
static int wait_socket(VuChnl *device_ctx);
static void vscmi_destroy_channels(VuChnl *device_ctx);

static int vscmi_process_msg(VuDev *dev, VhostUserMsg *msg, int *do_reply)
{
    VuChnl *chnl = container_of(dev, VuChnl, dev.parent);

    if (msg->request == VHOST_USER_NONE) {
        g_info("%s: path %s VHOST_USER_NONE", __func__, chnl->path);
        vscmi_destroy_channels(chnl);
        wait_socket(chnl);
        return 1;
    }

    if (notification && chnl->guest) {
        /*
	 * In notification only mode, backend is a device that need to be setup
	 * with guest vring information.
	 *
	 * Save guest vring information.
	 */
        if (msg->request == VHOST_USER_SET_VRING_NUM) {
            unsigned int index = msg->payload.state.index;
            unsigned int num = msg->payload.state.num;

            /* Save guest virtqueue size */
            chnl->vring_num[index]= num;

            g_info("%s: path %s VHOST_USER_SET_VRING_NUM idx %d num %d", __func__, chnl->path, index, num);
        }

        if (msg->request == VHOST_USER_SET_VRING_ADDR) {
            struct vhost_vring_addr addr = msg->payload.addr, *vra = &addr;
            unsigned int index = vra->index;

            /* Save guest virtqueue addresses */
            chnl->vring[index].index = index;
            chnl->vring[index].desc_user_addr = (uint64_t)vu_qva_to_gpa(dev,vra->desc_user_addr);
            chnl->vring[index].used_user_addr = (uint64_t)vu_qva_to_gpa(dev,vra->used_user_addr);
            chnl->vring[index].avail_user_addr = (uint64_t)vu_qva_to_gpa(dev,vra->avail_user_addr);

            g_info("%s: path %s VHOST_USER_SET_VRING_ADDR idx %d", __func__, chnl->path, index);
            g_info("    guest phys desc:   0x%016" PRIx64 , (uint64_t)chnl->vring[index].desc_user_addr);
            g_info("    guest phys used:   0x%016" PRIx64 , (uint64_t)chnl->vring[index].used_user_addr);
            g_info("    guest phys avail:  0x%016" PRIx64 , (uint64_t)chnl->vring[index].avail_user_addr);
        }
    }

    if (debug) {
        g_info("%s: path %s : request %d", __func__, chnl->path, msg->request);
    }

    return 0;
}

static const VuDevIface vuiface = {
    .set_features = vscmi_set_features,
    .get_features = vscmi_get_features,
    .queue_set_started = vscmi_queue_set_started,
    .process_msg = vscmi_process_msg,
};

static gboolean hangup(gpointer user_data)
{
    GMainLoop *loop = (GMainLoop *) user_data;
    g_info("%s: caught hangup/quit signal, quitting main loop", __func__);
    g_main_loop_quit(loop);
    return true;
}

static void vscmi_panic(VuDev *dev, const char *msg)
{
    g_info("%s\n", __func__);
    g_critical("%s\n", msg);
    exit(EXIT_FAILURE);
}

/* Print vhost-user.json backend program capabilities */
static void print_capabilities(void)
{
    g_info("%s\n", __func__);
    printf("{\n");
    printf("  \"type\": \"scmi\"\n");
    printf("}\n");
}

static void vscmi_destroy_channels(VuChnl *device_ctx)
{
    int i;

    g_info("%s: %s", __func__, device_ctx->path);
    if (device_ctx->socket != NULL) {
        vug_deinit(&device_ctx->dev);
        g_socket_close(device_ctx->socket, NULL);
        g_object_unref(device_ctx->socket);
        device_ctx->socket = NULL;
    }

    for (i = 0; i < VHOST_USER_SCMI_MAX_QUEUES; i++)
        device_ctx->elem[i] = NULL;

    unlink(device_ctx->path);
}

static void vscmi_destroy(VuScmi *scmi)
{
    g_info("%s\n", __func__);
    vscmi_destroy_channels(&scmi->vm_dev);
    vscmi_destroy_channels(&scmi->be_dev);
}

static int vscmi_init_channels(VuChnl *device_ctx)
{
    GError *error = NULL;
    const gchar *socket_path = (const gchar *) device_ctx->path;

    /*
     * Now create a vhost-user socket that we will receive messages
     * on
     */
    if (!socket_path)
        return 0;

    g_autoptr(GSocketAddress) addr = g_unix_socket_address_new(socket_path);
    g_autoptr(GSocket) bind_socket = g_socket_new(G_SOCKET_FAMILY_UNIX, G_SOCKET_TYPE_STREAM,
                                                  G_SOCKET_PROTOCOL_DEFAULT, &error);

    if (!g_socket_bind(bind_socket, addr, false, &error)) {
        g_printerr("Failed to bind to socket at %s (%s).\n",
                   socket_path, error->message);
        return VIRTIO_SCMI_MSG_ERR;
    }

    if (!g_socket_listen(bind_socket, &error)) {
        g_printerr("Failed to listen on socket %s (%s).\n",
                   socket_path, error->message);
        return VIRTIO_SCMI_MSG_ERR;
    }

    g_message("awaiting connection to %s", socket_path);
    device_ctx->socket = g_socket_accept(bind_socket, NULL, &error);
    if (!device_ctx->socket) {
        g_printerr("Failed to accept on socket %s (%s).\n",
                   socket_path, error->message);
        return VIRTIO_SCMI_MSG_ERR;
    }
    g_message("got a connection to %s", socket_path);

    if (!vug_init(&device_ctx->dev, VHOST_USER_SCMI_MAX_QUEUES, g_socket_get_fd(device_ctx->socket),
        vscmi_panic, &vuiface)) {
        g_printerr("Failed to initialize libvhost-user-glib.\n");
        return VIRTIO_SCMI_MSG_ERR;
    }

    return VIRTIO_SCMI_MSG_OK;
}

static gpointer server_wait_vm_thread(gpointer data)
{
    VuChnl *device_ctx = (VuChnl *)data;

    vscmi_init_channels(device_ctx);

    return NULL;
}

static int wait_socket(VuChnl *device_ctx)
{

    device_ctx->thread = g_thread_new(device_ctx->path, server_wait_vm_thread, device_ctx);

    return VIRTIO_SCMI_MSG_OK;
}

int main(int argc, char *argv[])
{
    GError *error = NULL;
    GOptionContext *context;
    g_autoptr(GSocket) socket = NULL;
    VuScmi scmi = {0};

    context = g_option_context_new("vhost-user emulation of SCMI device");
    g_option_context_add_main_entries(context, options, "vhost-user-scmi");
    if (!g_option_context_parse(context, &argc, &argv, &error)) {
        g_printerr("option parsing failed: %s\n", error->message);
        exit(1);
    }

    if (print_cap) {
        print_capabilities();
        exit(0);
    }

    if (!socket_path_vm ||  !socket_path_be) {
        g_printerr("Please specify --socket-path for both vm and be\n");
        exit(EXIT_FAILURE);
    }

    if (verbose) {
        g_log_set_handler(NULL, G_LOG_LEVEL_MASK, g_log_default_handler, NULL);
        g_setenv("G_MESSAGES_DEBUG", "all", true);
    } else {
        g_log_set_handler(NULL, G_LOG_LEVEL_WARNING | G_LOG_LEVEL_CRITICAL |
                          G_LOG_LEVEL_ERROR, g_log_default_handler, NULL);
    }
    /*
     * Now create a vhost-user sockets that we will receive messages
     * on. Once we have our handler set up we can enter the glib main
     * loop.
     */
    scmi.be_dev.path = socket_path_be;
    scmi.be_dev.guest = false;
    scmi.be_dev.started = false;

    scmi.vm_dev.path = socket_path_vm;
    scmi.vm_dev.guest = true;
    scmi.vm_dev.started = false;

    wait_socket(&scmi.be_dev);
    wait_socket(&scmi.vm_dev);

    /*
     * Create the main loop first so all the various sources can be
     * added. As well as catching signals we need to ensure vug_init
     * can add it's GSource watches.
     */

    scmi.loop = g_main_loop_new(NULL, FALSE);
    /* catch exit signals */
    g_unix_signal_add(SIGHUP, hangup, scmi.loop);
    g_unix_signal_add(SIGINT, hangup, scmi.loop);

    g_message("entering main loop, awaiting messages");
    g_main_loop_run(scmi.loop);
    g_message("finished main loop, cleaning up");

    g_main_loop_unref(scmi.loop);
    vscmi_destroy(&scmi);
}