[linux-block.git] / samples / bpf / sockex3_kern.c

/* Copyright (c) 2015 PLUMgrid, http://plumgrid.com
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of version 2 of the GNU General Public
 * License as published by the Free Software Foundation.
 */
#include <uapi/linux/bpf.h>
#include <uapi/linux/in.h>
#include <uapi/linux/if.h>
#include <uapi/linux/if_ether.h>
#include <uapi/linux/ip.h>
#include <uapi/linux/ipv6.h>
#include <uapi/linux/if_tunnel.h>
#include <uapi/linux/mpls.h>
#include <bpf/bpf_helpers.h>
#include "bpf_legacy.h"
#define IP_MF		0x2000
#define IP_OFFSET	0x1FFF

#define PROG(F) SEC("socket/"__stringify(F)) int bpf_func_##F

struct {
	__uint(type, BPF_MAP_TYPE_PROG_ARRAY);
	__uint(key_size, sizeof(u32));
	__uint(value_size, sizeof(u32));
	__uint(max_entries, 8);
} jmp_table SEC(".maps");

#define PARSE_VLAN 1
#define PARSE_MPLS 2
#define PARSE_IP 3
#define PARSE_IPV6 4

/* Protocol dispatch routine. It tail-calls next BPF program depending
 * on eth proto. Note, we could have used ...
 *
 *   bpf_tail_call(skb, &jmp_table, proto);
 *
 * ... but it would need large prog_array and cannot be optimised given
 * the map key is not static.
 */
static inline void parse_eth_proto(struct __sk_buff *skb, u32 proto)
{
	switch (proto) {
	case ETH_P_8021Q:
	case ETH_P_8021AD:
		bpf_tail_call(skb, &jmp_table, PARSE_VLAN);
		break;
	case ETH_P_MPLS_UC:
	case ETH_P_MPLS_MC:
		bpf_tail_call(skb, &jmp_table, PARSE_MPLS);
		break;
	case ETH_P_IP:
		bpf_tail_call(skb, &jmp_table, PARSE_IP);
		break;
	case ETH_P_IPV6:
		bpf_tail_call(skb, &jmp_table, PARSE_IPV6);
		break;
	}
}

struct vlan_hdr {
	__be16 h_vlan_TCI;
	__be16 h_vlan_encapsulated_proto;
};

struct flow_key_record {
	__be32 src;
	__be32 dst;
	union {
		__be32 ports;
		__be16 port16[2];
	};
	__u32 ip_proto;
};

static inline int ip_is_fragment(struct __sk_buff *ctx, __u64 nhoff)
{
	return load_half(ctx, nhoff + offsetof(struct iphdr, frag_off))
		& (IP_MF | IP_OFFSET);
}

static inline __u32 ipv6_addr_hash(struct __sk_buff *ctx, __u64 off)
{
	__u64 w0 = load_word(ctx, off);
	__u64 w1 = load_word(ctx, off + 4);
	__u64 w2 = load_word(ctx, off + 8);
	__u64 w3 = load_word(ctx, off + 12);

	return (__u32)(w0 ^ w1 ^ w2 ^ w3);
}

struct globals {
	struct flow_key_record flow;
};

struct {
	__uint(type, BPF_MAP_TYPE_ARRAY);
	__type(key, __u32);
	__type(value, struct globals);
	__uint(max_entries, 32);
} percpu_map SEC(".maps");

/* user poor man's per_cpu until native support is ready */
static struct globals *this_cpu_globals(void)
{
	u32 key = bpf_get_smp_processor_id();

	return bpf_map_lookup_elem(&percpu_map, &key);
}

/* some simple stats for user space consumption */
struct pair {
	__u64 packets;
	__u64 bytes;
};

struct {
	__uint(type, BPF_MAP_TYPE_HASH);
	__type(key, struct flow_key_record);
	__type(value, struct pair);
	__uint(max_entries, 1024);
} hash_map SEC(".maps");

