samples/bpf/xdp2_kern.c

0001 /* Copyright (c) 2016 PLUMgrid
0002  *
0003  * This program is free software; you can redistribute it and/or
0004  * modify it under the terms of version 2 of the GNU General Public
0005  * License as published by the Free Software Foundation.
0006  */
0007 #define KBUILD_MODNAME "foo"
0008 #include <uapi/linux/bpf.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 #include <linux/ipv6.h>
0015 #include <bpf/bpf_helpers.h>
0016
0017 struct {
0018     __uint(type, BPF_MAP_TYPE_PERCPU_ARRAY);
0019     __type(key, u32);
0020     __type(value, long);
0021     __uint(max_entries, 256);
0022 } rxcnt SEC(".maps");
0023
0024 static void swap_src_dst_mac(void *data)
0025 {
0026     unsigned short *p = data;
0027     unsigned short dst[3];
0028
0029     dst[0] = p[0];
0030     dst[1] = p[1];
0031     dst[2] = p[2];
0032     p[0] = p[3];
0033     p[1] = p[4];
0034     p[2] = p[5];
0035     p[3] = dst[0];
0036     p[4] = dst[1];
0037     p[5] = dst[2];
0038 }
0039
0040 static int parse_ipv4(void *data, u64 nh_off, void *data_end)
0041 {
0042     struct iphdr *iph = data + nh_off;
0043
0044     if (iph + 1 > data_end)
0045         return 0;
0046     return iph->protocol;
0047 }
0048
0049 static int parse_ipv6(void *data, u64 nh_off, void *data_end)
0050 {
0051     struct ipv6hdr *ip6h = data + nh_off;
0052
0053     if (ip6h + 1 > data_end)
0054         return 0;
0055     return ip6h->nexthdr;
0056 }
0057
0058 #define XDPBUFSIZE  64
0059 SEC("xdp.frags")
0060 int xdp_prog1(struct xdp_md *ctx)
0061 {
0062     __u8 pkt[XDPBUFSIZE] = {};
0063     void *data_end = &pkt[XDPBUFSIZE-1];
0064     void *data = pkt;
0065     struct ethhdr *eth = data;
0066     int rc = XDP_DROP;
0067     long *value;
0068     u16 h_proto;
0069     u64 nh_off;
0070     u32 ipproto;
0071
0072     if (bpf_xdp_load_bytes(ctx, 0, pkt, sizeof(pkt)))
0073         return rc;
0074
0075     nh_off = sizeof(*eth);
0076     if (data + nh_off > data_end)
0077         return rc;
0078
0079     h_proto = eth->h_proto;
0080
0081     /* Handle VLAN tagged packet */
0082     if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
0083         struct vlan_hdr *vhdr;
0084
0085         vhdr = data + nh_off;
0086         nh_off += sizeof(struct vlan_hdr);
0087         if (data + nh_off > data_end)
0088             return rc;
0089         h_proto = vhdr->h_vlan_encapsulated_proto;
0090     }
0091     /* Handle double VLAN tagged packet */
0092     if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
0093         struct vlan_hdr *vhdr;
0094
0095         vhdr = data + nh_off;
0096         nh_off += sizeof(struct vlan_hdr);
0097         if (data + nh_off > data_end)
0098             return rc;
0099         h_proto = vhdr->h_vlan_encapsulated_proto;
0100     }
0101
0102     if (h_proto == htons(ETH_P_IP))
0103         ipproto = parse_ipv4(data, nh_off, data_end);
0104     else if (h_proto == htons(ETH_P_IPV6))
0105         ipproto = parse_ipv6(data, nh_off, data_end);
0106     else
0107         ipproto = 0;
0108
0109     value = bpf_map_lookup_elem(&rxcnt, &ipproto);
0110     if (value)
0111         *value += 1;
0112
0113     if (ipproto == IPPROTO_UDP) {
0114         swap_src_dst_mac(data);
0115         rc = XDP_TX;
0116     }
0117
0118     return rc;
0119 }
0120
0121 char _license[] SEC("license") = "GPL";