1105699Srwatson// SPDX-License-Identifier: GPL-2.0
2105699Srwatson/* Copyright (c) 2019, Oracle and/or its affiliates. All rights reserved. */
3105699Srwatson
4105699Srwatson#define KBUILD_MODNAME "foo"
5105699Srwatson#include <stddef.h>
6229783Suqs#include <string.h>
7105699Srwatson#include <linux/bpf.h>
8105699Srwatson#include <linux/icmp.h>
9105699Srwatson#include <linux/in.h>
10105699Srwatson#include <linux/if_ether.h>
11105699Srwatson#include <linux/if_packet.h>
12105699Srwatson#include <linux/if_vlan.h>
13105699Srwatson#include <linux/ip.h>
14122453Srwatson
15122453Srwatson#include <bpf/bpf_helpers.h>
16122453Srwatson#include <bpf/bpf_endian.h>
17122726Srwatson
18105699Srwatson#include "bpf_compiler.h"
19#include "xdping.h"
20
21struct {
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
28static __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
44static __always_inline __u16 csum_fold_helper(__wsum sum)
45{
46	sum = (sum & 0xffff) + (sum >> 16);
47	return ~((sum & 0xffff) + (sum >> 16));
48}
49
50static __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
60static __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
90SEC("xdp")
91int 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(icmph, ICMP_ECHO_LEN);
146
147	pinginfo->seq = seq;
148	pinginfo->start = bpf_ktime_get_ns();
149
150	return XDP_TX;
151}
152
153SEC("xdp")
154int 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(icmph, ICMP_ECHO_LEN);
179
180	return XDP_TX;
181}
182
183char _license[] SEC("license") = "GPL";
184