static void update_stats(struct __sk_buff *skb, struct globals *g)
{
	struct flow_key_record key = g->flow;
	struct pair *value;

	value = bpf_map_lookup_elem(&hash_map, &key);
	if (value) {
		__sync_fetch_and_add(&value->packets, 1);
		__sync_fetch_and_add(&value->bytes, skb->len);
	} else {
		struct pair val = {1, skb->len};

		bpf_map_update_elem(&hash_map, &key, &val, BPF_ANY);
	}
}

static __always_inline void parse_ip_proto(struct __sk_buff *skb,
					   struct globals *g, __u32 ip_proto)
{
	__u32 nhoff = skb->cb[0];
	int poff;

	switch (ip_proto) {
	case IPPROTO_GRE: {
		struct gre_hdr {
			__be16 flags;
			__be16 proto;
		};

		__u32 gre_flags = load_half(skb,
					    nhoff + offsetof(struct gre_hdr, flags));
		__u32 gre_proto = load_half(skb,
					    nhoff + offsetof(struct gre_hdr, proto));

		if (gre_flags & (GRE_VERSION|GRE_ROUTING))
			break;

		nhoff += 4;
		if (gre_flags & GRE_CSUM)
			nhoff += 4;
		if (gre_flags & GRE_KEY)
			nhoff += 4;
		if (gre_flags & GRE_SEQ)
			nhoff += 4;

		skb->cb[0] = nhoff;
		parse_eth_proto(skb, gre_proto);
		break;
	}
	case IPPROTO_IPIP:
		parse_eth_proto(skb, ETH_P_IP);
		break;
	case IPPROTO_IPV6:
		parse_eth_proto(skb, ETH_P_IPV6);
		break;
	case IPPROTO_TCP:
	case IPPROTO_UDP:
		g->flow.ports = load_word(skb, nhoff);
	case IPPROTO_ICMP:
		g->flow.ip_proto = ip_proto;
		update_stats(skb, g);
		break;
	default:
		break;
	}
}

PROG(PARSE_IP)(struct __sk_buff *skb)
{
	struct globals *g = this_cpu_globals();
	__u32 nhoff, verlen, ip_proto;

	if (!g)
		return 0;

	nhoff = skb->cb[0];

	if (unlikely(ip_is_fragment(skb, nhoff)))
		return 0;

	ip_proto = load_byte(skb, nhoff + offsetof(struct iphdr, protocol));

	if (ip_proto != IPPROTO_GRE) {
		g->flow.src = load_word(skb, nhoff + offsetof(struct iphdr, saddr));
		g->flow.dst = load_word(skb, nhoff + offsetof(struct iphdr, daddr));
	}

	verlen = load_byte(skb, nhoff + 0/*offsetof(struct iphdr, ihl)*/);
	nhoff += (verlen & 0xF) << 2;

	skb->cb[0] = nhoff;
	parse_ip_proto(skb, g, ip_proto);
	return 0;
}

PROG(PARSE_IPV6)(struct __sk_buff *skb)
{
	struct globals *g = this_cpu_globals();
	__u32 nhoff, ip_proto;

	if (!g)
		return 0;

	nhoff = skb->cb[0];

	ip_proto = load_byte(skb,
			     nhoff + offsetof(struct ipv6hdr, nexthdr));
	g->flow.src = ipv6_addr_hash(skb,
				     nhoff + offsetof(struct ipv6hdr, saddr));
	g->flow.dst = ipv6_addr_hash(skb,
				     nhoff + offsetof(struct ipv6hdr, daddr));
	nhoff += sizeof(struct ipv6hdr);

	skb->cb[0] = nhoff;
	parse_ip_proto(skb, g, ip_proto);
	return 0;
}

PROG(PARSE_VLAN)(struct __sk_buff *skb)
{
	__u32 nhoff, proto;

	nhoff = skb->cb[0];

	proto = load_half(skb, nhoff + offsetof(struct vlan_hdr,
						h_vlan_encapsulated_proto));
	nhoff += sizeof(struct vlan_hdr);
	skb->cb[0] = nhoff;

	parse_eth_proto(skb, proto);

	return 0;
}

