1 // SPDX-License-Identifier: GPL-2.0
2 #include <stddef.h>
3 #include <stdint.h>
4 #include <stdbool.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 ip4_src			0xac100164 /* 172.16.1.100 */
22 #define ip4_dst			0xac100264 /* 172.16.2.100 */
23 
24 #define ip6_src			{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, \
25 				  0x00, 0x01, 0xde, 0xad, 0xbe, 0xef, 0xca, 0xfe }
26 #define ip6_dst			{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, \
27 				  0x00, 0x02, 0xde, 0xad, 0xbe, 0xef, 0xca, 0xfe }
28 
29 #ifndef v6_equal
30 # define v6_equal(a, b)		(a.s6_addr32[0] == b.s6_addr32[0] && \
31 				 a.s6_addr32[1] == b.s6_addr32[1] && \
32 				 a.s6_addr32[2] == b.s6_addr32[2] && \
33 				 a.s6_addr32[3] == b.s6_addr32[3])
34 #endif
35 
36 volatile const __u32 IFINDEX_SRC;
37 volatile const __u32 IFINDEX_DST;
38 
39 static __always_inline bool is_remote_ep_v4(struct __sk_buff *skb,
40 					    __be32 addr)
41 {
42 	void *data_end = ctx_ptr(skb->data_end);
43 	void *data = ctx_ptr(skb->data);
44 	struct iphdr *ip4h;
45 
46 	if (data + sizeof(struct ethhdr) > data_end)
47 		return false;
48 
49 	ip4h = (struct iphdr *)(data + sizeof(struct ethhdr));
50 	if ((void *)(ip4h + 1) > data_end)
51 		return false;
52 
53 	return ip4h->daddr == addr;
54 }
55 
56 static __always_inline bool is_remote_ep_v6(struct __sk_buff *skb,
57 					    struct in6_addr addr)
58 {
59 	void *data_end = ctx_ptr(skb->data_end);
60 	void *data = ctx_ptr(skb->data);
61 	struct ipv6hdr *ip6h;
62 
63 	if (data + sizeof(struct ethhdr) > data_end)
64 		return false;
65 
66 	ip6h = (struct ipv6hdr *)(data + sizeof(struct ethhdr));
67 	if ((void *)(ip6h + 1) > data_end)
68 		return false;
69 
70 	return v6_equal(ip6h->daddr, addr);
71 }
72 
73 SEC("tc")
74 int tc_chk(struct __sk_buff *skb)
75 {
76 	void *data_end = ctx_ptr(skb->data_end);
77 	void *data = ctx_ptr(skb->data);
78 	__u32 *raw = data;
79 
80 	if (data + sizeof(struct ethhdr) > data_end)
81 		return TC_ACT_SHOT;
82 
83 	return !raw[0] && !raw[1] && !raw[2] ? TC_ACT_SHOT : TC_ACT_OK;
84 }
85 
86 SEC("tc")
87 int tc_dst(struct __sk_buff *skb)
88 {
89 	__u8 zero[ETH_ALEN * 2];
90 	bool redirect = false;
91 
92 	switch (skb->protocol) {
93 	case __bpf_constant_htons(ETH_P_IP):
94 		redirect = is_remote_ep_v4(skb, __bpf_constant_htonl(ip4_src));
95 		break;
96 	case __bpf_constant_htons(ETH_P_IPV6):
97 		redirect = is_remote_ep_v6(skb, (struct in6_addr)ip6_src);
98 		break;
99 	}
100 
101 	if (!redirect)
102 		return TC_ACT_OK;
103 
104 	__builtin_memset(&zero, 0, sizeof(zero));
105 	if (bpf_skb_store_bytes(skb, 0, &zero, sizeof(zero), 0) < 0)
106 		return TC_ACT_SHOT;
107 
108 	return bpf_redirect_neigh(IFINDEX_SRC, NULL, 0, 0);
109 }
110 
111 SEC("tc")
112 int tc_src(struct __sk_buff *skb)
113 {
114 	__u8 zero[ETH_ALEN * 2];
115 	bool redirect = false;
116 
117 	switch (skb->protocol) {
118 	case __bpf_constant_htons(ETH_P_IP):
119 		redirect = is_remote_ep_v4(skb, __bpf_constant_htonl(ip4_dst));
120 		break;
121 	case __bpf_constant_htons(ETH_P_IPV6):
122 		redirect = is_remote_ep_v6(skb, (struct in6_addr)ip6_dst);
123 		break;
124 	}
125 
126 	if (!redirect)
127 		return TC_ACT_OK;
128 
129 	__builtin_memset(&zero, 0, sizeof(zero));
130 	if (bpf_skb_store_bytes(skb, 0, &zero, sizeof(zero), 0) < 0)
131 		return TC_ACT_SHOT;
132 
133 	return bpf_redirect_neigh(IFINDEX_DST, NULL, 0, 0);
134 }
135 
136 char __license[] SEC("license") = "GPL";
137