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 "xdping.h"
19 
20 struct {
21 	__uint(type, BPF_MAP_TYPE_HASH);
22 	__uint(max_entries, 256);
23 	__type(key, __u32);
24 	__type(value, struct pinginfo);
25 } ping_map SEC(".maps");
26 
27 static __always_inline void swap_src_dst_mac(void *data)
28 {
29 	unsigned short *p = data;
30 	unsigned short dst[3];
31 
32 	dst[0] = p[0];
33 	dst[1] = p[1];
34 	dst[2] = p[2];
35 	p[0] = p[3];
36 	p[1] = p[4];
37 	p[2] = p[5];
38 	p[3] = dst[0];
39 	p[4] = dst[1];
40 	p[5] = dst[2];
41 }
42 
43 static __always_inline __u16 csum_fold_helper(__wsum sum)
44 {
45 	sum = (sum & 0xffff) + (sum >> 16);
46 	return ~((sum & 0xffff) + (sum >> 16));
47 }
48 
49 static __always_inline __u16 ipv4_csum(void *data_start, int data_size)
50 {
51 	__wsum sum;
52 
53 	sum = bpf_csum_diff(0, 0, data_start, data_size, 0);
54 	return csum_fold_helper(sum);
55 }
56 
57 #define ICMP_ECHO_LEN		64
58 
59 static __always_inline int icmp_check(struct xdp_md *ctx, int type)
60 {
61 	void *data_end = (void *)(long)ctx->data_end;
62 	void *data = (void *)(long)ctx->data;
63 	struct ethhdr *eth = data;
64 	struct icmphdr *icmph;
65 	struct iphdr *iph;
66 
67 	if (data + sizeof(*eth) + sizeof(*iph) + ICMP_ECHO_LEN > data_end)
68 		return XDP_PASS;
69 
70 	if (eth->h_proto != bpf_htons(ETH_P_IP))
71 		return XDP_PASS;
72 
73 	iph = data + sizeof(*eth);
74 
75 	if (iph->protocol != IPPROTO_ICMP)
76 		return XDP_PASS;
77 
78 	if (bpf_ntohs(iph->tot_len) - sizeof(*iph) != ICMP_ECHO_LEN)
79 		return XDP_PASS;
80 
81 	icmph = data + sizeof(*eth) + sizeof(*iph);
82 
83 	if (icmph->type != type)
84 		return XDP_PASS;
85 
86 	return XDP_TX;
87 }
88 
89 SEC("xdp")
90 int xdping_client(struct xdp_md *ctx)
91 {
92 	void *data_end = (void *)(long)ctx->data_end;
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 clang 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(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_end = (void *)(long)ctx->data_end;
157 	void *data = (void *)(long)ctx->data;
158 	struct ethhdr *eth = data;
159 	struct icmphdr *icmph;
160 	struct iphdr *iph;
161 	__be32 raddr;
162 	int ret;
163 
164 	ret = icmp_check(ctx, ICMP_ECHO);
165 
166 	if (ret != XDP_TX)
167 		return ret;
168 
169 	iph = data + sizeof(*eth);
170 	icmph = data + sizeof(*eth) + sizeof(*iph);
171 	raddr = iph->saddr;
172 
173 	/* Now convert request into echo reply. */
174 	swap_src_dst_mac(data);
175 	iph->saddr = iph->daddr;
176 	iph->daddr = raddr;
177 	icmph->type = ICMP_ECHOREPLY;
178 	icmph->checksum = 0;
179 	icmph->checksum = ipv4_csum(icmph, ICMP_ECHO_LEN);
180 
181 	return XDP_TX;
182 }
183 
184 char _license[] SEC("license") = "GPL";
185