PROG(PARSE_MPLS)(struct __sk_buff *skb)
{
	__u32 nhoff, label;

	nhoff = skb->cb[0];

	label = load_word(skb, nhoff);
	nhoff += sizeof(struct mpls_label);
	skb->cb[0] = nhoff;

	if (label & MPLS_LS_S_MASK) {
		__u8 verlen = load_byte(skb, nhoff);
		if ((verlen & 0xF0) == 4)
			parse_eth_proto(skb, ETH_P_IP);
		else
			parse_eth_proto(skb, ETH_P_IPV6);
	} else {
		parse_eth_proto(skb, ETH_P_MPLS_UC);
	}

	return 0;
}

SEC("socket/0")
int main_prog(struct __sk_buff *skb)
{
	__u32 nhoff = ETH_HLEN;
	__u32 proto = load_half(skb, 12);

	skb->cb[0] = nhoff;
	parse_eth_proto(skb, proto);
	return 0;
}

char _license[] SEC("license") = "GPL";
Commit	Line	Data
530b2c86 AS	1	/* Copyright (c) 2015 PLUMgrid, http://plumgrid.com
	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	#include <uapi/linux/bpf.h>
530b2c86 AS	8	#include <uapi/linux/in.h>
	9	#include <uapi/linux/if.h>
	10	#include <uapi/linux/if_ether.h>
	11	#include <uapi/linux/ip.h>
	12	#include <uapi/linux/ipv6.h>
	13	#include <uapi/linux/if_tunnel.h>
	14	#include <uapi/linux/mpls.h>
03421a92 YS	15	#include <bpf/bpf_helpers.h>
03421a92 YS	16	#include "bpf_legacy.h"
530b2c86 AS	17	#define IP_MF 0x2000
	18	#define IP_OFFSET 0x1FFF
	19
	20	#define PROG(F) SEC("socket/"__stringify(F)) int bpf_func_##F
	21
59929cd1 DL	22	struct {
	23	__uint(type, BPF_MAP_TYPE_PROG_ARRAY);
	24	__uint(key_size, sizeof(u32));
	25	__uint(value_size, sizeof(u32));
	26	__uint(max_entries, 8);
	27	} jmp_table SEC(".maps");
530b2c86 AS	28
	29	#define PARSE_VLAN 1
	30	#define PARSE_MPLS 2
	31	#define PARSE_IP 3
	32	#define PARSE_IPV6 4
	33
faef26fa DB	34	/* Protocol dispatch routine. It tail-calls next BPF program depending
	35	* on eth proto. Note, we could have used ...
	36	*
	37	* bpf_tail_call(skb, &jmp_table, proto);
	38	*
	39	* ... but it would need large prog_array and cannot be optimised given
	40	* the map key is not static.
530b2c86 AS	41	*/
	42	static inline void parse_eth_proto(struct __sk_buff *skb, u32 proto)
	43	{
	44	switch (proto) {
	45	case ETH_P_8021Q:
	46	case ETH_P_8021AD:
3652c9a1	47	bpf_tail_call(skb, &jmp_table, PARSE_VLAN);
530b2c86 AS	48	break;
	49	case ETH_P_MPLS_UC:
	50	case ETH_P_MPLS_MC:
3652c9a1	51	bpf_tail_call(skb, &jmp_table, PARSE_MPLS);
530b2c86 AS	52	break;
530b2c86 AS	53	case ETH_P_IP:
3652c9a1	54	bpf_tail_call(skb, &jmp_table, PARSE_IP);
530b2c86 AS	55	break;
530b2c86 AS	56	case ETH_P_IPV6:
3652c9a1	57	bpf_tail_call(skb, &jmp_table, PARSE_IPV6);
530b2c86 AS	58	break;
	59	}
	60	}
	61
	62	struct vlan_hdr {
	63	__be16 h_vlan_TCI;
	64	__be16 h_vlan_encapsulated_proto;
	65	};
	66
