1// SPDX-License-Identifier: GPL-2.0
2#include <stdint.h>
3#include <stdbool.h>
4#include <stddef.h>
5
6#include <linux/bpf.h>
7#include <linux/stddef.h>
8#include <linux/pkt_cls.h>
9#include <linux/if_ether.h>
10#include <linux/in.h>
11#include <linux/ip.h>
12#include <linux/ipv6.h>
13
14#include <bpf/bpf_helpers.h>
15#include <bpf/bpf_endian.h>
16
17#ifndef ctx_ptr
18# define ctx_ptr(field)		(void *)(long)(field)
19#endif
20
21#define AF_INET 2
22#define AF_INET6 10
23
24static __always_inline int fill_fib_params_v4(struct __sk_buff *skb,
25					      struct bpf_fib_lookup *fib_params)
26{
27	void *data_end = ctx_ptr(skb->data_end);
28	void *data = ctx_ptr(skb->data);
29	struct iphdr *ip4h;
30
31	if (data + sizeof(struct ethhdr) > data_end)
32		return -1;
33
34	ip4h = (struct iphdr *)(data + sizeof(struct ethhdr));
35	if ((void *)(ip4h + 1) > data_end)
36		return -1;
37
38	fib_params->family = AF_INET;
39	fib_params->tos = ip4h->tos;
40	fib_params->l4_protocol = ip4h->protocol;
41	fib_params->sport = 0;
42	fib_params->dport = 0;
43	fib_params->tot_len = bpf_ntohs(ip4h->tot_len);
44	fib_params->ipv4_src = ip4h->saddr;
45	fib_params->ipv4_dst = ip4h->daddr;
46
47	return 0;
48}
49
50static __always_inline int fill_fib_params_v6(struct __sk_buff *skb,
51					      struct bpf_fib_lookup *fib_params)
52{
53	struct in6_addr *src = (struct in6_addr *)fib_params->ipv6_src;
54	struct in6_addr *dst = (struct in6_addr *)fib_params->ipv6_dst;
55	void *data_end = ctx_ptr(skb->data_end);
56	void *data = ctx_ptr(skb->data);
57	struct ipv6hdr *ip6h;
58
59	if (data + sizeof(struct ethhdr) > data_end)
60		return -1;
61
62	ip6h = (struct ipv6hdr *)(data + sizeof(struct ethhdr));
63	if ((void *)(ip6h + 1) > data_end)
64		return -1;
65
66	fib_params->family = AF_INET6;
67	fib_params->flowinfo = 0;
68	fib_params->l4_protocol = ip6h->nexthdr;
69	fib_params->sport = 0;
70	fib_params->dport = 0;
71	fib_params->tot_len = bpf_ntohs(ip6h->payload_len);
72	*src = ip6h->saddr;
73	*dst = ip6h->daddr;
74
75	return 0;
76}
77
78SEC("tc")
79int tc_chk(struct __sk_buff *skb)
80{
81	void *data_end = ctx_ptr(skb->data_end);
82	void *data = ctx_ptr(skb->data);
83	__u32 *raw = data;
84
85	if (data + sizeof(struct ethhdr) > data_end)
86		return TC_ACT_SHOT;
87
88	return !raw[0] && !raw[1] && !raw[2] ? TC_ACT_SHOT : TC_ACT_OK;
89}
90
91static __always_inline int tc_redir(struct __sk_buff *skb)
92{
93	struct bpf_fib_lookup fib_params = { .ifindex = skb->ingress_ifindex };
94	__u8 zero[ETH_ALEN * 2];
95	int ret = -1;
96
97	switch (skb->protocol) {
98	case __bpf_constant_htons(ETH_P_IP):
99		ret = fill_fib_params_v4(skb, &fib_params);
100		break;
101	case __bpf_constant_htons(ETH_P_IPV6):
102		ret = fill_fib_params_v6(skb, &fib_params);
103		break;
104	}
105
106	if (ret)
107		return TC_ACT_OK;
108
109	ret = bpf_fib_lookup(skb, &fib_params, sizeof(fib_params), 0);
110	if (ret == BPF_FIB_LKUP_RET_NOT_FWDED || ret < 0)
111		return TC_ACT_OK;
112
113	__builtin_memset(&zero, 0, sizeof(zero));
114	if (bpf_skb_store_bytes(skb, 0, &zero, sizeof(zero), 0) < 0)
115		return TC_ACT_SHOT;
116
117	if (ret == BPF_FIB_LKUP_RET_NO_NEIGH) {
118		struct bpf_redir_neigh nh_params = {};
119
120		nh_params.nh_family = fib_params.family;
121		__builtin_memcpy(&nh_params.ipv6_nh, &fib_params.ipv6_dst,
122				 sizeof(nh_params.ipv6_nh));
123
124		return bpf_redirect_neigh(fib_params.ifindex, &nh_params,
125					  sizeof(nh_params), 0);
126
127	} else if (ret == BPF_FIB_LKUP_RET_SUCCESS) {
128		void *data_end = ctx_ptr(skb->data_end);
129		struct ethhdr *eth = ctx_ptr(skb->data);
130
131		if (eth + 1 > data_end)
132			return TC_ACT_SHOT;
133
134		__builtin_memcpy(eth->h_dest, fib_params.dmac, ETH_ALEN);
135		__builtin_memcpy(eth->h_source, fib_params.smac, ETH_ALEN);
136
137		return bpf_redirect(fib_params.ifindex, 0);
138	}
139
140	return TC_ACT_SHOT;
141}
142
143/* these are identical, but keep them separate for compatibility with the
144 * section names expected by test_tc_redirect.sh
145 */
146SEC("tc")
147int tc_dst(struct __sk_buff *skb)
148{
149	return tc_redir(skb);
150}
151
152SEC("tc")
153int tc_src(struct __sk_buff *skb)
154{
155	return tc_redir(skb);
156}
157
158char __license[] SEC("license") = "GPL";
159