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
20struct {
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
27static __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
43static __always_inline __u16 csum_fold_helper(__wsum sum)
44{
45	sum = (sum & 0xffff) + (sum >> 16);
46	return ~((sum & 0xffff) + (sum >> 16));
47}
48
49static __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
59static __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
89SEC("xdpclient")
90int 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
153SEC("xdpserver")
154int 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
184char _license[] SEC("license") = "GPL";
185