32c00979	67	struct flow_key_record {
530b2c86 AS	68	__be32 src;
	69	__be32 dst;
	70	union {
	71	__be32 ports;
	72	__be16 port16[2];
	73	};
	74	__u32 ip_proto;
	75	};
	76
	77	static inline int ip_is_fragment(struct __sk_buff *ctx, __u64 nhoff)
	78	{
	79	return load_half(ctx, nhoff + offsetof(struct iphdr, frag_off))
	80	& (IP_MF \| IP_OFFSET);
	81	}
	82
	83	static inline __u32 ipv6_addr_hash(struct __sk_buff *ctx, __u64 off)
	84	{
	85	__u64 w0 = load_word(ctx, off);
	86	__u64 w1 = load_word(ctx, off + 4);
	87	__u64 w2 = load_word(ctx, off + 8);
	88	__u64 w3 = load_word(ctx, off + 12);
	89
	90	return (__u32)(w0 ^ w1 ^ w2 ^ w3);
	91	}
	92
	93	struct globals {
32c00979	94	struct flow_key_record flow;
530b2c86 AS	95	};
530b2c86 AS	96
59929cd1 DL	97	struct {
	98	__uint(type, BPF_MAP_TYPE_ARRAY);
	99	__type(key, __u32);
	100	__type(value, struct globals);
	101	__uint(max_entries, 32);
	102	} percpu_map SEC(".maps");
530b2c86 AS	103
	104	/* user poor man's per_cpu until native support is ready */
	105	static struct globals *this_cpu_globals(void)
	106	{
	107	u32 key = bpf_get_smp_processor_id();
	108
	109	return bpf_map_lookup_elem(&percpu_map, &key);
	110	}
	111
	112	/* some simple stats for user space consumption */
	113	struct pair {
	114	__u64 packets;
	115	__u64 bytes;
	116	};
	117
59929cd1 DL	118	struct {
	119	__uint(type, BPF_MAP_TYPE_HASH);
	120	__type(key, struct flow_key_record);
	121	__type(value, struct pair);
	122	__uint(max_entries, 1024);
	123	} hash_map SEC(".maps");
