AF_XDP-interaction: Packet fill helpers

Signed-off-by: Jesper Dangaard Brouer <brouer@redhat.com>

AF_XDP-interaction/af_xdp_user.c

@@ -26,9 +26,11 @@
 #include <net/if.h>
 #include <linux/if_link.h>
 #include <linux/if_ether.h>
+#include <netinet/ether.h>
 #include <linux/ip.h>
 #include <linux/ipv6.h>
 #include <linux/icmpv6.h>
+#include <linux/udp.h>
 
 #include <bpf/btf.h> /* provided by libbpf */
 
@@ -39,6 +41,7 @@
 
 #include "lib_xsk_extend.h"
 #include "ethtool_utils.h"
+#include "lib_checksum.h"
 
 #define NUM_FRAMES         4096 /* Frames per queue */
 #define FRAME_SIZE         XSK_UMEM__DEFAULT_FRAME_SIZE /* 4096 */
@@ -489,6 +492,115 @@ static inline void csum_replace2(__sum16 *sum, __be16 old, __be16 new)
 	*sum = ~csum16_add(csum16_sub(~(*sum), old), new);
 }
 
+/**
+ * Packet fill helpers
+ */
+static uint8_t base_pkt_data[FRAME_SIZE];
+
+static struct ether_addr opt_tx_smac =
+{{ 0x24, 0x5e, 0xbe, 0x57, 0xf1, 0x64 }};
+static struct ether_addr opt_tx_dmac =
+{{ 0x00, 0x1b, 0x21, 0xbb, 0x9a, 0x82 }};
+
+#define MIN_PKT_SIZE 64
+static uint16_t opt_pkt_size = MIN_PKT_SIZE;
+
+#define PKT_HDR_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \
+		      sizeof(struct udphdr))
+
+#define ETH_FCS_SIZE 4
+#define PKT_SIZE		(opt_pkt_size - ETH_FCS_SIZE)
+#define IP_PKT_SIZE		(PKT_SIZE - sizeof(struct ethhdr))
+#define UDP_PKT_SIZE		(IP_PKT_SIZE - sizeof(struct iphdr))
+#define UDP_PKT_DATA_SIZE	(UDP_PKT_SIZE - sizeof(struct udphdr))
+
+static void gen_eth_hdr(struct ethhdr *eth_hdr)
+{
+	/* Ethernet header */
+	memcpy(eth_hdr->h_dest  , &opt_tx_dmac, ETH_ALEN);
+	memcpy(eth_hdr->h_source, &opt_tx_smac, ETH_ALEN);
+	eth_hdr->h_proto = htons(ETH_P_IP);
+}
+
+static bool get_ipv4_u32(char *ip_str, uint32_t *ip_addr)
+{
+	int res;
+
+	res = inet_pton(AF_INET, ip_str, ip_addr);
+	if (res <= 0) {
+		if (res == 0)
+			fprintf(stderr,	"ERROR: IP%s \"%s\" not in presentation format\n",
+				"v4", ip_str);
+		else
+			perror("inet_pton");
+		return false;
+	}
+	return true;
+}
+
+static char *opt_ip_str_src = "192.168.44.2";
+static char *opt_ip_str_dst = "192.168.44.3";
+
+static void gen_ip_hdr(struct iphdr *ip_hdr)
+{
+	uint32_t saddr;
+	uint32_t daddr;
+
+	get_ipv4_u32(opt_ip_str_src, &saddr);
+	get_ipv4_u32(opt_ip_str_dst, &daddr);
+
+	/* IP header */
+	ip_hdr->version = IPVERSION;
+	ip_hdr->ihl = 0x5; /* 20 byte header */
+	ip_hdr->tos = 0x0;
+	ip_hdr->tot_len = htons(IP_PKT_SIZE);
+	ip_hdr->id = 0;
+	ip_hdr->frag_off = 0;
+	ip_hdr->ttl = IPDEFTTL;
+	ip_hdr->protocol = IPPROTO_UDP;
+	ip_hdr->saddr = saddr;
+	ip_hdr->daddr = daddr;
+
+	/* IP header checksum */
+	ip_hdr->check = 0;
+	ip_hdr->check = ip_fast_csum((const void *)ip_hdr, ip_hdr->ihl);
+}
+
+static uint32_t opt_pkt_fill_pattern = 0x2a2b2c2d;
+
+static void gen_udp_hdr(struct udphdr *udp_hdr, struct iphdr *ip_hdr)
+{
+	/* UDP header */
+	udp_hdr->source = htons(0x1000);
+	udp_hdr->dest = htons(0x1000);
+	udp_hdr->len = htons(UDP_PKT_SIZE);
+
+	/* UDP data */
+	memset32_htonl(udp_hdr + sizeof(struct udphdr),
+		       opt_pkt_fill_pattern,
+		       UDP_PKT_DATA_SIZE);
+
+	/* UDP header checksum */
+	udp_hdr->check = 0;
+	udp_hdr->check = udp_csum(ip_hdr->saddr, ip_hdr->daddr, UDP_PKT_SIZE,
+				  IPPROTO_UDP, (__u16 *)udp_hdr);
+}
+
+static void gen_base_pkt(uint8_t *pkt_ptr)
+{
+	struct ethhdr *eth_hdr = (struct ethhdr *)pkt_ptr;
+	struct iphdr *ip_hdr = (struct iphdr *)(pkt_ptr +
+						sizeof(struct ethhdr));
+	struct udphdr *udp_hdr = (struct udphdr *)(pkt_ptr +
+						   sizeof(struct ethhdr) +
+						   sizeof(struct iphdr));
+
+	gen_eth_hdr(eth_hdr);
+	gen_ip_hdr(ip_hdr);
+	gen_udp_hdr(udp_hdr, ip_hdr);
+}
+
+
 /**
  * BTF accessing XDP-hints
  * -----------------------
@@ -851,6 +963,7 @@ static void tx_pkt(struct config *cfg,
 	pr_addr_info(__func__, pkt_addr, umem);
 
 	pkt = xsk_umem__get_data(umem->buffer, pkt_addr);
+	gen_base_pkt(pkt);
 
 	mem_free_umem_frame(&umem->mem, pkt_addr);
 }
@@ -1091,6 +1204,9 @@ int main(int argc, char **argv)
 		exit(EXIT_FAILURE);
 	}
 
+	/* Generate packets to TX */
+	gen_base_pkt((uint8_t*)&base_pkt_data);
+
 	/* Open and configure the AF_XDP (xsk) socket(s) */
 	for (i = 0; i < xsks.num; i++) {
 		struct xsk_socket_info *xski;

AF_XDP-interaction/lib_checksum.h

@@ -0,0 +1,181 @@
+// SPDX-License-Identifier: GPL-2.0
+// Code taken from kernel samples/bpf/xdpsock_user.c
+
+#ifndef __LIB_CHECKSUM_H
+#define __LIB_CHECKSUM_H
+
+static void *memset32_htonl(void *dest, __u32 val, __u32 size)
+{
+	__u32 *ptr = (__u32 *)dest;
+	int i;
+
+	val = htonl(val);
+
+	for (i = 0; i < (size & (~0x3)); i += 4)
+		ptr[i >> 2] = val;
+
+	for (; i < size; i++)
+		((char *)dest)[i] = ((char *)&val)[i & 3];
+
+	return dest;
+}
+
+/*
+ * This function code has been taken from
+ * Linux kernel lib/checksum.c
+ */
+static inline unsigned short from32to16(unsigned int x)
+{
+	/* add up 16-bit and 16-bit for 16+c bit */
+	x = (x & 0xffff) + (x >> 16);
+	/* add up carry.. */
+	x = (x & 0xffff) + (x >> 16);
+	return x;
+}
+
+/*
+ * This function code has been taken from
+ * Linux kernel lib/checksum.c
+ */
+static unsigned int do_csum(const unsigned char *buff, int len)
+{
+	unsigned int result = 0;
+	int odd;
+
+	if (len <= 0)
+		goto out;
+	odd = 1 & (unsigned long)buff;
+	if (odd) {
+#ifdef __LITTLE_ENDIAN
+		result += (*buff << 8);
+#else
+		result = *buff;
+#endif
+		len--;
+		buff++;
+	}
+	if (len >= 2) {
+		if (2 & (unsigned long)buff) {
+			result += *(unsigned short *)buff;
+			len -= 2;
+			buff += 2;
+		}
+		if (len >= 4) {
+			const unsigned char *end = buff +
+						   ((unsigned int)len & ~3);
+			unsigned int carry = 0;
+
+			do {
+				unsigned int w = *(unsigned int *)buff;
+
+				buff += 4;
+				result += carry;
+				result += w;
+				carry = (w > result);
+			} while (buff < end);
+			result += carry;
+			result = (result & 0xffff) + (result >> 16);
+		}
+		if (len & 2) {
+			result += *(unsigned short *)buff;
+			buff += 2;
+		}
+	}
+	if (len & 1)
+#ifdef __LITTLE_ENDIAN
+		result += *buff;
+#else
+		result += (*buff << 8);
+#endif
+	result = from32to16(result);
+	if (odd)
+		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
+out:
+	return result;
+}
+
+/*
+ *	This is a version of ip_compute_csum() optimized for IP headers,
+ *	which always checksum on 4 octet boundaries.
+ *	This function code has been taken from
+ *	Linux kernel lib/checksum.c
+ */
+static inline __sum16 ip_fast_csum(const void *iph, unsigned int ihl)
+{
+	return (__sum16)~do_csum(iph, ihl * 4);
+}
+
+/*
+ * Fold a partial checksum
+ * This function code has been taken from
+ * Linux kernel include/asm-generic/checksum.h
+ */
+static inline __sum16 csum_fold(__wsum csum)
+{
+	__u32 sum = (__u32)csum;
+
+	sum = (sum & 0xffff) + (sum >> 16);
+	sum = (sum & 0xffff) + (sum >> 16);
+	return (__sum16)~sum;
+}
+
+/*
+ * This function code has been taken from
+ * Linux kernel lib/checksum.c
+ */
+static inline __u32 from64to32(__u64 x)
+{
+	/* add up 32-bit and 32-bit for 32+c bit */
+	x = (x & 0xffffffff) + (x >> 32);
+	/* add up carry.. */
+	x = (x & 0xffffffff) + (x >> 32);
+	return (__u32)x;
+}
+
+__wsum csum_tcpudp_nofold(__be32 saddr, __be32 daddr,
+			  __u32 len, __u8 proto, __wsum sum);
+
+/*
+ * This function code has been taken from
+ * Linux kernel lib/checksum.c
+ */
+__wsum csum_tcpudp_nofold(__be32 saddr, __be32 daddr,
+			  __u32 len, __u8 proto, __wsum sum)
+{
+	unsigned long long s = (__u32)sum;
+
+	s += (__u32)saddr;
+	s += (__u32)daddr;
+#ifdef __BIG_ENDIAN__
+	s += proto + len;
+#else
+	s += (proto + len) << 8;
+#endif
+	return (__wsum)from64to32(s);
+}
+
+/*
+ * This function has been taken from
+ * Linux kernel include/asm-generic/checksum.h
+ */
+static inline __sum16
+csum_tcpudp_magic(__be32 saddr, __be32 daddr, __u32 len,
+		  __u8 proto, __wsum sum)
+{
+	return csum_fold(csum_tcpudp_nofold(saddr, daddr, len, proto, sum));
+}
+
+static inline __u16 udp_csum(__u32 saddr, __u32 daddr, __u32 len,
+			   __u8 proto, __u16 *udp_pkt)
+{
+	__u32 csum = 0;
+	__u32 cnt = 0;
+
+	/* udp hdr and data */
+	for (; cnt < len; cnt += 2)
+		csum += udp_pkt[cnt >> 1];
+
+	return csum_tcpudp_magic(saddr, daddr, len, proto, csum);
+}
+
+#endif /* __LIB_CHECKSUM_H */