[linux-2.6-block.git] / samples / bpf / test_lwt_bpf.c

/* Copyright (c) 2016 Thomas Graf <tgraf@tgraf.ch>
 *
 * 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.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 */

#include <stdint.h>
#include <stddef.h>
#include <linux/bpf.h>
#include <linux/ip.h>
#include <linux/in.h>
#include <linux/in6.h>
#include <linux/tcp.h>
#include <linux/udp.h>
#include <linux/icmpv6.h>
#include <linux/if_ether.h>
#include "bpf_helpers.h"
#include <string.h>

# define printk(fmt, ...)						\
		({							\
			char ____fmt[] = fmt;				\
			bpf_trace_printk(____fmt, sizeof(____fmt),	\
				     ##__VA_ARGS__);			\
		})

#define CB_MAGIC 1234

/* Test: Pass all packets through */
SEC("nop")
int do_nop(struct __sk_buff *skb)
{
	return BPF_OK;
}

/* Test: Verify context information can be accessed */
SEC("test_ctx")
int do_test_ctx(struct __sk_buff *skb)
{
	skb->cb[0] = CB_MAGIC;
	printk("len %d hash %d protocol %d\n", skb->len, skb->hash,
	       skb->protocol);
	printk("cb %d ingress_ifindex %d ifindex %d\n", skb->cb[0],
	       skb->ingress_ifindex, skb->ifindex);

	return BPF_OK;
}

/* Test: Ensure skb->cb[] buffer is cleared */
SEC("test_cb")
int do_test_cb(struct __sk_buff *skb)
{
	printk("cb0: %x cb1: %x cb2: %x\n", skb->cb[0], skb->cb[1],
	       skb->cb[2]);
	printk("cb3: %x cb4: %x\n", skb->cb[3], skb->cb[4]);

	return BPF_OK;
}

/* Test: Verify skb data can be read */
SEC("test_data")
int do_test_data(struct __sk_buff *skb)
{
	void *data = (void *)(long)skb->data;
	void *data_end = (void *)(long)skb->data_end;
	struct iphdr *iph = data;

	if (data + sizeof(*iph) > data_end) {
		printk("packet truncated\n");
		return BPF_DROP;
	}

	printk("src: %x dst: %x\n", iph->saddr, iph->daddr);

	return BPF_OK;
}

#define IP_CSUM_OFF offsetof(struct iphdr, check)
#define IP_DST_OFF offsetof(struct iphdr, daddr)
#define IP_SRC_OFF offsetof(struct iphdr, saddr)
#define IP_PROTO_OFF offsetof(struct iphdr, protocol)
#define TCP_CSUM_OFF offsetof(struct tcphdr, check)
#define UDP_CSUM_OFF offsetof(struct udphdr, check)
#define IS_PSEUDO 0x10

static inline int rewrite(struct __sk_buff *skb, uint32_t old_ip,
			  uint32_t new_ip, int rw_daddr)
{
	int ret, off = 0, flags = IS_PSEUDO;
	uint8_t proto;

	ret = bpf_skb_load_bytes(skb, IP_PROTO_OFF, &proto, 1);
	if (ret < 0) {
		printk("bpf_l4_csum_replace failed: %d\n", ret);
		return BPF_DROP;
	}

	switch (proto) {
	case IPPROTO_TCP:
		off = TCP_CSUM_OFF;
		break;

	case IPPROTO_UDP:
		off = UDP_CSUM_OFF;
		flags |= BPF_F_MARK_MANGLED_0;
		break;

	case IPPROTO_ICMPV6:
		off = offsetof(struct icmp6hdr, icmp6_cksum);
		break;
	}

	if (off) {
		ret = bpf_l4_csum_replace(skb, off, old_ip, new_ip,
					  flags | sizeof(new_ip));
		if (ret < 0) {
			printk("bpf_l4_csum_replace failed: %d\n");
			return BPF_DROP;
		}
	}

	ret = bpf_l3_csum_replace(skb, IP_CSUM_OFF, old_ip, new_ip, sizeof(new_ip));
	if (ret < 0) {
		printk("bpf_l3_csum_replace failed: %d\n", ret);
		return BPF_DROP;
	}

	if (rw_daddr)
		ret = bpf_skb_store_bytes(skb, IP_DST_OFF, &new_ip, sizeof(new_ip), 0);
	else
		ret = bpf_skb_store_bytes(skb, IP_SRC_OFF, &new_ip, sizeof(new_ip), 0);

	if (ret < 0) {
		printk("bpf_skb_store_bytes() failed: %d\n", ret);
		return BPF_DROP;
	}

	return BPF_OK;
}

/* Test: Verify skb data can be modified */
SEC("test_rewrite")
int do_test_rewrite(struct __sk_buff *skb)
{
	uint32_t old_ip, new_ip = 0x3fea8c0;
	int ret;

	ret = bpf_skb_load_bytes(skb, IP_DST_OFF, &old_ip, 4);
	if (ret < 0) {
		printk("bpf_skb_load_bytes failed: %d\n", ret);
		return BPF_DROP;
	}

	if (old_ip == 0x2fea8c0) {
		printk("out: rewriting from %x to %x\n", old_ip, new_ip);
		return rewrite(skb, old_ip, new_ip, 1);
	}

	return BPF_OK;
}

static inline int __do_push_ll_and_redirect(struct __sk_buff *skb)
{
	uint64_t smac = SRC_MAC, dmac = DST_MAC;
	int ret, ifindex = DST_IFINDEX;
	struct ethhdr ehdr;

	ret = bpf_skb_change_head(skb, 14, 0);
	if (ret < 0) {
		printk("skb_change_head() failed: %d\n", ret);
	}

	ehdr.h_proto = __constant_htons(ETH_P_IP);
	memcpy(&ehdr.h_source, &smac, 6);
	memcpy(&ehdr.h_dest, &dmac, 6);

	ret = bpf_skb_store_bytes(skb, 0, &ehdr, sizeof(ehdr), 0);
	if (ret < 0) {
		printk("skb_store_bytes() failed: %d\n", ret);
		return BPF_DROP;
	}

	return bpf_redirect(ifindex, 0);
}

SEC("push_ll_and_redirect_silent")
int do_push_ll_and_redirect_silent(struct __sk_buff *skb)
{
	return __do_push_ll_and_redirect(skb);
}

SEC("push_ll_and_redirect")
int do_push_ll_and_redirect(struct __sk_buff *skb)
{
	int ret, ifindex = DST_IFINDEX;

	ret = __do_push_ll_and_redirect(skb);
	if (ret >= 0)
		printk("redirected to %d\n", ifindex);

	return ret;
}

static inline void __fill_garbage(struct __sk_buff *skb)
{
	uint64_t f = 0xFFFFFFFFFFFFFFFF;

	bpf_skb_store_bytes(skb, 0, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 8, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 16, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 24, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 32, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 40, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 48, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 56, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 64, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 72, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 80, &f, sizeof(f), 0);
	bpf_skb_store_bytes(skb, 88, &f, sizeof(f), 0);
}

SEC("fill_garbage")
int do_fill_garbage(struct __sk_buff *skb)
{
	__fill_garbage(skb);
	printk("Set initial 96 bytes of header to FF\n");
	return BPF_OK;
}

SEC("fill_garbage_and_redirect")
int do_fill_garbage_and_redirect(struct __sk_buff *skb)
{
	int ifindex = DST_IFINDEX;
	__fill_garbage(skb);
	printk("redirected to %d\n", ifindex);
	return bpf_redirect(ifindex, 0);
}

/* Drop all packets */
SEC("drop_all")
int do_drop_all(struct __sk_buff *skb)
{
	printk("dropping with: %d\n", BPF_DROP);
	return BPF_DROP;
}

char _license[] SEC("license") = "GPL";
Commit	Line	Data
f74599f7 TG	1	/* Copyright (c) 2016 Thomas Graf <tgraf@tgraf.ch>
	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	* This program is distributed in the hope that it will be useful, but
	8	* WITHOUT ANY WARRANTY; without even the implied warranty of
	9	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	10	* General Public License for more details.
	11	*/
	12
	13	#include <stdint.h>
	14	#include <stddef.h>
	15	#include <linux/bpf.h>
	16	#include <linux/ip.h>
	17	#include <linux/in.h>
	18	#include <linux/in6.h>
	19	#include <linux/tcp.h>
	20	#include <linux/udp.h>
	21	#include <linux/icmpv6.h>
	22	#include <linux/if_ether.h>
	23	#include "bpf_helpers.h"
	24	#include <string.h>
	25
	26	# define printk(fmt, ...) \
	27	({ \
	28	char ____fmt[] = fmt; \
	29	bpf_trace_printk(____fmt, sizeof(____fmt), \
	30	##__VA_ARGS__); \
	31	})
	32
	33	#define CB_MAGIC 1234
	34
	35	/* Test: Pass all packets through */
	36	SEC("nop")
	37	int do_nop(struct __sk_buff *skb)
	38	{
	39	return BPF_OK;
	40	}
	41
	42	/* Test: Verify context information can be accessed */
	43	SEC("test_ctx")
	44	int do_test_ctx(struct __sk_buff *skb)
	45	{
	46	skb->cb[0] = CB_MAGIC;
	47	printk("len %d hash %d protocol %d\n", skb->len, skb->hash,
	48	skb->protocol);
	49	printk("cb %d ingress_ifindex %d ifindex %d\n", skb->cb[0],
	50	skb->ingress_ifindex, skb->ifindex);
	51
	52	return BPF_OK;
	53	}
	54
	55	/* Test: Ensure skb->cb[] buffer is cleared */
	56	SEC("test_cb")
	57	int do_test_cb(struct __sk_buff *skb)
	58	{
	59	printk("cb0: %x cb1: %x cb2: %x\n", skb->cb[0], skb->cb[1],
	60	skb->cb[2]);
	61	printk("cb3: %x cb4: %x\n", skb->cb[3], skb->cb[4]);
	62
	63	return BPF_OK;
	64	}
65
66	/* Test: Verify skb data can be read */
67	SEC("test_data")
68	int do_test_data(struct __sk_buff *skb)
69	{
70	void data = (void )(long)skb->data;
71	void data_end = (void )(long)skb->data_end;
72	struct iphdr *iph = data;
73
74	if (data + sizeof(*iph) > data_end) {
75	printk("packet truncated\n");
76	return BPF_DROP;
77	}
78
79	printk("src: %x dst: %x\n", iph->saddr, iph->daddr);
80
81	return BPF_OK;
82	}
83
84	#define IP_CSUM_OFF offsetof(struct iphdr, check)
85	#define IP_DST_OFF offsetof(struct iphdr, daddr)
86	#define IP_SRC_OFF offsetof(struct iphdr, saddr)
87	#define IP_PROTO_OFF offsetof(struct iphdr, protocol)
88	#define TCP_CSUM_OFF offsetof(struct tcphdr, check)
89	#define UDP_CSUM_OFF offsetof(struct udphdr, check)
90	#define IS_PSEUDO 0x10
91
92	static inline int rewrite(struct __sk_buff *skb, uint32_t old_ip,
93	uint32_t new_ip, int rw_daddr)
94	{
95	int ret, off = 0, flags = IS_PSEUDO;
96	uint8_t proto;
97
98	ret = bpf_skb_load_bytes(skb, IP_PROTO_OFF, &proto, 1);
99	if (ret < 0) {
100	printk("bpf_l4_csum_replace failed: %d\n", ret);
101	return BPF_DROP;
102	}
103
104	switch (proto) {
105	case IPPROTO_TCP:
106	off = TCP_CSUM_OFF;
107	break;
108
109	case IPPROTO_UDP:
110	off = UDP_CSUM_OFF;
111	flags \|= BPF_F_MARK_MANGLED_0;
112	break;
113
114	case IPPROTO_ICMPV6:
115	off = offsetof(struct icmp6hdr, icmp6_cksum);
116	break;
117	}
118
119	if (off) {
120	ret = bpf_l4_csum_replace(skb, off, old_ip, new_ip,
121	flags \| sizeof(new_ip));
122	if (ret < 0) {
123	printk("bpf_l4_csum_replace failed: %d\n");
124	return BPF_DROP;
125	}
126	}
127
128	ret = bpf_l3_csum_replace(skb, IP_CSUM_OFF, old_ip, new_ip, sizeof(new_ip));
129	if (ret < 0) {
130	printk("bpf_l3_csum_replace failed: %d\n", ret);
131	return BPF_DROP;
132	}
133
134	if (rw_daddr)
135	ret = bpf_skb_store_bytes(skb, IP_DST_OFF, &new_ip, sizeof(new_ip), 0);
136	else
137	ret = bpf_skb_store_bytes(skb, IP_SRC_OFF, &new_ip, sizeof(new_ip), 0);
138
139	if (ret < 0) {
140	printk("bpf_skb_store_bytes() failed: %d\n", ret);
141	return BPF_DROP;
142	}
143
144	return BPF_OK;
145	}
146
147	/* Test: Verify skb data can be modified */
148	SEC("test_rewrite")
149	int do_test_rewrite(struct __sk_buff *skb)
150	{
151	uint32_t old_ip, new_ip = 0x3fea8c0;
152	int ret;
153
154	ret = bpf_skb_load_bytes(skb, IP_DST_OFF, &old_ip, 4);
155	if (ret < 0) {
156	printk("bpf_skb_load_bytes failed: %d\n", ret);
157	return BPF_DROP;
158	}
159
160	if (old_ip == 0x2fea8c0) {
161	printk("out: rewriting from %x to %x\n", old_ip, new_ip);
162	return rewrite(skb, old_ip, new_ip, 1);
163	}
164
165	return BPF_OK;
166	}
167
168	static inline int __do_push_ll_and_redirect(struct __sk_buff *skb)
169	{
170	uint64_t smac = SRC_MAC, dmac = DST_MAC;
171	int ret, ifindex = DST_IFINDEX;
172	struct ethhdr ehdr;
173
174	ret = bpf_skb_change_head(skb, 14, 0);
175	if (ret < 0) {
176	printk("skb_change_head() failed: %d\n", ret);
177	}
178
179	ehdr.h_proto = __constant_htons(ETH_P_IP);
180	memcpy(&ehdr.h_source, &smac, 6);
181	memcpy(&ehdr.h_dest, &dmac, 6);
182
183	ret = bpf_skb_store_bytes(skb, 0, &ehdr, sizeof(ehdr), 0);
184	if (ret < 0) {
185	printk("skb_store_bytes() failed: %d\n", ret);
186	return BPF_DROP;
187	}
188
189	return bpf_redirect(ifindex, 0);
190	}
191
192	SEC("push_ll_and_redirect_silent")
193	int do_push_ll_and_redirect_silent(struct __sk_buff *skb)
194	{
195	return __do_push_ll_and_redirect(skb);
196	}
197
198	SEC("push_ll_and_redirect")
199	int do_push_ll_and_redirect(struct __sk_buff *skb)
200	{
201	int ret, ifindex = DST_IFINDEX;
202
203	ret = __do_push_ll_and_redirect(skb);
204	if (ret >= 0)
205	printk("redirected to %d\n", ifindex);
206
207	return ret;
208	}
209
210	static inline void __fill_garbage(struct __sk_buff *skb)
211	{
212	uint64_t f = 0xFFFFFFFFFFFFFFFF;
213
214	bpf_skb_store_bytes(skb, 0, &f, sizeof(f), 0);
215	bpf_skb_store_bytes(skb, 8, &f, sizeof(f), 0);
216	bpf_skb_store_bytes(skb, 16, &f, sizeof(f), 0);
217	bpf_skb_store_bytes(skb, 24, &f, sizeof(f), 0);
218	bpf_skb_store_bytes(skb, 32, &f, sizeof(f), 0);
219	bpf_skb_store_bytes(skb, 40, &f, sizeof(f), 0);
220	bpf_skb_store_bytes(skb, 48, &f, sizeof(f), 0);
221	bpf_skb_store_bytes(skb, 56, &f, sizeof(f), 0);
222	bpf_skb_store_bytes(skb, 64, &f, sizeof(f), 0);
223	bpf_skb_store_bytes(skb, 72, &f, sizeof(f), 0);
224	bpf_skb_store_bytes(skb, 80, &f, sizeof(f), 0);
225	bpf_skb_store_bytes(skb, 88, &f, sizeof(f), 0);
226	}
227
228	SEC("fill_garbage")
229	int do_fill_garbage(struct __sk_buff *skb)
230	{
231	__fill_garbage(skb);
232	printk("Set initial 96 bytes of header to FF\n");
233	return BPF_OK;
234	}
235
236	SEC("fill_garbage_and_redirect")
237	int do_fill_garbage_and_redirect(struct __sk_buff *skb)
238	{
239	int ifindex = DST_IFINDEX;
240	__fill_garbage(skb);
241	printk("redirected to %d\n", ifindex);
242	return bpf_redirect(ifindex, 0);
243	}
244
245	/* Drop all packets */
246	SEC("drop_all")
247	int do_drop_all(struct __sk_buff *skb)
248	{
249	printk("dropping with: %d\n", BPF_DROP);
250	return BPF_DROP;
251	}
252
253	char _license[] SEC("license") = "GPL";