LXR linux/tools/testing/selftests/bpf/progs/xdping

   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_helpers.h"
  16#include "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