1 files changed, 204 insertions, 2 deletions

diff --git a/tools/testing/selftests/net/lib/xdp_native.bpf.c b/tools/testing/selftests/net/lib/xdp_native.bpf.c
index d3c66c891589..7bb2dff86d8f 100644
--- a/tools/testing/selftests/net/lib/xdp_native.bpf.c
+++ b/tools/testing/selftests/net/lib/xdp_native.bpf.c
@@ -10,15 +10,22 @@
 #include <bpf/bpf_endian.h>
 #include <bpf/bpf_helpers.h>
 
+#define MAX_ADJST_OFFSET 256
+#define MAX_PAYLOAD_LEN 5000
+#define MAX_HDR_LEN 64
+
 enum {
 	XDP_MODE = 0,
 	XDP_PORT = 1,
+	XDP_ADJST_OFFSET = 2,
+	XDP_ADJST_TAG = 3,
 } xdp_map_setup_keys;
 
 enum {
 	XDP_MODE_PASS = 0,
 	XDP_MODE_DROP = 1,
 	XDP_MODE_TX = 2,
+	XDP_MODE_TAIL_ADJST = 3,
 } xdp_map_modes;
 
 enum {
@@ -26,22 +33,28 @@ enum {
 	STATS_PASS = 1,
 	STATS_DROP = 2,
 	STATS_TX = 3,
+	STATS_ABORT = 4,
 } xdp_stats;
 
 struct {
 	__uint(type, BPF_MAP_TYPE_ARRAY);
-	__uint(max_entries, 2);
+	__uint(max_entries, 5);
 	__type(key, __u32);
 	__type(value, __s32);
 } map_xdp_setup SEC(".maps");
 
 struct {
 	__uint(type, BPF_MAP_TYPE_ARRAY);
-	__uint(max_entries, 4);
+	__uint(max_entries, 5);
 	__type(key, __u32);
 	__type(value, __u64);
 } map_xdp_stats SEC(".maps");
 
+static __u32 min(__u32 a, __u32 b)
+{
+	return a < b ? a : b;
+}
+
 static void record_stats(struct xdp_md *ctx, __u32 stat_type)
 {
 	__u64 *count;
@@ -195,6 +208,193 @@ static int xdp_mode_tx_handler(struct xdp_md *ctx, __u16 port)
 	return XDP_PASS;
 }
 
+static void *update_pkt(struct xdp_md *ctx, __s16 offset, __u32 *udp_csum)
+{
+	void *data_end = (void *)(long)ctx->data_end;
+	void *data = (void *)(long)ctx->data;
+	struct udphdr *udph = NULL;
+	struct ethhdr *eth = data;
+	__u32 len, len_new;
+
+	if (data + sizeof(*eth) > data_end)
+		return NULL;
+
+	if (eth->h_proto == bpf_htons(ETH_P_IP)) {
+		struct iphdr *iph = data + sizeof(*eth);
+		__u16 total_len;
+
+		if (iph + 1 > (struct iphdr *)data_end)
+			return NULL;
+
+		iph->tot_len = bpf_htons(bpf_ntohs(iph->tot_len) + offset);
+
+		udph = (void *)eth + sizeof(*iph) + sizeof(*eth);
+		if (!udph || udph + 1 > (struct udphdr *)data_end)
+			return NULL;
+
+		len_new = bpf_htons(bpf_ntohs(udph->len) + offset);
+	} else if (eth->h_proto  == bpf_htons(ETH_P_IPV6)) {
+		struct ipv6hdr *ipv6h = data + sizeof(*eth);
+		__u16 payload_len;
+
+		if (ipv6h + 1 > (struct ipv6hdr *)data_end)
+			return NULL;
+
+		udph = (void *)eth + sizeof(*ipv6h) + sizeof(*eth);
+		if (!udph || udph + 1 > (struct udphdr *)data_end)
+			return NULL;
+
+		*udp_csum = ~((__u32)udph->check);
+
+		len = ipv6h->payload_len;
+		len_new = bpf_htons(bpf_ntohs(len) + offset);
+		ipv6h->payload_len = len_new;
+
+		*udp_csum = bpf_csum_diff(&len, sizeof(len), &len_new,
+					  sizeof(len_new), *udp_csum);
+
+		len = udph->len;
+		len_new = bpf_htons(bpf_ntohs(udph->len) + offset);
+		*udp_csum = bpf_csum_diff(&len, sizeof(len), &len_new,
+					  sizeof(len_new), *udp_csum);
+	} else {
+		return NULL;
+	}
+
+	udph->len = len_new;
+
+	return udph;
+}
+
+static __u16 csum_fold_helper(__u32 csum)
+{
+	return ~((csum & 0xffff) + (csum >> 16)) ? : 0xffff;
+}
+
+static int xdp_adjst_tail_shrnk_data(struct xdp_md *ctx, __u16 offset,
+				     __u32 hdr_len)
+{
+	char tmp_buff[MAX_ADJST_OFFSET];
+	__u32 buff_pos, udp_csum = 0;
+	struct udphdr *udph = NULL;
+	__u32 buff_len;
+
+	udph = update_pkt(ctx, 0 - offset, &udp_csum);
+	if (!udph)
+		return -1;
+
+	buff_len = bpf_xdp_get_buff_len(ctx);
+
+	offset = (offset & 0x1ff) >= MAX_ADJST_OFFSET ? MAX_ADJST_OFFSET :
+				     offset & 0xff;
+	if (offset == 0)
+		return -1;
+
+	/* Make sure we have enough data to avoid eating the header */
+	if (buff_len - offset < hdr_len)
+		return -1;
+
+	buff_pos = buff_len - offset;
+	if (bpf_xdp_load_bytes(ctx, buff_pos, tmp_buff, offset) < 0)
+		return -1;
+
+	udp_csum = bpf_csum_diff((__be32 *)tmp_buff, offset, 0, 0, udp_csum);
+	udph->check = (__u16)csum_fold_helper(udp_csum);
+
+	if (bpf_xdp_adjust_tail(ctx, 0 - offset) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int xdp_adjst_tail_grow_data(struct xdp_md *ctx, __u16 offset)
+{
+	char tmp_buff[MAX_ADJST_OFFSET];
+	__u32 buff_pos, udp_csum = 0;
+	__u32 buff_len, hdr_len, key;
+	struct udphdr *udph;
+	__s32 *val;
+	__u8 tag;
+
+	/* Proceed to update the packet headers before attempting to adjuste
+	 * the tail. Once the tail is adjusted we lose access to the offset
+	 * amount of data at the end of the packet which is crucial to update
+	 * the checksum.
+	 * Since any failure beyond this would abort the packet, we should
+	 * not worry about passing a packet up the stack with wrong headers
+	 */
+	udph = update_pkt(ctx, offset, &udp_csum);
+	if (!udph)
+		return -1;
+
+	key = XDP_ADJST_TAG;
+	val = bpf_map_lookup_elem(&map_xdp_setup, &key);
+	if (!val)
+		return -1;
+
+	tag = (__u8)(*val);
+
+	for (int i = 0; i < MAX_ADJST_OFFSET; i++)
+		__builtin_memcpy(&tmp_buff[i], &tag, 1);
+
+	offset = (offset & 0x1ff) >= MAX_ADJST_OFFSET ? MAX_ADJST_OFFSET :
+				     offset & 0xff;
+	if (offset == 0)
+		return -1;
+
+	udp_csum = bpf_csum_diff(0, 0, (__be32 *)tmp_buff, offset, udp_csum);
+	udph->check = (__u16)csum_fold_helper(udp_csum);
+
+	buff_len = bpf_xdp_get_buff_len(ctx);
+
+	if (bpf_xdp_adjust_tail(ctx, offset) < 0) {
+		bpf_printk("Failed to adjust tail\n");
+		return -1;
+	}
+
+	if (bpf_xdp_store_bytes(ctx, buff_len, tmp_buff, offset) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int xdp_adjst_tail(struct xdp_md *ctx, __u16 port)
+{
+	void *data = (void *)(long)ctx->data;
+	struct udphdr *udph = NULL;
+	__s32 *adjust_offset, *val;
+	__u32 key, hdr_len;
+	void *offset_ptr;
+	__u8 tag;
+	int ret;
+
+	udph = filter_udphdr(ctx, port);
+	if (!udph)
+		return XDP_PASS;
+
+	hdr_len = (void *)udph - data + sizeof(struct udphdr);
+	key = XDP_ADJST_OFFSET;
+	adjust_offset = bpf_map_lookup_elem(&map_xdp_setup, &key);
+	if (!adjust_offset)
+		return XDP_PASS;
+
+	if (*adjust_offset < 0)
+		ret = xdp_adjst_tail_shrnk_data(ctx,
+						(__u16)(0 - *adjust_offset),
+						hdr_len);
+	else
+		ret = xdp_adjst_tail_grow_data(ctx, (__u16)(*adjust_offset));
+	if (ret)
+		goto abort_pkt;
+
+	record_stats(ctx, STATS_PASS);
+	return XDP_PASS;
+
+abort_pkt:
+	record_stats(ctx, STATS_ABORT);
+	return XDP_ABORTED;
+}
+
 static int xdp_prog_common(struct xdp_md *ctx)
 {
 	__u32 key, *port;
@@ -217,6 +417,8 @@ static int xdp_prog_common(struct xdp_md *ctx)
 		return xdp_mode_drop_handler(ctx, (__u16)(*port));
 	case XDP_MODE_TX:
 		return xdp_mode_tx_handler(ctx, (__u16)(*port));
+	case XDP_MODE_TAIL_ADJST:
+		return xdp_adjst_tail(ctx, (__u16)(*port));
 	}
 
 	/* Default action is to simple pass */