[RFC PATCH v1 06/12] vsock/virtio: non-linear skb handling for TAP dev

[Arseniy Krasnov <AVKrasnov@sberdevices.ru>]

For TAP device new skb is created and data from the current skb is
copied to it. This adds copying data from non-linear skb to new
the skb.

Signed-off-by: Arseniy Krasnov <AVKrasnov@sberdevices.ru>
---
 net/vmw_vsock/virtio_transport_common.c | 43 +++++++++++++++++++++++--
 1 file changed, 40 insertions(+), 3 deletions(-)

diff --git a/net/vmw_vsock/virtio_transport_common.c b/net/vmw_vsock/virtio_transport_common.c
index a1581c77cf84..05ce97b967ad 100644
--- a/net/vmw_vsock/virtio_transport_common.c
+++ b/net/vmw_vsock/virtio_transport_common.c
@@ -101,6 +101,39 @@ virtio_transport_alloc_skb(struct virtio_vsock_pkt_info *info,
 	return NULL;
 }
 
+static void virtio_transport_copy_nonlinear_skb(struct sk_buff *skb,
+						void *dst,
+						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));
+
+		memcpy(dst, page_to_virt(curr_vec->bv_page) + curr_offs,
+		       to_copy);
+
+		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;
+		}
+	}
+}
+
 /* Packet capture */
 static struct sk_buff *virtio_transport_build_skb(void *opaque)
 {
@@ -109,7 +142,6 @@ static struct sk_buff *virtio_transport_build_skb(void *opaque)
 	struct af_vsockmon_hdr *hdr;
 	struct sk_buff *skb;
 	size_t payload_len;
-	void *payload_buf;
 
 	/* A packet could be split to fit the RX buffer, so we can retrieve
 	 * the payload length from the header and the buffer pointer taking
@@ -117,7 +149,6 @@ static struct sk_buff *virtio_transport_build_skb(void *opaque)
 	 */
 	pkt_hdr = virtio_vsock_hdr(pkt);
 	payload_len = pkt->len;
-	payload_buf = pkt->data;
 
 	skb = alloc_skb(sizeof(*hdr) + sizeof(*pkt_hdr) + payload_len,
 			GFP_ATOMIC);
@@ -160,7 +191,13 @@ static struct sk_buff *virtio_transport_build_skb(void *opaque)
 	skb_put_data(skb, pkt_hdr, sizeof(*pkt_hdr));
 
 	if (payload_len) {
-		skb_put_data(skb, payload_buf, payload_len);
+		if (skb_is_nonlinear(skb)) {
+			void *data = skb_put(skb, payload_len);
+
+			virtio_transport_copy_nonlinear_skb(skb, data, payload_len);
+		} else {
+			skb_put_data(skb, pkt->data, payload_len);
+		}
 	}
 
 	return skb;
-- 
2.25.1