1 /* Copyright (c) 2016 Facebook
2  *
3  * This program is free software; you can redistribute it and/or
4  * modify it under the terms of version 2 of the GNU General Public
5  * License as published by the Free Software Foundation.
6  */
7 #define KBUILD_MODNAME "foo"
8 #include <linux/if_ether.h>
9 #include <linux/if_vlan.h>
10 #include <linux/ip.h>
11 #include <linux/ipv6.h>
12 #include <linux/in.h>
13 #include <linux/tcp.h>
14 #include <linux/udp.h>
15 #include <uapi/linux/bpf.h>
16 #include <net/ip.h>
17 #include <bpf/bpf_helpers.h>
18 
19 #define DEFAULT_PKTGEN_UDP_PORT 9
20 #define DEBUG 0
21 
tcp(void * data,uint64_t tp_off,void * data_end)22 static int tcp(void *data, uint64_t tp_off, void *data_end)
23 {
24 	struct tcphdr *tcp = data + tp_off;
25 
26 	if (tcp + 1 > data_end)
27 		return 0;
28 	if (tcp->dest == htons(80) || tcp->source == htons(80))
29 		return TC_ACT_SHOT;
30 	return 0;
31 }
32 
udp(void * data,uint64_t tp_off,void * data_end)33 static int udp(void *data, uint64_t tp_off, void *data_end)
34 {
35 	struct udphdr *udp = data + tp_off;
36 
37 	if (udp + 1 > data_end)
38 		return 0;
39 	if (udp->dest == htons(DEFAULT_PKTGEN_UDP_PORT) ||
40 	    udp->source == htons(DEFAULT_PKTGEN_UDP_PORT)) {
41 		if (DEBUG) {
42 			char fmt[] = "udp port 9 indeed\n";
43 
44 			bpf_trace_printk(fmt, sizeof(fmt));
45 		}
46 		return TC_ACT_SHOT;
47 	}
48 	return 0;
49 }
50 
parse_ipv4(void * data,uint64_t nh_off,void * data_end)51 static int parse_ipv4(void *data, uint64_t nh_off, void *data_end)
52 {
53 	struct iphdr *iph;
54 	uint64_t ihl_len;
55 
56 	iph = data + nh_off;
57 	if (iph + 1 > data_end)
58 		return 0;
59 
60 	if (ip_is_fragment(iph))
61 		return 0;
62 	ihl_len = iph->ihl * 4;
63 
64 	if (iph->protocol == IPPROTO_IPIP) {
65 		iph = data + nh_off + ihl_len;
66 		if (iph + 1 > data_end)
67 			return 0;
68 		ihl_len += iph->ihl * 4;
69 	}
70 
71 	if (iph->protocol == IPPROTO_TCP)
72 		return tcp(data, nh_off + ihl_len, data_end);
73 	else if (iph->protocol == IPPROTO_UDP)
74 		return udp(data, nh_off + ihl_len, data_end);
75 	return 0;
76 }
77 
parse_ipv6(void * data,uint64_t nh_off,void * data_end)78 static int parse_ipv6(void *data, uint64_t nh_off, void *data_end)
79 {
80 	struct ipv6hdr *ip6h;
81 	struct iphdr *iph;
82 	uint64_t ihl_len = sizeof(struct ipv6hdr);
83 	uint64_t nexthdr;
84 
85 	ip6h = data + nh_off;
86 	if (ip6h + 1 > data_end)
87 		return 0;
88 
89 	nexthdr = ip6h->nexthdr;
90 
91 	if (nexthdr == IPPROTO_IPIP) {
92 		iph = data + nh_off + ihl_len;
93 		if (iph + 1 > data_end)
94 			return 0;
95 		ihl_len += iph->ihl * 4;
96 		nexthdr = iph->protocol;
97 	} else if (nexthdr == IPPROTO_IPV6) {
98 		ip6h = data + nh_off + ihl_len;
99 		if (ip6h + 1 > data_end)
100 			return 0;
101 		ihl_len += sizeof(struct ipv6hdr);
102 		nexthdr = ip6h->nexthdr;
103 	}
104 
105 	if (nexthdr == IPPROTO_TCP)
106 		return tcp(data, nh_off + ihl_len, data_end);
107 	else if (nexthdr == IPPROTO_UDP)
108 		return udp(data, nh_off + ihl_len, data_end);
109 	return 0;
110 }
111 
112 SEC("varlen")
handle_ingress(struct __sk_buff * skb)113 int handle_ingress(struct __sk_buff *skb)
114 {
115 	void *data = (void *)(long)skb->data;
116 	struct ethhdr *eth = data;
117 	void *data_end = (void *)(long)skb->data_end;
118 	uint64_t h_proto, nh_off;
119 
120 	nh_off = sizeof(*eth);
121 	if (data + nh_off > data_end)
122 		return 0;
123 
124 	h_proto = eth->h_proto;
125 
126 	if (h_proto == ETH_P_8021Q || h_proto == ETH_P_8021AD) {
127 		struct vlan_hdr *vhdr;
128 
129 		vhdr = data + nh_off;
130 		nh_off += sizeof(struct vlan_hdr);
131 		if (data + nh_off > data_end)
132 			return 0;
133 		h_proto = vhdr->h_vlan_encapsulated_proto;
134 	}
135 	if (h_proto == ETH_P_8021Q || h_proto == ETH_P_8021AD) {
136 		struct vlan_hdr *vhdr;
137 
138 		vhdr = data + nh_off;
139 		nh_off += sizeof(struct vlan_hdr);
140 		if (data + nh_off > data_end)
141 			return 0;
142 		h_proto = vhdr->h_vlan_encapsulated_proto;
143 	}
144 	if (h_proto == htons(ETH_P_IP))
145 		return parse_ipv4(data, nh_off, data_end);
146 	else if (h_proto == htons(ETH_P_IPV6))
147 		return parse_ipv6(data, nh_off, data_end);
148 	return 0;
149 }
150 char _license[] SEC("license") = "GPL";
151