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
 17struct {
 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
 24static int parse_ipv4(void *data, u64 nh_off, void *data_end)
 25{
 26	struct iphdr *iph = data + nh_off;
 27
 28	if (iph + 1 > data_end)
 29		return 0;
 30	return iph->protocol;
 31}
 32
 33static int parse_ipv6(void *data, u64 nh_off, void *data_end)
 34{
 35	struct ipv6hdr *ip6h = data + nh_off;
 36
 37	if (ip6h + 1 > data_end)
 38		return 0;
 39	return ip6h->nexthdr;
 40}
 41
 42#define XDPBUFSIZE	64
 43SEC("xdp.frags")
 44int xdp_prog1(struct xdp_md *ctx)
 45{
 46	__u8 pkt[XDPBUFSIZE] = {};
 47	void *data_end = &pkt[XDPBUFSIZE-1];
 48	void *data = pkt;
 49	struct ethhdr *eth = data;
 50	int rc = XDP_DROP;
 51	long *value;
 52	u16 h_proto;
 53	u64 nh_off;
 54	u32 ipproto;
 55
 56	if (bpf_xdp_load_bytes(ctx, 0, pkt, sizeof(pkt)))
 57		return rc;
 58
 59	nh_off = sizeof(*eth);
 60	if (data + nh_off > data_end)
 61		return rc;
 62
 63	h_proto = eth->h_proto;
 64
 65	/* Handle VLAN tagged packet */
 66	if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
 67		struct vlan_hdr *vhdr;
 68
 69		vhdr = data + nh_off;
 70		nh_off += sizeof(struct vlan_hdr);
 71		if (data + nh_off > data_end)
 72			return rc;
 73		h_proto = vhdr->h_vlan_encapsulated_proto;
 74	}
 75	/* Handle double VLAN tagged packet */
 76	if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
 77		struct vlan_hdr *vhdr;
 78
 79		vhdr = data + nh_off;
 80		nh_off += sizeof(struct vlan_hdr);
 81		if (data + nh_off > data_end)
 82			return rc;
 83		h_proto = vhdr->h_vlan_encapsulated_proto;
 84	}
 85
 86	if (h_proto == htons(ETH_P_IP))
 87		ipproto = parse_ipv4(data, nh_off, data_end);
 88	else if (h_proto == htons(ETH_P_IPV6))
 89		ipproto = parse_ipv6(data, nh_off, data_end);
 90	else
 91		ipproto = 0;
 92
 93	value = bpf_map_lookup_elem(&rxcnt, &ipproto);
 94	if (value)
 95		*value += 1;
 96
 97	return rc;
 98}
 99
100char _license[] SEC("license") = "GPL";