1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (c) 2019, Oracle and/or its affiliates. All rights reserved. */ |
3 | |
4 | #define KBUILD_MODNAME "foo" |
5 | #include <stddef.h> |
6 | #include <string.h> |
7 | #include <linux/bpf.h> |
8 | #include <linux/icmp.h> |
9 | #include <linux/in.h> |
10 | #include <linux/if_ether.h> |
11 | #include <linux/if_packet.h> |
12 | #include <linux/if_vlan.h> |
13 | #include <linux/ip.h> |
14 | |
15 | #include <bpf/bpf_helpers.h> |
16 | #include <bpf/bpf_endian.h> |
17 | |
18 | #include "bpf_compiler.h" |
19 | #include "xdping.h" |
20 | |
21 | struct { |
22 | __uint(type, BPF_MAP_TYPE_HASH); |
23 | __uint(max_entries, 256); |
24 | __type(key, __u32); |
25 | __type(value, struct pinginfo); |
26 | } ping_map SEC(".maps" ); |
27 | |
28 | static __always_inline void swap_src_dst_mac(void *data) |
29 | { |
30 | unsigned short *p = data; |
31 | unsigned short dst[3]; |
32 | |
33 | dst[0] = p[0]; |
34 | dst[1] = p[1]; |
35 | dst[2] = p[2]; |
36 | p[0] = p[3]; |
37 | p[1] = p[4]; |
38 | p[2] = p[5]; |
39 | p[3] = dst[0]; |
40 | p[4] = dst[1]; |
41 | p[5] = dst[2]; |
42 | } |
43 | |
44 | static __always_inline __u16 csum_fold_helper(__wsum sum) |
45 | { |
46 | sum = (sum & 0xffff) + (sum >> 16); |
47 | return ~((sum & 0xffff) + (sum >> 16)); |
48 | } |
49 | |
50 | static __always_inline __u16 ipv4_csum(void *data_start, int data_size) |
51 | { |
52 | __wsum sum; |
53 | |
54 | sum = bpf_csum_diff(0, 0, data_start, data_size, 0); |
55 | return csum_fold_helper(sum); |
56 | } |
57 | |
58 | #define ICMP_ECHO_LEN 64 |
59 | |
60 | static __always_inline int icmp_check(struct xdp_md *ctx, int type) |
61 | { |
62 | void *data_end = (void *)(long)ctx->data_end; |
63 | void *data = (void *)(long)ctx->data; |
64 | struct ethhdr *eth = data; |
65 | struct icmphdr *icmph; |
66 | struct iphdr *iph; |
67 | |
68 | if (data + sizeof(*eth) + sizeof(*iph) + ICMP_ECHO_LEN > data_end) |
69 | return XDP_PASS; |
70 | |
71 | if (eth->h_proto != bpf_htons(ETH_P_IP)) |
72 | return XDP_PASS; |
73 | |
74 | iph = data + sizeof(*eth); |
75 | |
76 | if (iph->protocol != IPPROTO_ICMP) |
77 | return XDP_PASS; |
78 | |
79 | if (bpf_ntohs(iph->tot_len) - sizeof(*iph) != ICMP_ECHO_LEN) |
80 | return XDP_PASS; |
81 | |
82 | icmph = data + sizeof(*eth) + sizeof(*iph); |
83 | |
84 | if (icmph->type != type) |
85 | return XDP_PASS; |
86 | |
87 | return XDP_TX; |
88 | } |
89 | |
90 | SEC("xdp" ) |
91 | int xdping_client(struct xdp_md *ctx) |
92 | { |
93 | void *data = (void *)(long)ctx->data; |
94 | struct pinginfo *pinginfo = NULL; |
95 | struct ethhdr *eth = data; |
96 | struct icmphdr *icmph; |
97 | struct iphdr *iph; |
98 | __u64 recvtime; |
99 | __be32 raddr; |
100 | __be16 seq; |
101 | int ret; |
102 | __u8 i; |
103 | |
104 | ret = icmp_check(ctx, ICMP_ECHOREPLY); |
105 | |
106 | if (ret != XDP_TX) |
107 | return ret; |
108 | |
109 | iph = data + sizeof(*eth); |
110 | icmph = data + sizeof(*eth) + sizeof(*iph); |
111 | raddr = iph->saddr; |
112 | |
113 | /* Record time reply received. */ |
114 | recvtime = bpf_ktime_get_ns(); |
115 | pinginfo = bpf_map_lookup_elem(&ping_map, &raddr); |
116 | if (!pinginfo || pinginfo->seq != icmph->un.echo.sequence) |
117 | return XDP_PASS; |
118 | |
119 | if (pinginfo->start) { |
120 | __pragma_loop_unroll_full |
121 | for (i = 0; i < XDPING_MAX_COUNT; i++) { |
122 | if (pinginfo->times[i] == 0) |
123 | break; |
124 | } |
125 | /* verifier is fussy here... */ |
126 | if (i < XDPING_MAX_COUNT) { |
127 | pinginfo->times[i] = recvtime - |
128 | pinginfo->start; |
129 | pinginfo->start = 0; |
130 | i++; |
131 | } |
132 | /* No more space for values? */ |
133 | if (i == pinginfo->count || i == XDPING_MAX_COUNT) |
134 | return XDP_PASS; |
135 | } |
136 | |
137 | /* Now convert reply back into echo request. */ |
138 | swap_src_dst_mac(data); |
139 | iph->saddr = iph->daddr; |
140 | iph->daddr = raddr; |
141 | icmph->type = ICMP_ECHO; |
142 | seq = bpf_htons(bpf_ntohs(icmph->un.echo.sequence) + 1); |
143 | icmph->un.echo.sequence = seq; |
144 | icmph->checksum = 0; |
145 | icmph->checksum = ipv4_csum(data_start: icmph, ICMP_ECHO_LEN); |
146 | |
147 | pinginfo->seq = seq; |
148 | pinginfo->start = bpf_ktime_get_ns(); |
149 | |
150 | return XDP_TX; |
151 | } |
152 | |
153 | SEC("xdp" ) |
154 | int xdping_server(struct xdp_md *ctx) |
155 | { |
156 | void *data = (void *)(long)ctx->data; |
157 | struct ethhdr *eth = data; |
158 | struct icmphdr *icmph; |
159 | struct iphdr *iph; |
160 | __be32 raddr; |
161 | int ret; |
162 | |
163 | ret = icmp_check(ctx, ICMP_ECHO); |
164 | |
165 | if (ret != XDP_TX) |
166 | return ret; |
167 | |
168 | iph = data + sizeof(*eth); |
169 | icmph = data + sizeof(*eth) + sizeof(*iph); |
170 | raddr = iph->saddr; |
171 | |
172 | /* Now convert request into echo reply. */ |
173 | swap_src_dst_mac(data); |
174 | iph->saddr = iph->daddr; |
175 | iph->daddr = raddr; |
176 | icmph->type = ICMP_ECHOREPLY; |
177 | icmph->checksum = 0; |
178 | icmph->checksum = ipv4_csum(data_start: icmph, ICMP_ECHO_LEN); |
179 | |
180 | return XDP_TX; |
181 | } |
182 | |
183 | char _license[] SEC("license" ) = "GPL" ; |
184 | |