linux/tools/testing/selftests/bpf/progs/test_xdp_loop.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0
   2// Copyright (c) 2019 Facebook
   3#include <stddef.h>
   4#include <string.h>
   5#include <linux/bpf.h>
   6#include <linux/if_ether.h>
   7#include <linux/if_packet.h>
   8#include <linux/ip.h>
   9#include <linux/ipv6.h>
  10#include <linux/in.h>
  11#include <linux/udp.h>
  12#include <linux/tcp.h>
  13#include <linux/pkt_cls.h>
  14#include <sys/socket.h>
  15#include <bpf/bpf_helpers.h>
  16#include <bpf/bpf_endian.h>
  17#include "test_iptunnel_common.h"
  18
  19int _version SEC("version") = 1;
  20
  21struct {
  22        __uint(type, BPF_MAP_TYPE_PERCPU_ARRAY);
  23        __uint(max_entries, 256);
  24        __type(key, __u32);
  25        __type(value, __u64);
  26} rxcnt SEC(".maps");
  27
  28struct {
  29        __uint(type, BPF_MAP_TYPE_HASH);
  30        __uint(max_entries, MAX_IPTNL_ENTRIES);
  31        __type(key, struct vip);
  32        __type(value, struct iptnl_info);
  33} vip2tnl SEC(".maps");
  34
  35static __always_inline void count_tx(__u32 protocol)
  36{
  37        __u64 *rxcnt_count;
  38
  39        rxcnt_count = bpf_map_lookup_elem(&rxcnt, &protocol);
  40        if (rxcnt_count)
  41                *rxcnt_count += 1;
  42}
  43
  44static __always_inline int get_dport(void *trans_data, void *data_end,
  45                                     __u8 protocol)
  46{
  47        struct tcphdr *th;
  48        struct udphdr *uh;
  49
  50        switch (protocol) {
  51        case IPPROTO_TCP:
  52                th = (struct tcphdr *)trans_data;
  53                if (th + 1 > data_end)
  54                        return -1;
  55                return th->dest;
  56        case IPPROTO_UDP:
  57                uh = (struct udphdr *)trans_data;
  58                if (uh + 1 > data_end)
  59                        return -1;
  60                return uh->dest;
  61        default:
  62                return 0;
  63        }
  64}
  65
  66static __always_inline void set_ethhdr(struct ethhdr *new_eth,
  67                                       const struct ethhdr *old_eth,
  68                                       const struct iptnl_info *tnl,
  69                                       __be16 h_proto)
  70{
  71        memcpy(new_eth->h_source, old_eth->h_dest, sizeof(new_eth->h_source));
  72        memcpy(new_eth->h_dest, tnl->dmac, sizeof(new_eth->h_dest));
  73        new_eth->h_proto = h_proto;
  74}
  75
  76static __always_inline int handle_ipv4(struct xdp_md *xdp)
  77{
  78        void *data_end = (void *)(long)xdp->data_end;
  79        void *data = (void *)(long)xdp->data;
  80        struct iptnl_info *tnl;
  81        struct ethhdr *new_eth;
  82        struct ethhdr *old_eth;
  83        struct iphdr *iph = data + sizeof(struct ethhdr);
  84        __u16 *next_iph;
  85        __u16 payload_len;
  86        struct vip vip = {};
  87        int dport;
  88        __u32 csum = 0;
  89        int i;
  90
  91        if (iph + 1 > data_end)
  92                return XDP_DROP;
  93
  94        dport = get_dport(iph + 1, data_end, iph->protocol);
  95        if (dport == -1)
  96                return XDP_DROP;
  97
  98        vip.protocol = iph->protocol;
  99        vip.family = AF_INET;
 100        vip.daddr.v4 = iph->daddr;
 101        vip.dport = dport;
 102        payload_len = bpf_ntohs(iph->tot_len);
 103
 104        tnl = bpf_map_lookup_elem(&vip2tnl, &vip);
 105        /* It only does v4-in-v4 */
 106        if (!tnl || tnl->family != AF_INET)
 107                return XDP_PASS;
 108
 109        if (bpf_xdp_adjust_head(xdp, 0 - (int)sizeof(struct iphdr)))
 110                return XDP_DROP;
 111
 112        data = (void *)(long)xdp->data;
 113        data_end = (void *)(long)xdp->data_end;
 114
 115        new_eth = data;
 116        iph = data + sizeof(*new_eth);
 117        old_eth = data + sizeof(*iph);
 118
 119        if (new_eth + 1 > data_end ||
 120            old_eth + 1 > data_end ||
 121            iph + 1 > data_end)
 122                return XDP_DROP;
 123
 124        set_ethhdr(new_eth, old_eth, tnl, bpf_htons(ETH_P_IP));
 125
 126        iph->version = 4;
 127        iph->ihl = sizeof(*iph) >> 2;
 128        iph->frag_off = 0;
 129        iph->protocol = IPPROTO_IPIP;
 130        iph->check = 0;
 131        iph->tos = 0;
 132        iph->tot_len = bpf_htons(payload_len + sizeof(*iph));
 133        iph->daddr = tnl->daddr.v4;
 134        iph->saddr = tnl->saddr.v4;
 135        iph->ttl = 8;
 136
 137        next_iph = (__u16 *)iph;
 138#pragma clang loop unroll(disable)
 139        for (i = 0; i < sizeof(*iph) >> 1; i++)
 140                csum += *next_iph++;
 141
 142        iph->check = ~((csum & 0xffff) + (csum >> 16));
 143
 144        count_tx(vip.protocol);
 145
 146        return XDP_TX;
 147}
 148
 149static __always_inline int handle_ipv6(struct xdp_md *xdp)
 150{
 151        void *data_end = (void *)(long)xdp->data_end;
 152        void *data = (void *)(long)xdp->data;
 153        struct iptnl_info *tnl;
 154        struct ethhdr *new_eth;
 155        struct ethhdr *old_eth;
 156        struct ipv6hdr *ip6h = data + sizeof(struct ethhdr);
 157        __u16 payload_len;
 158        struct vip vip = {};
 159        int dport;
 160
 161        if (ip6h + 1 > data_end)
 162                return XDP_DROP;
 163
 164        dport = get_dport(ip6h + 1, data_end, ip6h->nexthdr);
 165        if (dport == -1)
 166                return XDP_DROP;
 167
 168        vip.protocol = ip6h->nexthdr;
 169        vip.family = AF_INET6;
 170        memcpy(vip.daddr.v6, ip6h->daddr.s6_addr32, sizeof(vip.daddr));
 171        vip.dport = dport;
 172        payload_len = ip6h->payload_len;
 173
 174        tnl = bpf_map_lookup_elem(&vip2tnl, &vip);
 175        /* It only does v6-in-v6 */
 176        if (!tnl || tnl->family != AF_INET6)
 177                return XDP_PASS;
 178
 179        if (bpf_xdp_adjust_head(xdp, 0 - (int)sizeof(struct ipv6hdr)))
 180                return XDP_DROP;
 181
 182        data = (void *)(long)xdp->data;
 183        data_end = (void *)(long)xdp->data_end;
 184
 185        new_eth = data;
 186        ip6h = data + sizeof(*new_eth);
 187        old_eth = data + sizeof(*ip6h);
 188
 189        if (new_eth + 1 > data_end || old_eth + 1 > data_end ||
 190            ip6h + 1 > data_end)
 191                return XDP_DROP;
 192
 193        set_ethhdr(new_eth, old_eth, tnl, bpf_htons(ETH_P_IPV6));
 194
 195        ip6h->version = 6;
 196        ip6h->priority = 0;
 197        memset(ip6h->flow_lbl, 0, sizeof(ip6h->flow_lbl));
 198        ip6h->payload_len = bpf_htons(bpf_ntohs(payload_len) + sizeof(*ip6h));
 199        ip6h->nexthdr = IPPROTO_IPV6;
 200        ip6h->hop_limit = 8;
 201        memcpy(ip6h->saddr.s6_addr32, tnl->saddr.v6, sizeof(tnl->saddr.v6));
 202        memcpy(ip6h->daddr.s6_addr32, tnl->daddr.v6, sizeof(tnl->daddr.v6));
 203
 204        count_tx(vip.protocol);
 205
 206        return XDP_TX;
 207}
 208
 209SEC("xdp_tx_iptunnel")
 210int _xdp_tx_iptunnel(struct xdp_md *xdp)
 211{
 212        void *data_end = (void *)(long)xdp->data_end;
 213        void *data = (void *)(long)xdp->data;
 214        struct ethhdr *eth = data;
 215        __u16 h_proto;
 216
 217        if (eth + 1 > data_end)
 218                return XDP_DROP;
 219
 220        h_proto = eth->h_proto;
 221
 222        if (h_proto == bpf_htons(ETH_P_IP))
 223                return handle_ipv4(xdp);
 224        else if (h_proto == bpf_htons(ETH_P_IPV6))
 225
 226                return handle_ipv6(xdp);
 227        else
 228                return XDP_DROP;
 229}
 230
 231char _license[] SEC("license") = "GPL";
 232