[RFC PATCH v1 04/12] vhost/vsock: non-linear skb handling support

[Arseniy Krasnov <AVKrasnov@xxxxxxxxxxxxxx>]

This adds copying to guest's virtio buffers from non-linear skbs. Such
skbs are created by protocol layer when MSG_ZEROCOPY flags is used.

Signed-off-by: Arseniy Krasnov <AVKrasnov@xxxxxxxxxxxxxx>
---
 drivers/vhost/vsock.c        | 56 ++++++++++++++++++++++++++++++++----
 include/linux/virtio_vsock.h | 12 ++++++++
 2 files changed, 63 insertions(+), 5 deletions(-)

diff --git a/drivers/vhost/vsock.c b/drivers/vhost/vsock.c
index 1f3b89c885cc..60b9cafa3e31 100644
--- a/drivers/vhost/vsock.c
+++ b/drivers/vhost/vsock.c
@@ -86,6 +86,44 @@ static struct vhost_vsock *vhost_vsock_get(u32 guest_cid)
 	return NULL;
 }
 
+static int vhost_transport_copy_nonlinear_skb(struct sk_buff *skb,
+					      struct iov_iter *iov_iter,
+					      size_t len)
+{
+	size_t rest_len = len;
+
+	while (rest_len && virtio_vsock_skb_has_frags(skb)) {
+		struct bio_vec *curr_vec;
+		size_t curr_vec_end;
+		size_t to_copy;
+		int curr_frag;
+		int curr_offs;
+
+		curr_frag = VIRTIO_VSOCK_SKB_CB(skb)->curr_frag;
+		curr_offs = VIRTIO_VSOCK_SKB_CB(skb)->frag_off;
+		curr_vec = &skb_shinfo(skb)->frags[curr_frag];
+
+		curr_vec_end = curr_vec->bv_offset + curr_vec->bv_len;
+		to_copy = min(rest_len, (size_t)(curr_vec_end - curr_offs));
+
+		if (copy_page_to_iter(curr_vec->bv_page, curr_offs,
+				      to_copy, iov_iter) != to_copy)
+			return -1;
+
+		rest_len -= to_copy;
+		VIRTIO_VSOCK_SKB_CB(skb)->frag_off += to_copy;
+
+		if (VIRTIO_VSOCK_SKB_CB(skb)->frag_off == (curr_vec_end)) {
+			VIRTIO_VSOCK_SKB_CB(skb)->curr_frag++;
+			VIRTIO_VSOCK_SKB_CB(skb)->frag_off = 0;
+		}
+	}
+
+	skb->data_len -= len;
+
+	return 0;
+}
+
 static void
 vhost_transport_do_send_pkt(struct vhost_vsock *vsock,
 			    struct vhost_virtqueue *vq)
@@ -197,11 +235,19 @@ vhost_transport_do_send_pkt(struct vhost_vsock *vsock,
 			break;
 		}
 
-		nbytes = copy_to_iter(skb->data, payload_len, &iov_iter);
-		if (nbytes != payload_len) {
-			kfree_skb(skb);
-			vq_err(vq, "Faulted on copying pkt buf\n");
-			break;
+		if (skb_is_nonlinear(skb)) {
+			if (vhost_transport_copy_nonlinear_skb(skb, &iov_iter,
+							       payload_len)) {
+				vq_err(vq, "Faulted on copying pkt buf from page\n");
+				break;
+			}
+		} else {
+			nbytes = copy_to_iter(skb->data, payload_len, &iov_iter);
+			if (nbytes != payload_len) {
+				kfree_skb(skb);
+				vq_err(vq, "Faulted on copying pkt buf\n");
+				break;
+			}
 		}
 
 		/* Deliver to monitoring devices all packets that we
diff --git a/include/linux/virtio_vsock.h b/include/linux/virtio_vsock.h
index 3f9c16611306..e7efdb78ce6e 100644
--- a/include/linux/virtio_vsock.h
+++ b/include/linux/virtio_vsock.h
@@ -12,6 +12,10 @@
 struct virtio_vsock_skb_cb {
 	bool reply;
 	bool tap_delivered;
+	/* Current fragment in 'frags' of skb. */
+	u32 curr_frag;
+	/* Offset from 0 in current fragment. */
+	u32 frag_off;
 };
 
 #define VIRTIO_VSOCK_SKB_CB(skb) ((struct virtio_vsock_skb_cb *)((skb)->cb))
@@ -46,6 +50,14 @@ static inline void virtio_vsock_skb_clear_tap_delivered(struct sk_buff *skb)
 	VIRTIO_VSOCK_SKB_CB(skb)->tap_delivered = false;
 }
 
+static inline bool virtio_vsock_skb_has_frags(struct sk_buff *skb)
+{
+	if (!skb_is_nonlinear(skb))
+		return false;
+
+	return VIRTIO_VSOCK_SKB_CB(skb)->curr_frag != skb_shinfo(skb)->nr_frags;
+}
+
 static inline void virtio_vsock_skb_rx_put(struct sk_buff *skb)
 {
 	u32 len;
-- 
2.25.1