samples/bpf/sockex3_kern.c

   1 /* Copyright (c) 2015 PLUMgrid, http://plumgrid.com
   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 #include <uapi/linux/bpf.h>
   8 #include <uapi/linux/in.h>
   9 #include <uapi/linux/if.h>
  10 #include <uapi/linux/if_ether.h>
  11 #include <uapi/linux/ip.h>
  12 #include <uapi/linux/ipv6.h>
  13 #include <uapi/linux/if_tunnel.h>
  14 #include <uapi/linux/mpls.h>
  15 #include <bpf/bpf_helpers.h>
  16 #include "bpf_legacy.h"
  17 #define IP_MF           0x2000
  18 #define IP_OFFSET       0x1FFF
  19
  20 #define PARSE_VLAN 1
  21 #define PARSE_MPLS 2
  22 #define PARSE_IP 3
  23 #define PARSE_IPV6 4
  24
  25 struct vlan_hdr {
  26         __be16 h_vlan_TCI;
  27         __be16 h_vlan_encapsulated_proto;
  28 };
  29
  30 struct flow_key_record {
  31         __be32 src;
  32         __be32 dst;
  33         union {
  34                 __be32 ports;
  35                 __be16 port16[2];
  36         };
  37         __u32 ip_proto;
  38 };
  39
  40 static inline void parse_eth_proto(struct __sk_buff *skb, u32 proto);
  41
  42 static inline int ip_is_fragment(struct __sk_buff *ctx, __u64 nhoff)
  43 {
  44         return load_half(ctx, nhoff + offsetof(struct iphdr, frag_off))
  45                 & (IP_MF | IP_OFFSET);
  46 }
  47
  48 static inline __u32 ipv6_addr_hash(struct __sk_buff *ctx, __u64 off)
  49 {
  50         __u64 w0 = load_word(ctx, off);
  51         __u64 w1 = load_word(ctx, off + 4);
  52         __u64 w2 = load_word(ctx, off + 8);
  53         __u64 w3 = load_word(ctx, off + 12);
  54
  55         return (__u32)(w0 ^ w1 ^ w2 ^ w3);
  56 }
  57
  58 struct globals {
  59         struct flow_key_record flow;
  60 };
  61
  62 struct {
  63         __uint(type, BPF_MAP_TYPE_ARRAY);
  64         __type(key, __u32);
  65         __type(value, struct globals);
  66         __uint(max_entries, 32);
  67 } percpu_map SEC(".maps");
  68
  69 /* user poor man's per_cpu until native support is ready */
  70 static struct globals *this_cpu_globals(void)
  71 {
  72         u32 key = bpf_get_smp_processor_id();
  73
  74         return bpf_map_lookup_elem(&percpu_map, &key);
  75 }
  76
  77 /* some simple stats for user space consumption */
  78 struct pair {
  79         __u64 packets;
  80         __u64 bytes;
  81 };
  82
  83 struct {
  84         __uint(type, BPF_MAP_TYPE_HASH);
  85         __type(key, struct flow_key_record);
  86         __type(value, struct pair);
  87         __uint(max_entries, 1024);
  88 } hash_map SEC(".maps");
  89
  90 static void update_stats(struct __sk_buff *skb, struct globals *g)
  91 {
  92         struct flow_key_record key = g->flow;
  93         struct pair *value;
  94
  95         value = bpf_map_lookup_elem(&hash_map, &key);
  96         if (value) {
  97                 __sync_fetch_and_add(&value->packets, 1);
  98                 __sync_fetch_and_add(&value->bytes, skb->len);
  99         } else {
 100                 struct pair val = {1, skb->len};
 101
 102                 bpf_map_update_elem(&hash_map, &key, &val, BPF_ANY);
 103         }
 104 }
 105
 106 static __always_inline void parse_ip_proto(struct __sk_buff *skb,
 107                                            struct globals *g, __u32 ip_proto)
 108 {
 109         __u32 nhoff = skb->cb[0];
 110         int poff;
 111
 112         switch (ip_proto) {
 113         case IPPROTO_GRE: {
 114                 struct gre_hdr {
 115                         __be16 flags;
 116                         __be16 proto;
 117                 };
 118
 119                 __u32 gre_flags = load_half(skb,
 120                                             nhoff + offsetof(struct gre_hdr, flags));
 121                 __u32 gre_proto = load_half(skb,
 122                                             nhoff + offsetof(struct gre_hdr, proto));
 123
 124                 if (gre_flags & (GRE_VERSION|GRE_ROUTING))
 125                         break;
 126
 127                 nhoff += 4;
 128                 if (gre_flags & GRE_CSUM)
 129                         nhoff += 4;
 130                 if (gre_flags & GRE_KEY)
 131                         nhoff += 4;
 132                 if (gre_flags & GRE_SEQ)
 133                         nhoff += 4;
 134
 135                 skb->cb[0] = nhoff;
 136                 parse_eth_proto(skb, gre_proto);
 137                 break;
 138         }
 139         case IPPROTO_IPIP:
 140                 parse_eth_proto(skb, ETH_P_IP);
 141                 break;
 142         case IPPROTO_IPV6:
 143                 parse_eth_proto(skb, ETH_P_IPV6);
 144                 break;
 145         case IPPROTO_TCP:
 146         case IPPROTO_UDP:
 147                 g->flow.ports = load_word(skb, nhoff);
 148         case IPPROTO_ICMP:
 149                 g->flow.ip_proto = ip_proto;
 150                 update_stats(skb, g);
 151                 break;
 152         default:
 153                 break;
 154         }
 155 }
 156
 157 SEC("socket")
 158 int bpf_func_ip(struct __sk_buff *skb)
 159 {
 160         struct globals *g = this_cpu_globals();
 161         __u32 nhoff, verlen, ip_proto;
 162
 163         if (!g)
 164                 return 0;
 165
 166         nhoff = skb->cb[0];
 167
 168         if (unlikely(ip_is_fragment(skb, nhoff)))
 169                 return 0;
 170
 171         ip_proto = load_byte(skb, nhoff + offsetof(struct iphdr, protocol));
 172
 173         if (ip_proto != IPPROTO_GRE) {
 174                 g->flow.src = load_word(skb, nhoff + offsetof(struct iphdr, saddr));
 175                 g->flow.dst = load_word(skb, nhoff + offsetof(struct iphdr, daddr));
 176         }
 177
 178         verlen = load_byte(skb, nhoff + 0/*offsetof(struct iphdr, ihl)*/);
 179         nhoff += (verlen & 0xF) << 2;
 180
 181         skb->cb[0] = nhoff;
 182         parse_ip_proto(skb, g, ip_proto);
 183         return 0;
 184 }
 185
 186 SEC("socket")
 187 int bpf_func_ipv6(struct __sk_buff *skb)
 188 {
 189         struct globals *g = this_cpu_globals();
 190         __u32 nhoff, ip_proto;
 191
 192         if (!g)
 193                 return 0;
 194
 195         nhoff = skb->cb[0];
 196
 197         ip_proto = load_byte(skb,
 198                              nhoff + offsetof(struct ipv6hdr, nexthdr));
 199         g->flow.src = ipv6_addr_hash(skb,
 200                                      nhoff + offsetof(struct ipv6hdr, saddr));
 201         g->flow.dst = ipv6_addr_hash(skb,
 202                                      nhoff + offsetof(struct ipv6hdr, daddr));
 203         nhoff += sizeof(struct ipv6hdr);
 204
 205         skb->cb[0] = nhoff;
 206         parse_ip_proto(skb, g, ip_proto);
 207         return 0;
 208 }
 209
 210 SEC("socket")
 211 int bpf_func_vlan(struct __sk_buff *skb)
 212 {
 213         __u32 nhoff, proto;
 214
 215         nhoff = skb->cb[0];
 216
 217         proto = load_half(skb, nhoff + offsetof(struct vlan_hdr,
 218                                                 h_vlan_encapsulated_proto));
 219         nhoff += sizeof(struct vlan_hdr);
 220         skb->cb[0] = nhoff;
 221
 222         parse_eth_proto(skb, proto);
 223
 224         return 0;
 225 }
 226
 227 SEC("socket")
 228 int bpf_func_mpls(struct __sk_buff *skb)
 229 {
 230         __u32 nhoff, label;
 231
 232         nhoff = skb->cb[0];
 233
 234         label = load_word(skb, nhoff);
 235         nhoff += sizeof(struct mpls_label);
 236         skb->cb[0] = nhoff;
 237
 238         if (label & MPLS_LS_S_MASK) {
 239                 __u8 verlen = load_byte(skb, nhoff);
 240                 if ((verlen & 0xF0) == 4)
 241                         parse_eth_proto(skb, ETH_P_IP);
 242                 else
 243                         parse_eth_proto(skb, ETH_P_IPV6);
 244         } else {
 245                 parse_eth_proto(skb, ETH_P_MPLS_UC);
 246         }
 247
 248         return 0;
 249 }
 250
 251 struct {
 252         __uint(type, BPF_MAP_TYPE_PROG_ARRAY);
 253         __uint(key_size, sizeof(u32));
 254         __uint(max_entries, 8);
 255         __array(values, u32 (void *));
 256 } prog_array_init SEC(".maps") = {
 257         .values = {
 258                 [PARSE_VLAN] = (void *)&bpf_func_vlan,
 259                 [PARSE_IP]   = (void *)&bpf_func_ip,
 260                 [PARSE_IPV6] = (void *)&bpf_func_ipv6,
 261                 [PARSE_MPLS] = (void *)&bpf_func_mpls,
 262         },
 263 };
 264
 265 /* Protocol dispatch routine. It tail-calls next BPF program depending
 266  * on eth proto. Note, we could have used ...
 267  *
 268  *   bpf_tail_call(skb, &prog_array_init, proto);
 269  *
 270  * ... but it would need large prog_array and cannot be optimised given
 271  * the map key is not static.
 272  */
 273 static inline void parse_eth_proto(struct __sk_buff *skb, u32 proto)
 274 {
 275         switch (proto) {
 276         case ETH_P_8021Q:
 277         case ETH_P_8021AD:
 278                 bpf_tail_call(skb, &prog_array_init, PARSE_VLAN);
 279                 break;
 280         case ETH_P_MPLS_UC:
 281         case ETH_P_MPLS_MC:
 282                 bpf_tail_call(skb, &prog_array_init, PARSE_MPLS);
 283                 break;
 284         case ETH_P_IP:
 285                 bpf_tail_call(skb, &prog_array_init, PARSE_IP);
 286                 break;
 287         case ETH_P_IPV6:
 288                 bpf_tail_call(skb, &prog_array_init, PARSE_IPV6);
 289                 break;
 290         }
 291 }
 292
 293 SEC("socket")
 294 int main_prog(struct __sk_buff *skb)
 295 {
 296         __u32 nhoff = ETH_HLEN;
 297         __u32 proto = load_half(skb, 12);
 298
 299         skb->cb[0] = nhoff;
 300         parse_eth_proto(skb, proto);
 301         return 0;
 302 }
 303
 304 char _license[] SEC("license") = "GPL";