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
 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";