samples/bpf/xdp2_kern.c

   1 /* Copyright (c) 2016 PLUMgrid
   2  *
   3  * This program is free software; you can redistribute it and/or
   4  * modify it under the terms of version 2 of the GNU General Public
   5  * License as published by the Free Software Foundation.
   6  */
   7 #define KBUILD_MODNAME "foo"
   8 #include <uapi/linux/bpf.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 #include <linux/ipv6.h>
  15 #include <bpf/bpf_helpers.h>
  16
  17 struct {
  18         __uint(type, BPF_MAP_TYPE_PERCPU_ARRAY);
  19         __type(key, u32);
  20         __type(value, long);
  21         __uint(max_entries, 256);
  22 } rxcnt SEC(".maps");
  23
  24 static void swap_src_dst_mac(void *data)
  25 {
  26         unsigned short *p = data;
  27         unsigned short dst[3];
  28
  29         dst[0] = p[0];
  30         dst[1] = p[1];
  31         dst[2] = p[2];
  32         p[0] = p[3];
  33         p[1] = p[4];
  34         p[2] = p[5];
  35         p[3] = dst[0];
  36         p[4] = dst[1];
  37         p[5] = dst[2];
  38 }
  39
  40 static int parse_ipv4(void *data, u64 nh_off, void *data_end)
  41 {
  42         struct iphdr *iph = data + nh_off;
  43
  44         if (iph + 1 > data_end)
  45                 return 0;
  46         return iph->protocol;
  47 }
  48
  49 static int parse_ipv6(void *data, u64 nh_off, void *data_end)
  50 {
  51         struct ipv6hdr *ip6h = data + nh_off;
  52
  53         if (ip6h + 1 > data_end)
  54                 return 0;
  55         return ip6h->nexthdr;
  56 }
  57
  58 #define XDPBUFSIZE      64
  59 SEC("xdp.frags")
  60 int xdp_prog1(struct xdp_md *ctx)
  61 {
  62         __u8 pkt[XDPBUFSIZE] = {};
  63         void *data_end = &pkt[XDPBUFSIZE-1];
  64         void *data = pkt;
  65         struct ethhdr *eth = data;
  66         int rc = XDP_DROP;
  67         long *value;
  68         u16 h_proto;
  69         u64 nh_off;
  70         u32 ipproto;
  71
  72         if (bpf_xdp_load_bytes(ctx, 0, pkt, sizeof(pkt)))
  73                 return rc;
  74
  75         nh_off = sizeof(*eth);
  76         if (data + nh_off > data_end)
  77                 return rc;
  78
  79         h_proto = eth->h_proto;
  80
  81         /* Handle VLAN tagged packet */
  82         if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
  83                 struct vlan_hdr *vhdr;
  84
  85                 vhdr = data + nh_off;
  86                 nh_off += sizeof(struct vlan_hdr);
  87                 if (data + nh_off > data_end)
  88                         return rc;
  89                 h_proto = vhdr->h_vlan_encapsulated_proto;
  90         }
  91         /* Handle double VLAN tagged packet */
  92         if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
  93                 struct vlan_hdr *vhdr;
  94
  95                 vhdr = data + nh_off;
  96                 nh_off += sizeof(struct vlan_hdr);
  97                 if (data + nh_off > data_end)
  98                         return rc;
  99                 h_proto = vhdr->h_vlan_encapsulated_proto;
 100         }
 101
 102         if (h_proto == htons(ETH_P_IP))
 103                 ipproto = parse_ipv4(data, nh_off, data_end);
 104         else if (h_proto == htons(ETH_P_IPV6))
 105                 ipproto = parse_ipv6(data, nh_off, data_end);
 106         else
 107                 ipproto = 0;
 108
 109         value = bpf_map_lookup_elem(&rxcnt, &ipproto);
 110         if (value)
 111                 *value += 1;
 112
 113         if (ipproto == IPPROTO_UDP) {
 114                 swap_src_dst_mac(data);
 115                 rc = XDP_TX;
 116         }
 117
 118         return rc;
 119 }
 120
 121 char _license[] SEC("license") = "GPL";