1#include <uapi/linux/bpf.h>
  2#include "bpf_helpers.h"
  3#include <uapi/linux/in.h>
  4#include <uapi/linux/if.h>
  5#include <uapi/linux/if_ether.h>
  6#include <uapi/linux/ip.h>
  7#include <uapi/linux/ipv6.h>
  8#include <uapi/linux/if_tunnel.h>
  9#define IP_MF		0x2000
 10#define IP_OFFSET	0x1FFF
 11
 12struct vlan_hdr {
 13	__be16 h_vlan_TCI;
 14	__be16 h_vlan_encapsulated_proto;
 15};
 16
 17struct flow_key_record {
 18	__be32 src;
 19	__be32 dst;
 20	union {
 21		__be32 ports;
 22		__be16 port16[2];
 23	};
 24	__u16 thoff;
 25	__u8 ip_proto;
 26};
 27
 28static inline int proto_ports_offset(__u64 proto)
 29{
 30	switch (proto) {
 31	case IPPROTO_TCP:
 32	case IPPROTO_UDP:
 33	case IPPROTO_DCCP:
 34	case IPPROTO_ESP:
 35	case IPPROTO_SCTP:
 36	case IPPROTO_UDPLITE:
 37		return 0;
 38	case IPPROTO_AH:
 39		return 4;
 40	default:
 41		return 0;
 42	}
 43}
 44
 45static inline int ip_is_fragment(struct __sk_buff *ctx, __u64 nhoff)
 46{
 47	return load_half(ctx, nhoff + offsetof(struct iphdr, frag_off))
 48		& (IP_MF | IP_OFFSET);
 49}
 50
 51static inline __u32 ipv6_addr_hash(struct __sk_buff *ctx, __u64 off)
 52{
 53	__u64 w0 = load_word(ctx, off);
 54	__u64 w1 = load_word(ctx, off + 4);
 55	__u64 w2 = load_word(ctx, off + 8);
 56	__u64 w3 = load_word(ctx, off + 12);
 57
 58	return (__u32)(w0 ^ w1 ^ w2 ^ w3);
 59}
 60
 61static inline __u64 parse_ip(struct __sk_buff *skb, __u64 nhoff, __u64 *ip_proto,
 62			     struct flow_key_record *flow)
 63{
 64	__u64 verlen;
 65
 66	if (unlikely(ip_is_fragment(skb, nhoff)))
 67		*ip_proto = 0;
 68	else
 69		*ip_proto = load_byte(skb, nhoff + offsetof(struct iphdr, protocol));
 70
 71	if (*ip_proto != IPPROTO_GRE) {
 72		flow->src = load_word(skb, nhoff + offsetof(struct iphdr, saddr));
 73		flow->dst = load_word(skb, nhoff + offsetof(struct iphdr, daddr));
 74	}
 75
 76	verlen = load_byte(skb, nhoff + 0/*offsetof(struct iphdr, ihl)*/);
 77	if (likely(verlen == 0x45))
 78		nhoff += 20;
 79	else
 80		nhoff += (verlen & 0xF) << 2;
 81
 82	return nhoff;
 83}
 84
 85static inline __u64 parse_ipv6(struct __sk_buff *skb, __u64 nhoff, __u64 *ip_proto,
 86			       struct flow_key_record *flow)
 87{
 88	*ip_proto = load_byte(skb,
 89			      nhoff + offsetof(struct ipv6hdr, nexthdr));
 90	flow->src = ipv6_addr_hash(skb,
 91				   nhoff + offsetof(struct ipv6hdr, saddr));
 92	flow->dst = ipv6_addr_hash(skb,
 93				   nhoff + offsetof(struct ipv6hdr, daddr));
 94	nhoff += sizeof(struct ipv6hdr);
 95
 96	return nhoff;
 97}
 98
 99static inline bool flow_dissector(struct __sk_buff *skb,
100				  struct flow_key_record *flow)
101{
102	__u64 nhoff = ETH_HLEN;
103	__u64 ip_proto;
104	__u64 proto = load_half(skb, 12);
105	int poff;
106
107	if (proto == ETH_P_8021AD) {
108		proto = load_half(skb, nhoff + offsetof(struct vlan_hdr,
109							h_vlan_encapsulated_proto));
110		nhoff += sizeof(struct vlan_hdr);
111	}
112
113	if (proto == ETH_P_8021Q) {
114		proto = load_half(skb, nhoff + offsetof(struct vlan_hdr,
115							h_vlan_encapsulated_proto));
116		nhoff += sizeof(struct vlan_hdr);
117	}
118
119	if (likely(proto == ETH_P_IP))
120		nhoff = parse_ip(skb, nhoff, &ip_proto, flow);
121	else if (proto == ETH_P_IPV6)
122		nhoff = parse_ipv6(skb, nhoff, &ip_proto, flow);
123	else
124		return false;
125
126	switch (ip_proto) {
127	case IPPROTO_GRE: {
128		struct gre_hdr {
129			__be16 flags;
130			__be16 proto;
131		};
132
133		__u64 gre_flags = load_half(skb,
134					    nhoff + offsetof(struct gre_hdr, flags));
135		__u64 gre_proto = load_half(skb,
136					    nhoff + offsetof(struct gre_hdr, proto));
137
138		if (gre_flags & (GRE_VERSION|GRE_ROUTING))
139			break;
140
141		proto = gre_proto;
142		nhoff += 4;
143		if (gre_flags & GRE_CSUM)
144			nhoff += 4;
145		if (gre_flags & GRE_KEY)
146			nhoff += 4;
147		if (gre_flags & GRE_SEQ)
148			nhoff += 4;
149
150		if (proto == ETH_P_8021Q) {
151			proto = load_half(skb,
152					  nhoff + offsetof(struct vlan_hdr,
153							   h_vlan_encapsulated_proto));
154			nhoff += sizeof(struct vlan_hdr);
155		}
156
157		if (proto == ETH_P_IP)
158			nhoff = parse_ip(skb, nhoff, &ip_proto, flow);
159		else if (proto == ETH_P_IPV6)
160			nhoff = parse_ipv6(skb, nhoff, &ip_proto, flow);
161		else
162			return false;
163		break;
164	}
165	case IPPROTO_IPIP:
166		nhoff = parse_ip(skb, nhoff, &ip_proto, flow);
167		break;
168	case IPPROTO_IPV6:
169		nhoff = parse_ipv6(skb, nhoff, &ip_proto, flow);
170		break;
171	default:
172		break;
173	}
174
175	flow->ip_proto = ip_proto;
176	poff = proto_ports_offset(ip_proto);
177	if (poff >= 0) {
178		nhoff += poff;
179		flow->ports = load_word(skb, nhoff);
180	}
181
182	flow->thoff = (__u16) nhoff;
183
184	return true;
185}
186
187struct pair {
188	long packets;
189	long bytes;
190};
191
192struct bpf_map_def SEC("maps") hash_map = {
193	.type = BPF_MAP_TYPE_HASH,
194	.key_size = sizeof(__be32),
195	.value_size = sizeof(struct pair),
196	.max_entries = 1024,
197};
198
199SEC("socket2")
200int bpf_prog2(struct __sk_buff *skb)
201{
202	struct flow_key_record flow = {};
203	struct pair *value;
204	u32 key;
205
206	if (!flow_dissector(skb, &flow))
207		return 0;
208
209	key = flow.dst;
210	value = bpf_map_lookup_elem(&hash_map, &key);
211	if (value) {
212		__sync_fetch_and_add(&value->packets, 1);
213		__sync_fetch_and_add(&value->bytes, skb->len);
214	} else {
215		struct pair val = {1, skb->len};
216
217		bpf_map_update_elem(&hash_map, &key, &val, BPF_ANY);
218	}
219	return 0;
220}
221
222char _license[] SEC("license") = "GPL";