530b2c86 AS	124
	125	static void update_stats(struct __sk_buff skb, struct globals g)
	126	{
32c00979	127	struct flow_key_record key = g->flow;
530b2c86 AS	128	struct pair *value;
	129
	130	value = bpf_map_lookup_elem(&hash_map, &key);
	131	if (value) {
	132	__sync_fetch_and_add(&value->packets, 1);
	133	__sync_fetch_and_add(&value->bytes, skb->len);
	134	} else {
	135	struct pair val = {1, skb->len};
	136
	137	bpf_map_update_elem(&hash_map, &key, &val, BPF_ANY);
	138	}
	139	}
	140
	141	static __always_inline void parse_ip_proto(struct __sk_buff *skb,
	142	struct globals *g, __u32 ip_proto)
	143	{
d691f9e8	144	__u32 nhoff = skb->cb[0];
530b2c86 AS	145	int poff;
	146
	147	switch (ip_proto) {
	148	case IPPROTO_GRE: {
	149	struct gre_hdr {
	150	__be16 flags;
	151	__be16 proto;
	152	};
	153
	154	__u32 gre_flags = load_half(skb,
	155	nhoff + offsetof(struct gre_hdr, flags));
	156	__u32 gre_proto = load_half(skb,
	157	nhoff + offsetof(struct gre_hdr, proto));
	158
	159	if (gre_flags & (GRE_VERSION\|GRE_ROUTING))
	160	break;
	161
	162	nhoff += 4;
	163	if (gre_flags & GRE_CSUM)
	164	nhoff += 4;
	165	if (gre_flags & GRE_KEY)
	166	nhoff += 4;
	167	if (gre_flags & GRE_SEQ)
	168	nhoff += 4;
	169
d691f9e8	170	skb->cb[0] = nhoff;
530b2c86 AS	171	parse_eth_proto(skb, gre_proto);
	172	break;
	173	}
	174	case IPPROTO_IPIP:
	175	parse_eth_proto(skb, ETH_P_IP);
	176	break;
	177	case IPPROTO_IPV6:
	178	parse_eth_proto(skb, ETH_P_IPV6);
	179	break;
	180	case IPPROTO_TCP:
	181	case IPPROTO_UDP:
	182	g->flow.ports = load_word(skb, nhoff);
	183	case IPPROTO_ICMP:
	184	g->flow.ip_proto = ip_proto;
	185	update_stats(skb, g);
	186	break;
	187	default:
	188	break;
	189	}
	190	}
	191
	192	PROG(PARSE_IP)(struct __sk_buff *skb)
	193	{
	194	struct globals *g = this_cpu_globals();
	195	__u32 nhoff, verlen, ip_proto;
	196
	197	if (!g)
	198	return 0;
	199
d691f9e8	200	nhoff = skb->cb[0];
530b2c86 AS	201
	202	if (unlikely(ip_is_fragment(skb, nhoff)))
	203	return 0;
	204
	205	ip_proto = load_byte(skb, nhoff + offsetof(struct iphdr, protocol));
	206
	207	if (ip_proto != IPPROTO_GRE) {
	208	g->flow.src = load_word(skb, nhoff + offsetof(struct iphdr, saddr));
	209	g->flow.dst = load_word(skb, nhoff + offsetof(struct iphdr, daddr));
	210	}
	211
	212	verlen = load_byte(skb, nhoff + 0/offsetof(struct iphdr, ihl)/);
	213	nhoff += (verlen & 0xF) << 2;
	214
d691f9e8	215	skb->cb[0] = nhoff;
530b2c86 AS	216	parse_ip_proto(skb, g, ip_proto);
	217	return 0;
	218	}
	219
	220	PROG(PARSE_IPV6)(struct __sk_buff *skb)
	221	{
	222	struct globals *g = this_cpu_globals();
	223	__u32 nhoff, ip_proto;
	224
	225	if (!g)
	226	return 0;
	227
d691f9e8	228	nhoff = skb->cb[0];
530b2c86 AS	229
	230	ip_proto = load_byte(skb,
	231	nhoff + offsetof(struct ipv6hdr, nexthdr));
	232	g->flow.src = ipv6_addr_hash(skb,
	233	nhoff + offsetof(struct ipv6hdr, saddr));
	234	g->flow.dst = ipv6_addr_hash(skb,
	235	nhoff + offsetof(struct ipv6hdr, daddr));
	236	nhoff += sizeof(struct ipv6hdr);
	237
d691f9e8	238	skb->cb[0] = nhoff;
530b2c86 AS	239	parse_ip_proto(skb, g, ip_proto);
	240	return 0;
	241	}
	242
	243	PROG(PARSE_VLAN)(struct __sk_buff *skb)
	244	{
530b2c86 AS	245	__u32 nhoff, proto;
530b2c86 AS	246
d691f9e8	247	nhoff = skb->cb[0];
530b2c86 AS	248
	249	proto = load_half(skb, nhoff + offsetof(struct vlan_hdr,
	250	h_vlan_encapsulated_proto));
	251	nhoff += sizeof(struct vlan_hdr);
d691f9e8	252	skb->cb[0] = nhoff;
530b2c86 AS	253
	254	parse_eth_proto(skb, proto);
	255
	256	return 0;
	257	}
	258
	259	PROG(PARSE_MPLS)(struct __sk_buff *skb)
	260	{
530b2c86 AS	261	__u32 nhoff, label;
530b2c86 AS	262
d691f9e8	263	nhoff = skb->cb[0];
530b2c86 AS	264
	265	label = load_word(skb, nhoff);
	266	nhoff += sizeof(struct mpls_label);
d691f9e8	267	skb->cb[0] = nhoff;
530b2c86 AS	268
	269	if (label & MPLS_LS_S_MASK) {
	270	__u8 verlen = load_byte(skb, nhoff);
	271	if ((verlen & 0xF0) == 4)
	272	parse_eth_proto(skb, ETH_P_IP);
	273	else
	274	parse_eth_proto(skb, ETH_P_IPV6);
	275	} else {
	276	parse_eth_proto(skb, ETH_P_MPLS_UC);
	277	}
	278
	279	return 0;
	280	}
	281
	282	SEC("socket/0")
	283	int main_prog(struct __sk_buff *skb)
	284	{
530b2c86 AS	285	__u32 nhoff = ETH_HLEN;
	286	__u32 proto = load_half(skb, 12);
	287
d691f9e8	288	skb->cb[0] = nhoff;
530b2c86 AS	289	parse_eth_proto(skb, proto);
	290	return 0;
	291	}
	292
	293	char _license[] SEC("license") = "GPL";