static int init_vqs(struct virtio_balloon *vb)
{
struct virtqueue *vqs[3];
vq_callback_t *callbacks[] = { balloon_ack, balloon_ack, stats_request };
const char *names[] = { "inflate", "deflate", "stats" };
int err, nvqs;
/*
* We expect two virtqueues: inflate and deflate, and
* optionally stat.
*/
nvqs = virtio_has_feature(vb->vdev, VIRTIO_BALLOON_F_STATS_VQ) ? 3 : 2;
err = vb->vdev->config->find_vqs(vb->vdev, nvqs, vqs, callbacks, names);
if (err)
return err;
vb->inflate_vq = vqs[0];
vb->deflate_vq = vqs[1];
if (virtio_has_feature(vb->vdev, VIRTIO_BALLOON_F_STATS_VQ)) {
struct scatterlist sg;
vb->stats_vq = vqs[2];
/*
* Prime this virtqueue with one buffer so the hypervisor can
* use it to signal us later.
*/
sg_init_one(&sg, vb->stats, sizeof vb->stats);
if (virtqueue_add_outbuf(vb->stats_vq, &sg, 1, vb, GFP_KERNEL)
< 0)
BUG();
virtqueue_kick(vb->stats_vq);
}
return 0;
}
/*
* On error we are losing the status update, which isn't critical as
* this is typically used for stuff like keyboard leds.
*/
static int virtinput_send_status(struct virtio_input *vi,
u16 type, u16 code, s32 value)
{
struct virtio_input_event *stsbuf;
struct scatterlist sg[1];
unsigned long flags;
int rc;
stsbuf = kzalloc(sizeof(*stsbuf), GFP_ATOMIC);
if (!stsbuf)
return -ENOMEM;
stsbuf->type = cpu_to_le16(type);
stsbuf->code = cpu_to_le16(code);
stsbuf->value = cpu_to_le32(value);
sg_init_one(sg, stsbuf, sizeof(*stsbuf));
spin_lock_irqsave(&vi->lock, flags);
if (vi->ready) {
rc = virtqueue_add_outbuf(vi->sts, sg, 1, stsbuf, GFP_ATOMIC);
virtqueue_kick(vi->sts);
} else {
rc = -ENODEV;
}
spin_unlock_irqrestore(&vi->lock, flags);
if (rc != 0)
kfree(stsbuf);
return rc;
}
static void tell_host(struct virtio_balloon *vb, struct virtqueue *vq)
{
struct scatterlist sg;
unsigned int len;
sg_init_one(&sg, vb->pfns, sizeof(vb->pfns[0]) * vb->num_pfns);
/* We should always be able to add one buffer to an empty queue. */
virtqueue_add_outbuf(vq, &sg, 1, vb, GFP_KERNEL);
virtqueue_kick(vq);
/* When host has read buffer, this completes via balloon_ack */
wait_event(vb->acked, virtqueue_get_buf(vq, &len));
}
static void stats_handle_request(struct virtio_balloon *vb)
{
struct virtqueue *vq;
struct scatterlist sg;
unsigned int len, num_stats;
num_stats = update_balloon_stats(vb);
vq = vb->stats_vq;
if (!virtqueue_get_buf(vq, &len))
return;
sg_init_one(&sg, vb->stats, sizeof(vb->stats[0]) * num_stats);
virtqueue_add_outbuf(vq, &sg, 1, vb, GFP_KERNEL);
virtqueue_kick(vq);
}
static int send_cmd_id_stop(struct virtio_balloon *vb)
{
struct scatterlist sg;
struct virtqueue *vq = vb->free_page_vq;
int err, unused;
/* Detach all the used buffers from the vq */
while (virtqueue_get_buf(vq, &unused))
;
sg_init_one(&sg, &vb->cmd_id_stop, sizeof(vb->cmd_id_stop));
err = virtqueue_add_outbuf(vq, &sg, 1, &vb->cmd_id_stop, GFP_KERNEL);
if (!err)
virtqueue_kick(vq);
return err;
}
void vhost_vq_setup(struct vdev_info *dev, struct vq_info *info)
{
struct vhost_vring_state state = { .index = info->idx };
struct vhost_vring_file file = { .index = info->idx };
unsigned long long features = dev->vdev.features[0];
struct vhost_vring_addr addr = {
.index = info->idx,
.desc_user_addr = (uint64_t)(unsigned long)info->vring.desc,
.avail_user_addr = (uint64_t)(unsigned long)info->vring.avail,
.used_user_addr = (uint64_t)(unsigned long)info->vring.used,
};
int r;
r = ioctl(dev->control, VHOST_SET_FEATURES, &features);
assert(r >= 0);
state.num = info->vring.num;
r = ioctl(dev->control, VHOST_SET_VRING_NUM, &state);
assert(r >= 0);
state.num = 0;
r = ioctl(dev->control, VHOST_SET_VRING_BASE, &state);
assert(r >= 0);
r = ioctl(dev->control, VHOST_SET_VRING_ADDR, &addr);
assert(r >= 0);
file.fd = info->kick;
r = ioctl(dev->control, VHOST_SET_VRING_KICK, &file);
assert(r >= 0);
file.fd = info->call;
r = ioctl(dev->control, VHOST_SET_VRING_CALL, &file);
assert(r >= 0);
}
static void vq_info_add(struct vdev_info *dev, int num)
{
struct vq_info *info = &dev->vqs[dev->nvqs];
int r;
info->idx = dev->nvqs;
info->kick = eventfd(0, EFD_NONBLOCK);
info->call = eventfd(0, EFD_NONBLOCK);
r = posix_memalign(&info->ring, 4096, vring_size(num, 4096));
assert(r >= 0);
memset(info->ring, 0, vring_size(num, 4096));
vring_init(&info->vring, num, info->ring, 4096);
info->vq = vring_new_virtqueue(info->idx,
info->vring.num, 4096, &dev->vdev,
true, info->ring,
vq_notify, vq_callback, "test");
assert(info->vq);
info->vq->priv = info;
vhost_vq_setup(dev, info);
dev->fds[info->idx].fd = info->call;
dev->fds[info->idx].events = POLLIN;
dev->nvqs++;
}
static void vdev_info_init(struct vdev_info* dev, unsigned long long features)
{
int r;
memset(dev, 0, sizeof *dev);
dev->vdev.features[0] = features;
dev->vdev.features[1] = features >> 32;
dev->buf_size = 1024;
dev->buf = malloc(dev->buf_size);
assert(dev->buf);
dev->control = open("/dev/vhost-test", O_RDWR);
assert(dev->control >= 0);
r = ioctl(dev->control, VHOST_SET_OWNER, NULL);
assert(r >= 0);
dev->mem = malloc(offsetof(struct vhost_memory, regions) +
sizeof dev->mem->regions[0]);
assert(dev->mem);
memset(dev->mem, 0, offsetof(struct vhost_memory, regions) +
sizeof dev->mem->regions[0]);
dev->mem->nregions = 1;
dev->mem->regions[0].guest_phys_addr = (long)dev->buf;
dev->mem->regions[0].userspace_addr = (long)dev->buf;
dev->mem->regions[0].memory_size = dev->buf_size;
r = ioctl(dev->control, VHOST_SET_MEM_TABLE, dev->mem);
assert(r >= 0);
}
/* TODO: this is pretty bad: we get a cache line bounce
* for the wait queue on poll and another one on read,
* plus the read which is there just to clear the
* current state. */
static void wait_for_interrupt(struct vdev_info *dev)
{
int i;
unsigned long long val;
poll(dev->fds, dev->nvqs, -1);
for (i = 0; i < dev->nvqs; ++i)
if (dev->fds[i].revents & POLLIN) {
read(dev->fds[i].fd, &val, sizeof val);
}
}
static void run_test(struct vdev_info *dev, struct vq_info *vq,
bool delayed, int bufs)
{
struct scatterlist sl;
long started = 0, completed = 0;
long completed_before;
int r, test = 1;
unsigned len;
long long spurious = 0;
r = ioctl(dev->control, VHOST_TEST_RUN, &test);
assert(r >= 0);
for (;;) {
virtqueue_disable_cb(vq->vq);
completed_before = completed;
do {
if (started < bufs) {
sg_init_one(&sl, dev->buf, dev->buf_size);
r = virtqueue_add_outbuf(vq->vq, &sl, 1,
dev->buf + started,
GFP_ATOMIC);
if (likely(r == 0)) {
++started;
if (unlikely(!virtqueue_kick(vq->vq)))
r = -1;
}
} else
r = -1;
/* Flush out completed bufs if any */
if (virtqueue_get_buf(vq->vq, &len)) {
++completed;
r = 0;
}
} while (r == 0);
if (completed == completed_before)
++spurious;
assert(completed <= bufs);
assert(started <= bufs);
if (completed == bufs)
break;
if (delayed) {
if (virtqueue_enable_cb_delayed(vq->vq))
wait_for_interrupt(dev);
} else {
if (virtqueue_enable_cb(vq->vq))
wait_for_interrupt(dev);
}
}
test = 0;
r = ioctl(dev->control, VHOST_TEST_RUN, &test);
assert(r >= 0);
fprintf(stderr, "spurious wakeus: 0x%llx\n", spurious);
}
const char optstring[] = "h";
const struct option longopts[] = {
{
.name = "help",
.val = 'h',
},
{
.name = "event-idx",
/**
* rpmsg_send_offchannel_raw() - send a message across to the remote processor
* @rpdev: the rpmsg channel
* @src: source address
* @dst: destination address
* @data: payload of message
* @len: length of payload
* @wait: indicates whether caller should block in case no TX buffers available
*
* This function is the base implementation for all of the rpmsg sending API.
*
* It will send @data of length @len to @dst, and say it's from @src. The
* message will be sent to the remote processor which the @rpdev channel
* belongs to.
*
* The message is sent using one of the TX buffers that are available for
* communication with this remote processor.
*
* If @wait is true, the caller will be blocked until either a TX buffer is
* available, or 15 seconds elapses (we don't want callers to
* sleep indefinitely due to misbehaving remote processors), and in that
* case -ERESTARTSYS is returned. The number '15' itself was picked
* arbitrarily; there's little point in asking drivers to provide a timeout
* value themselves.
*
* Otherwise, if @wait is false, and there are no TX buffers available,
* the function will immediately fail, and -ENOMEM will be returned.
*
* Normally drivers shouldn't use this function directly; instead, drivers
* should use the appropriate rpmsg_{try}send{to, _offchannel} API
* (see include/linux/rpmsg.h).
*
* Returns 0 on success and an appropriate error value on failure.
*/
int rpmsg_send_offchannel_raw(struct rpmsg_channel *rpdev, u32 src, u32 dst,
void *data, int len, bool wait)
{
struct virtproc_info *vrp = rpdev->vrp;
struct device *dev = &rpdev->dev;
struct scatterlist sg;
struct rpmsg_hdr *msg;
int err;
/* bcasting isn't allowed */
if (src == RPMSG_ADDR_ANY || dst == RPMSG_ADDR_ANY) {
dev_err(dev, "invalid addr (src 0x%x, dst 0x%x)\n", src, dst);
return -EINVAL;
}
/*
* We currently use fixed-sized buffers, and therefore the payload
* length is limited.
*
* One of the possible improvements here is either to support
* user-provided buffers (and then we can also support zero-copy
* messaging), or to improve the buffer allocator, to support
* variable-length buffer sizes.
*/
if (len > RPMSG_BUF_SIZE - sizeof(struct rpmsg_hdr)) {
dev_err(dev, "message is too big (%d)\n", len);
return -EMSGSIZE;
}
/* grab a buffer */
msg = get_a_tx_buf(vrp);
if (!msg && !wait)
return -ENOMEM;
/* no free buffer ? wait for one (but bail after 15 seconds) */
while (!msg) {
/* enable "tx-complete" interrupts, if not already enabled */
rpmsg_upref_sleepers(vrp);
/*
* sleep until a free buffer is available or 15 secs elapse.
* the timeout period is not configurable because there's
* little point in asking drivers to specify that.
* if later this happens to be required, it'd be easy to add.
*/
err = wait_event_interruptible_timeout(vrp->sendq,
(msg = get_a_tx_buf(vrp)),
msecs_to_jiffies(15000));
/* disable "tx-complete" interrupts if we're the last sleeper */
rpmsg_downref_sleepers(vrp);
/* timeout ? */
if (!err) {
dev_err(dev, "timeout waiting for a tx buffer\n");
return -ERESTARTSYS;
}
}
msg->len = len;
msg->flags = 0;
msg->src = src;
msg->dst = dst;
msg->reserved = 0;
memcpy(msg->data, data, len);
dev_dbg(dev, "TX From 0x%x, To 0x%x, Len %d, Flags %d, Reserved %d\n",
msg->src, msg->dst, msg->len,
msg->flags, msg->reserved);
print_hex_dump(KERN_DEBUG, "rpmsg_virtio TX: ", DUMP_PREFIX_NONE, 16, 1,
msg, sizeof(*msg) + msg->len, true);
sg_init_one(&sg, msg, sizeof(*msg) + len);
mutex_lock(&vrp->tx_lock);
/* add message to the remote processor's virtqueue */
err = virtqueue_add_outbuf(vrp->svq, &sg, 1, msg, GFP_KERNEL);
if (err) {
/*
* need to reclaim the buffer here, otherwise it's lost
* (memory won't leak, but rpmsg won't use it again for TX).
* this will wait for a buffer management overhaul.
*/
dev_err(dev, "virtqueue_add_outbuf failed: %d\n", err);
goto out;
}
/* tell the remote processor it has a pending message to read */
virtqueue_kick(vrp->svq);
out:
mutex_unlock(&vrp->tx_lock);
return err;
}
static int init_vqs(struct virtio_balloon *vb)
{
struct virtqueue *vqs[VIRTIO_BALLOON_VQ_MAX];
vq_callback_t *callbacks[VIRTIO_BALLOON_VQ_MAX];
const char *names[VIRTIO_BALLOON_VQ_MAX];
int err;
/*
* Inflateq and deflateq are used unconditionally. The names[]
* will be NULL if the related feature is not enabled, which will
* cause no allocation for the corresponding virtqueue in find_vqs.
*/
callbacks[VIRTIO_BALLOON_VQ_INFLATE] = balloon_ack;
names[VIRTIO_BALLOON_VQ_INFLATE] = "inflate";
callbacks[VIRTIO_BALLOON_VQ_DEFLATE] = balloon_ack;
names[VIRTIO_BALLOON_VQ_DEFLATE] = "deflate";
names[VIRTIO_BALLOON_VQ_STATS] = NULL;
names[VIRTIO_BALLOON_VQ_FREE_PAGE] = NULL;
if (virtio_has_feature(vb->vdev, VIRTIO_BALLOON_F_STATS_VQ)) {
names[VIRTIO_BALLOON_VQ_STATS] = "stats";
callbacks[VIRTIO_BALLOON_VQ_STATS] = stats_request;
}
if (virtio_has_feature(vb->vdev, VIRTIO_BALLOON_F_FREE_PAGE_HINT)) {
names[VIRTIO_BALLOON_VQ_FREE_PAGE] = "free_page_vq";
callbacks[VIRTIO_BALLOON_VQ_FREE_PAGE] = NULL;
}
err = vb->vdev->config->find_vqs(vb->vdev, VIRTIO_BALLOON_VQ_MAX,
vqs, callbacks, names, NULL, NULL);
if (err)
return err;
vb->inflate_vq = vqs[VIRTIO_BALLOON_VQ_INFLATE];
vb->deflate_vq = vqs[VIRTIO_BALLOON_VQ_DEFLATE];
if (virtio_has_feature(vb->vdev, VIRTIO_BALLOON_F_STATS_VQ)) {
struct scatterlist sg;
unsigned int num_stats;
vb->stats_vq = vqs[VIRTIO_BALLOON_VQ_STATS];
/*
* Prime this virtqueue with one buffer so the hypervisor can
* use it to signal us later (it can't be broken yet!).
*/
num_stats = update_balloon_stats(vb);
sg_init_one(&sg, vb->stats, sizeof(vb->stats[0]) * num_stats);
err = virtqueue_add_outbuf(vb->stats_vq, &sg, 1, vb,
GFP_KERNEL);
if (err) {
dev_warn(&vb->vdev->dev, "%s: add stat_vq failed\n",
__func__);
return err;
}
virtqueue_kick(vb->stats_vq);
}
if (virtio_has_feature(vb->vdev, VIRTIO_BALLOON_F_FREE_PAGE_HINT))
vb->free_page_vq = vqs[VIRTIO_BALLOON_VQ_FREE_PAGE];
return 0;
}
/* Put the CAIF packet on the virtio ring and kick the receiver */
static int cfv_netdev_tx(struct sk_buff *skb, struct net_device *netdev)
{
struct cfv_info *cfv = netdev_priv(netdev);
struct buf_info *buf_info;
struct scatterlist sg;
unsigned long flags;
bool flow_off = false;
int ret;
/* garbage collect released buffers */
cfv_release_used_buf(cfv->vq_tx);
spin_lock_irqsave(&cfv->tx_lock, flags);
/* Flow-off check takes into account number of cpus to make sure
* virtqueue will not be overfilled in any possible smp conditions.
*
* Flow-on is triggered when sufficient buffers are freed
*/
if (unlikely(cfv->vq_tx->num_free <= num_present_cpus())) {
flow_off = true;
cfv->stats.tx_full_ring++;
}
/* If we run out of memory, we release the memory reserve and retry
* allocation.
*/
buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
if (unlikely(!buf_info)) {
cfv->stats.tx_no_mem++;
flow_off = true;
if (cfv->reserved_mem && cfv->genpool) {
gen_pool_free(cfv->genpool, cfv->reserved_mem,
cfv->reserved_size);
cfv->reserved_mem = 0;
buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
}
}
if (unlikely(flow_off)) {
/* Turn flow on when a 1/4 of the descriptors are released */
cfv->watermark_tx = virtqueue_get_vring_size(cfv->vq_tx) / 4;
/* Enable notifications of recycled TX buffers */
virtqueue_enable_cb(cfv->vq_tx);
netif_tx_stop_all_queues(netdev);
}
if (unlikely(!buf_info)) {
/* If the memory reserve does it's job, this shouldn't happen */
netdev_warn(cfv->ndev, "Out of gen_pool memory\n");
goto err;
}
ret = virtqueue_add_outbuf(cfv->vq_tx, &sg, 1, buf_info, GFP_ATOMIC);
if (unlikely((ret < 0))) {
/* If flow control works, this shouldn't happen */
netdev_warn(cfv->ndev, "Failed adding buffer to TX vring:%d\n",
ret);
goto err;
}
/* update netdev statistics */
cfv->ndev->stats.tx_packets++;
cfv->ndev->stats.tx_bytes += skb->len;
spin_unlock_irqrestore(&cfv->tx_lock, flags);
/* tell the remote processor it has a pending message to read */
virtqueue_kick(cfv->vq_tx);
dev_kfree_skb(skb);
return NETDEV_TX_OK;
err:
spin_unlock_irqrestore(&cfv->tx_lock, flags);
cfv->ndev->stats.tx_dropped++;
free_buf_info(cfv, buf_info);
dev_kfree_skb(skb);
return NETDEV_TX_OK;
}