static void
net_rx_pkt_pull_data(struct NetRxPkt *pkt,
const struct iovec *iov, int iovcnt,
size_t ploff)
{
if (pkt->vlan_stripped) {
net_rx_pkt_iovec_realloc(pkt, iovcnt + 1);
pkt->vec[0].iov_base = pkt->ehdr_buf;
pkt->vec[0].iov_len = sizeof(pkt->ehdr_buf);
pkt->tot_len =
iov_size(iov, iovcnt) - ploff + sizeof(struct eth_header);
pkt->vec_len = iov_copy(pkt->vec + 1, pkt->vec_len_total - 1,
iov, iovcnt, ploff, pkt->tot_len);
} else {
net_rx_pkt_iovec_realloc(pkt, iovcnt);
pkt->tot_len = iov_size(iov, iovcnt) - ploff;
pkt->vec_len = iov_copy(pkt->vec, pkt->vec_len_total,
iov, iovcnt, ploff, pkt->tot_len);
}
eth_get_protocols(pkt->vec, pkt->vec_len, &pkt->isip4, &pkt->isip6,
&pkt->isudp, &pkt->istcp,
&pkt->l3hdr_off, &pkt->l4hdr_off, &pkt->l5hdr_off,
&pkt->ip6hdr_info, &pkt->ip4hdr_info, &pkt->l4hdr_info);
trace_net_rx_pkt_parsed(pkt->isip4, pkt->isip6, pkt->isudp, pkt->istcp,
pkt->l3hdr_off, pkt->l4hdr_off, pkt->l5hdr_off);
}
static ssize_t dump_receive_iov(DumpState *s, const struct iovec *iov, int cnt)
{
struct pcap_sf_pkthdr hdr;
int64_t ts;
int caplen;
size_t size = iov_size(iov, cnt);
struct iovec dumpiov[cnt + 1];
/* Early return in case of previous error. */
if (s->fd < 0) {
return size;
}
ts = qemu_clock_get_us(QEMU_CLOCK_VIRTUAL);
caplen = size > s->pcap_caplen ? s->pcap_caplen : size;
hdr.ts.tv_sec = ts / 1000000 + s->start_ts;
hdr.ts.tv_usec = ts % 1000000;
hdr.caplen = caplen;
hdr.len = size;
dumpiov[0].iov_base = &hdr;
dumpiov[0].iov_len = sizeof(hdr);
cnt = iov_copy(&dumpiov[1], cnt, iov, cnt, 0, caplen);
if (writev(s->fd, dumpiov, cnt + 1) != sizeof(hdr) + caplen) {
error_report("network dump write error - stopping dump");
close(s->fd);
s->fd = -1;
}
return size;
}
ssize_t nbd_wr_syncv(QIOChannel *ioc,
struct iovec *iov,
size_t niov,
size_t length,
bool do_read)
{
ssize_t done = 0;
Error *local_err = NULL;
struct iovec *local_iov = g_new(struct iovec, niov);
struct iovec *local_iov_head = local_iov;
unsigned int nlocal_iov = niov;
nlocal_iov = iov_copy(local_iov, nlocal_iov, iov, niov, 0, length);
while (nlocal_iov > 0) {
ssize_t len;
if (do_read) {
len = qio_channel_readv(ioc, local_iov, nlocal_iov, &local_err);
} else {
len = qio_channel_writev(ioc, local_iov, nlocal_iov, &local_err);
}
if (len == QIO_CHANNEL_ERR_BLOCK) {
if (qemu_in_coroutine()) {
/* XXX figure out if we can create a variant on
* qio_channel_yield() that works with AIO contexts
* and consider using that in this branch */
qemu_coroutine_yield();
} else if (done) {
/* XXX this is needed by nbd_reply_ready. */
qio_channel_wait(ioc,
do_read ? G_IO_IN : G_IO_OUT);
} else {
return -EAGAIN;
}
continue;
}
if (len < 0) {
TRACE("I/O error: %s", error_get_pretty(local_err));
error_free(local_err);
/* XXX handle Error objects */
done = -EIO;
goto cleanup;
}
if (do_read && len == 0) {
break;
}
iov_discard_front(&local_iov, &nlocal_iov, len);
done += len;
}
cleanup:
g_free(local_iov_head);
return done;
}
int qio_channel_readv_all_eof(QIOChannel *ioc,
const struct iovec *iov,
size_t niov,
Error **errp)
{
int ret = -1;
struct iovec *local_iov = g_new(struct iovec, niov);
struct iovec *local_iov_head = local_iov;
unsigned int nlocal_iov = niov;
bool partial = false;
nlocal_iov = iov_copy(local_iov, nlocal_iov,
iov, niov,
0, iov_size(iov, niov));
while (nlocal_iov > 0) {
ssize_t len;
len = qio_channel_readv(ioc, local_iov, nlocal_iov, errp);
if (len == QIO_CHANNEL_ERR_BLOCK) {
if (qemu_in_coroutine()) {
qio_channel_yield(ioc, G_IO_IN);
} else {
qio_channel_wait(ioc, G_IO_IN);
}
continue;
} else if (len < 0) {
goto cleanup;
} else if (len == 0) {
if (partial) {
error_setg(errp,
"Unexpected end-of-file before all bytes were read");
} else {
ret = 0;
}
goto cleanup;
}
partial = true;
iov_discard_front(&local_iov, &nlocal_iov, len);
}
ret = 1;
cleanup:
g_free(local_iov_head);
return ret;
}
int qio_channel_writev_all(QIOChannel *ioc,
const struct iovec *iov,
size_t niov,
Error **errp)
{
int ret = -1;
struct iovec *local_iov = g_new(struct iovec, niov);
struct iovec *local_iov_head = local_iov;
unsigned int nlocal_iov = niov;
nlocal_iov = iov_copy(local_iov, nlocal_iov,
iov, niov,
0, iov_size(iov, niov));
while (nlocal_iov > 0) {
ssize_t len;
len = qio_channel_writev(ioc, local_iov, nlocal_iov, errp);
if (len == QIO_CHANNEL_ERR_BLOCK) {
if (qemu_in_coroutine()) {
qio_channel_yield(ioc, G_IO_OUT);
} else {
qio_channel_wait(ioc, G_IO_OUT);
}
continue;
}
if (len < 0) {
goto cleanup;
}
iov_discard_front(&local_iov, &nlocal_iov, len);
}
ret = 0;
cleanup:
g_free(local_iov_head);
return ret;
}
static void
vubr_backend_recv_cb(int sock, void *ctx)
{
VubrDev *vubr = (VubrDev *) ctx;
VuDev *dev = &vubr->vudev;
VuVirtq *vq = vu_get_queue(dev, 0);
VuVirtqElement *elem = NULL;
struct iovec mhdr_sg[VIRTQUEUE_MAX_SIZE];
struct virtio_net_hdr_mrg_rxbuf mhdr;
unsigned mhdr_cnt = 0;
int hdrlen = vubr->hdrlen;
int i = 0;
struct virtio_net_hdr hdr = {
.flags = 0,
.gso_type = VIRTIO_NET_HDR_GSO_NONE
};
DPRINT("\n\n *** IN UDP RECEIVE CALLBACK ***\n\n");
DPRINT(" hdrlen = %d\n", hdrlen);
if (!vu_queue_enabled(dev, vq) ||
!vu_queue_started(dev, vq) ||
!vu_queue_avail_bytes(dev, vq, hdrlen, 0)) {
DPRINT("Got UDP packet, but no available descriptors on RX virtq.\n");
return;
}
while (1) {
struct iovec *sg;
ssize_t ret, total = 0;
unsigned int num;
elem = vu_queue_pop(dev, vq, sizeof(VuVirtqElement));
if (!elem) {
break;
}
if (elem->in_num < 1) {
fprintf(stderr, "virtio-net contains no in buffers\n");
break;
}
sg = elem->in_sg;
num = elem->in_num;
if (i == 0) {
if (hdrlen == 12) {
mhdr_cnt = iov_copy(mhdr_sg, ARRAY_SIZE(mhdr_sg),
sg, elem->in_num,
offsetof(typeof(mhdr), num_buffers),
sizeof(mhdr.num_buffers));
}
iov_from_buf(sg, elem->in_num, 0, &hdr, sizeof hdr);
total += hdrlen;
ret = iov_discard_front(&sg, &num, hdrlen);
assert(ret == hdrlen);
}
struct msghdr msg = {
.msg_name = (struct sockaddr *) &vubr->backend_udp_dest,
.msg_namelen = sizeof(struct sockaddr_in),
.msg_iov = sg,
.msg_iovlen = num,
.msg_flags = MSG_DONTWAIT,
};
do {
ret = recvmsg(vubr->backend_udp_sock, &msg, 0);
} while (ret == -1 && (errno == EINTR));
if (i == 0) {
iov_restore_front(elem->in_sg, sg, hdrlen);
}
if (ret == -1) {
if (errno == EWOULDBLOCK) {
vu_queue_rewind(dev, vq, 1);
break;
}
vubr_die("recvmsg()");
}
total += ret;
iov_truncate(elem->in_sg, elem->in_num, total);
vu_queue_fill(dev, vq, elem, total, i++);
free(elem);
elem = NULL;
break; /* could loop if DONTWAIT worked? */
}
if (mhdr_cnt) {
mhdr.num_buffers = i;
iov_from_buf(mhdr_sg, mhdr_cnt,
0,
&mhdr.num_buffers, sizeof mhdr.num_buffers);
}
vu_queue_flush(dev, vq, i);
vu_queue_notify(dev, vq);
free(elem);
}
static void
vubr_receive_cb(int sock, void *ctx)
{
VubrDev *vubr = (VubrDev *)ctx;
if (!vu_dispatch(&vubr->vudev)) {
fprintf(stderr, "Error while dispatching\n");
}
}
typedef struct WatchData {
VuDev *dev;
vu_watch_cb cb;
void *data;
} WatchData;
static void
watch_cb(int sock, void *ctx)
{
struct WatchData *wd = ctx;
wd->cb(wd->dev, VU_WATCH_IN, wd->data);
}
static void
vubr_set_watch(VuDev *dev, int fd, int condition,
vu_watch_cb cb, void *data)
{
VubrDev *vubr = container_of(dev, VubrDev, vudev);
static WatchData watches[FD_SETSIZE];
struct WatchData *wd = &watches[fd];
wd->cb = cb;
wd->data = data;
wd->dev = dev;
dispatcher_add(&vubr->dispatcher, fd, wd, watch_cb);
}
static void
vubr_handle_tx(VuDev *dev, int qidx)
{
VuVirtq *vq = vu_get_queue(dev, qidx);
VubrDev *vubr = container_of(dev, VubrDev, vudev);
int hdrlen = vubr->hdrlen;
VuVirtqElement *elem = NULL;
assert(qidx % 2);
for (;;) {
ssize_t ret;
unsigned int out_num;
struct iovec sg[VIRTQUEUE_MAX_SIZE], *out_sg;
elem = vu_queue_pop(dev, vq, sizeof(VuVirtqElement));
if (!elem) {
break;
}
out_num = elem->out_num;
out_sg = elem->out_sg;
if (out_num < 1) {
fprintf(stderr, "virtio-net header not in first element\n");
break;
}
if (VHOST_USER_BRIDGE_DEBUG) {
iov_hexdump(out_sg, out_num, stderr, "TX:", 1024);
}
if (hdrlen) {
unsigned sg_num = iov_copy(sg, ARRAY_SIZE(sg),
out_sg, out_num,
hdrlen, -1);
out_num = sg_num;
out_sg = sg;
}
struct msghdr msg = {
.msg_name = (struct sockaddr *) &vubr->backend_udp_dest,
.msg_namelen = sizeof(struct sockaddr_in),
.msg_iov = out_sg,
.msg_iovlen = out_num,
};
do {
ret = sendmsg(vubr->backend_udp_sock, &msg, 0);
} while (ret == -1 && (errno == EAGAIN || errno == EINTR));
if (ret == -1) {
vubr_die("sendmsg()");
}
vu_queue_push(dev, vq, elem, 0);
vu_queue_notify(dev, vq);
free(elem);
elem = NULL;
}
free(elem);
}
/* this function reverse the effect of iov_discard_front() it must be
* called with 'front' being the original struct iovec and 'bytes'
* being the number of bytes you shaved off
*/
static void
iov_restore_front(struct iovec *front, struct iovec *iov, size_t bytes)
{
struct iovec *cur;
for (cur = front; cur != iov; cur++) {
assert(bytes >= cur->iov_len);
bytes -= cur->iov_len;
}
cur->iov_base -= bytes;
cur->iov_len += bytes;
}
static void
iov_truncate(struct iovec *iov, unsigned iovc, size_t bytes)
{
unsigned i;
for (i = 0; i < iovc; i++, iov++) {
if (bytes < iov->iov_len) {
iov->iov_len = bytes;
return;
}
bytes -= iov->iov_len;
}
assert(!"couldn't truncate iov");
}
ssize_t iov_send_recv(int sockfd, const struct iovec *_iov, unsigned iov_cnt,
size_t offset, size_t bytes,
bool do_send)
{
ssize_t total = 0;
ssize_t ret;
size_t orig_len, tail;
unsigned niov;
struct iovec *local_iov, *iov;
if (bytes <= 0) {
return 0;
}
local_iov = g_new0(struct iovec, iov_cnt);
iov_copy(local_iov, iov_cnt, _iov, iov_cnt, offset, bytes);
offset = 0;
iov = local_iov;
while (bytes > 0) {
/* Find the start position, skipping `offset' bytes:
* first, skip all full-sized vector elements, */
for (niov = 0; niov < iov_cnt && offset >= iov[niov].iov_len; ++niov) {
offset -= iov[niov].iov_len;
}
/* niov == iov_cnt would only be valid if bytes == 0, which
* we already ruled out in the loop condition. */
assert(niov < iov_cnt);
iov += niov;
iov_cnt -= niov;
if (offset) {
/* second, skip `offset' bytes from the (now) first element,
* undo it on exit */
iov[0].iov_base += offset;
iov[0].iov_len -= offset;
}
/* Find the end position skipping `bytes' bytes: */
/* first, skip all full-sized elements */
tail = bytes;
for (niov = 0; niov < iov_cnt && iov[niov].iov_len <= tail; ++niov) {
tail -= iov[niov].iov_len;
}
if (tail) {
/* second, fixup the last element, and remember the original
* length */
assert(niov < iov_cnt);
assert(iov[niov].iov_len > tail);
orig_len = iov[niov].iov_len;
iov[niov++].iov_len = tail;
ret = do_send_recv(sockfd, iov, niov, do_send);
/* Undo the changes above before checking for errors */
iov[niov-1].iov_len = orig_len;
} else {
ret = do_send_recv(sockfd, iov, niov, do_send);
}
if (offset) {
iov[0].iov_base -= offset;
iov[0].iov_len += offset;
}
if (ret < 0) {
assert(errno != EINTR);
g_free(local_iov);
if (errno == EAGAIN && total > 0) {
return total;
}
return -1;
}
if (ret == 0 && !do_send) {
/* recv returns 0 when the peer has performed an orderly
* shutdown. */
break;
}
/* Prepare for the next iteration */
offset += ret;
total += ret;
bytes -= ret;
}
g_free(local_iov);
return total;
}
