1// SPDX-License-Identifier: GPL-2.0
  2#include <linux/bpf.h>
  3#include <bpf/bpf_helpers.h>
  4#include <linux/if_ether.h>
  5#include <linux/in.h>
  6#include <linux/ip.h>
  7#include <linux/udp.h>
  8#include <linux/pkt_cls.h>
  9
 10long change_tail_ret = 1;
 11
 12static __always_inline struct iphdr *parse_ip_header(struct __sk_buff *skb, int *ip_proto)
 13{
 14	void *data_end = (void *)(long)skb->data_end;
 15	void *data = (void *)(long)skb->data;
 16	struct ethhdr *eth = data;
 17	struct iphdr *iph;
 18
 19	/* Verify Ethernet header */
 20	if ((void *)(data + sizeof(*eth)) > data_end)
 21		return NULL;
 22
 23	/* Skip Ethernet header to get to IP header */
 24	iph = (void *)(data + sizeof(struct ethhdr));
 25
 26	/* Verify IP header */
 27	if ((void *)(data + sizeof(struct ethhdr) + sizeof(*iph)) > data_end)
 28		return NULL;
 29
 30	/* Basic IP header validation */
 31	if (iph->version != 4)  /* Only support IPv4 */
 32		return NULL;
 33
 34	if (iph->ihl < 5)  /* Minimum IP header length */
 35		return NULL;
 36
 37	*ip_proto = iph->protocol;
 38	return iph;
 39}
 40
 41static __always_inline struct udphdr *parse_udp_header(struct __sk_buff *skb, struct iphdr *iph)
 42{
 43	void *data_end = (void *)(long)skb->data_end;
 44	void *hdr = (void *)iph;
 45	struct udphdr *udp;
 46
 47	/* Calculate UDP header position */
 48	udp = hdr + (iph->ihl * 4);
 49	hdr = (void *)udp;
 50
 51	/* Verify UDP header bounds */
 52	if ((void *)(hdr + sizeof(*udp)) > data_end)
 53		return NULL;
 54
 55	return udp;
 56}
 57
 58SEC("tc/ingress")
 59int change_tail(struct __sk_buff *skb)
 60{
 61	int len = skb->len;
 62	struct udphdr *udp;
 63	struct iphdr *iph;
 64	void *data_end;
 65	char *payload;
 66	int ip_proto;
 67
 68	bpf_skb_pull_data(skb, len);
 69
 70	data_end = (void *)(long)skb->data_end;
 71	iph = parse_ip_header(skb, &ip_proto);
 72	if (!iph)
 73		return TCX_PASS;
 74
 75	if (ip_proto != IPPROTO_UDP)
 76		return TCX_PASS;
 77
 78	udp = parse_udp_header(skb, iph);
 79	if (!udp)
 80		return TCX_PASS;
 81
 82	payload = (char *)udp + (sizeof(struct udphdr));
 83	if (payload + 1 > (char *)data_end)
 84		return TCX_PASS;
 85
 86	if (payload[0] == 'T') { /* Trim the packet */
 87		change_tail_ret = bpf_skb_change_tail(skb, len - 1, 0);
 88		if (!change_tail_ret)
 89			bpf_skb_change_tail(skb, len, 0);
 90		return TCX_PASS;
 91	} else if (payload[0] == 'G') { /* Grow the packet */
 92		change_tail_ret = bpf_skb_change_tail(skb, len + 1, 0);
 93		if (!change_tail_ret)
 94			bpf_skb_change_tail(skb, len, 0);
 95		return TCX_PASS;
 96	} else if (payload[0] == 'E') { /* Error */
 97		change_tail_ret = bpf_skb_change_tail(skb, 65535, 0);
 98		return TCX_PASS;
 99	} else if (payload[0] == 'Z') { /* Zero */
100		change_tail_ret = bpf_skb_change_tail(skb, 0, 0);
101		return TCX_PASS;
102	}
103	return TCX_DROP;
104}
105
106char _license[] SEC("license") = "GPL";