sockex3_kern.c 6.2 KB

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