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
42SEC("xdp1")
43int xdp_prog1(struct xdp_md *ctx)
44{
45	void *data_end = (void *)(long)ctx->data_end;
46	void *data = (void *)(long)ctx->data;
47	struct ethhdr *eth = data;
48	int rc = XDP_DROP;
49	long *value;
50	u16 h_proto;
51	u64 nh_off;
52	u32 ipproto;
53
54	nh_off = sizeof(*eth);
55	if (data + nh_off > data_end)
56		return rc;
57
58	h_proto = eth->h_proto;
59
60	if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
61		struct vlan_hdr *vhdr;
62
63		vhdr = data + nh_off;
64		nh_off += sizeof(struct vlan_hdr);
65		if (data + nh_off > data_end)
66			return rc;
67		h_proto = vhdr->h_vlan_encapsulated_proto;
68	}
69	if (h_proto == htons(ETH_P_8021Q) || h_proto == htons(ETH_P_8021AD)) {
70		struct vlan_hdr *vhdr;
71
72		vhdr = data + nh_off;
73		nh_off += sizeof(struct vlan_hdr);
74		if (data + nh_off > data_end)
75			return rc;
76		h_proto = vhdr->h_vlan_encapsulated_proto;
77	}
78
79	if (h_proto == htons(ETH_P_IP))
80		ipproto = parse_ipv4(data, nh_off, data_end);
81	else if (h_proto == htons(ETH_P_IPV6))
82		ipproto = parse_ipv6(data, nh_off, data_end);
83	else
84		ipproto = 0;
85
86	value = bpf_map_lookup_elem(&rxcnt, &ipproto);
87	if (value)
88		*value += 1;
89
90	return rc;
91}
92
93char _license[] SEC("license") = "GPL";
94