bpf/progs/xdping_kern.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /* Copyright (c) 2019, Oracle and/or its affiliates. All rights reserved. */
0003
0004 #define KBUILD_MODNAME "foo"
0005 #include <stddef.h>
0006 #include <string.h>
0007 #include <linux/bpf.h>
0008 #include <linux/icmp.h>
0009 #include <linux/in.h>
0010 #include <linux/if_ether.h>
0011 #include <linux/if_packet.h>
0012 #include <linux/if_vlan.h>
0013 #include <linux/ip.h>
0014
0015 #include <bpf/bpf_helpers.h>
0016 #include <bpf/bpf_endian.h>
0017
0018 #include "xdping.h"
0019
0020 struct {
0021     __uint(type, BPF_MAP_TYPE_HASH);
0022     __uint(max_entries, 256);
0023     __type(key, __u32);
0024     __type(value, struct pinginfo);
0025 } ping_map SEC(".maps");
0026
0027 static __always_inline void swap_src_dst_mac(void *data)
0028 {
0029     unsigned short *p = data;
0030     unsigned short dst[3];
0031
0032     dst[0] = p[0];
0033     dst[1] = p[1];
0034     dst[2] = p[2];
0035     p[0] = p[3];
0036     p[1] = p[4];
0037     p[2] = p[5];
0038     p[3] = dst[0];
0039     p[4] = dst[1];
0040     p[5] = dst[2];
0041 }
0042
0043 static __always_inline __u16 csum_fold_helper(__wsum sum)
0044 {
0045     sum = (sum & 0xffff) + (sum >> 16);
0046     return ~((sum & 0xffff) + (sum >> 16));
0047 }
0048
0049 static __always_inline __u16 ipv4_csum(void *data_start, int data_size)
0050 {
0051     __wsum sum;
0052
0053     sum = bpf_csum_diff(0, 0, data_start, data_size, 0);
0054     return csum_fold_helper(sum);
0055 }
0056
0057 #define ICMP_ECHO_LEN       64
0058
0059 static __always_inline int icmp_check(struct xdp_md *ctx, int type)
0060 {
0061     void *data_end = (void *)(long)ctx->data_end;
0062     void *data = (void *)(long)ctx->data;
0063     struct ethhdr *eth = data;
0064     struct icmphdr *icmph;
0065     struct iphdr *iph;
0066
0067     if (data + sizeof(*eth) + sizeof(*iph) + ICMP_ECHO_LEN > data_end)
0068         return XDP_PASS;
0069
0070     if (eth->h_proto != bpf_htons(ETH_P_IP))
0071         return XDP_PASS;
0072
0073     iph = data + sizeof(*eth);
0074
0075     if (iph->protocol != IPPROTO_ICMP)
0076         return XDP_PASS;
0077
0078     if (bpf_ntohs(iph->tot_len) - sizeof(*iph) != ICMP_ECHO_LEN)
0079         return XDP_PASS;
0080
0081     icmph = data + sizeof(*eth) + sizeof(*iph);
0082
0083     if (icmph->type != type)
0084         return XDP_PASS;
0085
0086     return XDP_TX;
0087 }
0088
0089 SEC("xdp")
0090 int xdping_client(struct xdp_md *ctx)
0091 {
0092     void *data_end = (void *)(long)ctx->data_end;
0093     void *data = (void *)(long)ctx->data;
0094     struct pinginfo *pinginfo = NULL;
0095     struct ethhdr *eth = data;
0096     struct icmphdr *icmph;
0097     struct iphdr *iph;
0098     __u64 recvtime;
0099     __be32 raddr;
0100     __be16 seq;
0101     int ret;
0102     __u8 i;
0103
0104     ret = icmp_check(ctx, ICMP_ECHOREPLY);
0105
0106     if (ret != XDP_TX)
0107         return ret;
0108
0109     iph = data + sizeof(*eth);
0110     icmph = data + sizeof(*eth) + sizeof(*iph);
0111     raddr = iph->saddr;
0112
0113     /* Record time reply received. */
0114     recvtime = bpf_ktime_get_ns();
0115     pinginfo = bpf_map_lookup_elem(&ping_map, &raddr);
0116     if (!pinginfo || pinginfo->seq != icmph->un.echo.sequence)
0117         return XDP_PASS;
0118
0119     if (pinginfo->start) {
0120 #pragma clang loop unroll(full)
0121         for (i = 0; i < XDPING_MAX_COUNT; i++) {
0122             if (pinginfo->times[i] == 0)
0123                 break;
0124         }
0125         /* verifier is fussy here... */
0126         if (i < XDPING_MAX_COUNT) {
0127             pinginfo->times[i] = recvtime -
0128                          pinginfo->start;
0129             pinginfo->start = 0;
0130             i++;
0131         }
0132         /* No more space for values? */
0133         if (i == pinginfo->count || i == XDPING_MAX_COUNT)
0134             return XDP_PASS;
0135     }
0136
0137     /* Now convert reply back into echo request. */
0138     swap_src_dst_mac(data);
0139     iph->saddr = iph->daddr;
0140     iph->daddr = raddr;
0141     icmph->type = ICMP_ECHO;
0142     seq = bpf_htons(bpf_ntohs(icmph->un.echo.sequence) + 1);
0143     icmph->un.echo.sequence = seq;
0144     icmph->checksum = 0;
0145     icmph->checksum = ipv4_csum(icmph, ICMP_ECHO_LEN);
0146
0147     pinginfo->seq = seq;
0148     pinginfo->start = bpf_ktime_get_ns();
0149
0150     return XDP_TX;
0151 }
0152
0153 SEC("xdp")
0154 int xdping_server(struct xdp_md *ctx)
0155 {
0156     void *data_end = (void *)(long)ctx->data_end;
0157     void *data = (void *)(long)ctx->data;
0158     struct ethhdr *eth = data;
0159     struct icmphdr *icmph;
0160     struct iphdr *iph;
0161     __be32 raddr;
0162     int ret;
0163
0164     ret = icmp_check(ctx, ICMP_ECHO);
0165
0166     if (ret != XDP_TX)
0167         return ret;
0168
0169     iph = data + sizeof(*eth);
0170     icmph = data + sizeof(*eth) + sizeof(*iph);
0171     raddr = iph->saddr;
0172
0173     /* Now convert request into echo reply. */
0174     swap_src_dst_mac(data);
0175     iph->saddr = iph->daddr;
0176     iph->daddr = raddr;
0177     icmph->type = ICMP_ECHOREPLY;
0178     icmph->checksum = 0;
0179     icmph->checksum = ipv4_csum(icmph, ICMP_ECHO_LEN);
0180
0181     return XDP_TX;
0182 }
0183
0184 char _license[] SEC("license") = "GPL";