parse_varlen.c 3.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150
  1. /* Copyright (c) 2016 Facebook
  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 <linux/if_ether.h>
  9. #include <linux/if_vlan.h>
  10. #include <linux/ip.h>
  11. #include <linux/ipv6.h>
  12. #include <linux/in.h>
  13. #include <linux/tcp.h>
  14. #include <linux/udp.h>
  15. #include <uapi/linux/bpf.h>
  16. #include <net/ip.h>
  17. #include <bpf/bpf_helpers.h>
  18. #define DEFAULT_PKTGEN_UDP_PORT 9
  19. #define DEBUG 0
  20. static int tcp(void *data, uint64_t tp_off, void *data_end)
  21. {
  22. struct tcphdr *tcp = data + tp_off;
  23. if (tcp + 1 > data_end)
  24. return 0;
  25. if (tcp->dest == htons(80) || tcp->source == htons(80))
  26. return TC_ACT_SHOT;
  27. return 0;
  28. }
  29. static int udp(void *data, uint64_t tp_off, void *data_end)
  30. {
  31. struct udphdr *udp = data + tp_off;
  32. if (udp + 1 > data_end)
  33. return 0;
  34. if (udp->dest == htons(DEFAULT_PKTGEN_UDP_PORT) ||
  35. udp->source == htons(DEFAULT_PKTGEN_UDP_PORT)) {
  36. if (DEBUG) {
  37. char fmt[] = "udp port 9 indeed\n";
  38. bpf_trace_printk(fmt, sizeof(fmt));
  39. }
  40. return TC_ACT_SHOT;
  41. }
  42. return 0;
  43. }
  44. static int parse_ipv4(void *data, uint64_t nh_off, void *data_end)
  45. {
  46. struct iphdr *iph;
  47. uint64_t ihl_len;
  48. iph = data + nh_off;
  49. if (iph + 1 > data_end)
  50. return 0;
  51. if (ip_is_fragment(iph))
  52. return 0;
  53. ihl_len = iph->ihl * 4;
  54. if (iph->protocol == IPPROTO_IPIP) {
  55. iph = data + nh_off + ihl_len;
  56. if (iph + 1 > data_end)
  57. return 0;
  58. ihl_len += iph->ihl * 4;
  59. }
  60. if (iph->protocol == IPPROTO_TCP)
  61. return tcp(data, nh_off + ihl_len, data_end);
  62. else if (iph->protocol == IPPROTO_UDP)
  63. return udp(data, nh_off + ihl_len, data_end);
  64. return 0;
  65. }
  66. static int parse_ipv6(void *data, uint64_t nh_off, void *data_end)
  67. {
  68. struct ipv6hdr *ip6h;
  69. struct iphdr *iph;
  70. uint64_t ihl_len = sizeof(struct ipv6hdr);
  71. uint64_t nexthdr;
  72. ip6h = data + nh_off;
  73. if (ip6h + 1 > data_end)
  74. return 0;
  75. nexthdr = ip6h->nexthdr;
  76. if (nexthdr == IPPROTO_IPIP) {
  77. iph = data + nh_off + ihl_len;
  78. if (iph + 1 > data_end)
  79. return 0;
  80. ihl_len += iph->ihl * 4;
  81. nexthdr = iph->protocol;
  82. } else if (nexthdr == IPPROTO_IPV6) {
  83. ip6h = data + nh_off + ihl_len;
  84. if (ip6h + 1 > data_end)
  85. return 0;
  86. ihl_len += sizeof(struct ipv6hdr);
  87. nexthdr = ip6h->nexthdr;
  88. }
  89. if (nexthdr == IPPROTO_TCP)
  90. return tcp(data, nh_off + ihl_len, data_end);
  91. else if (nexthdr == IPPROTO_UDP)
  92. return udp(data, nh_off + ihl_len, data_end);
  93. return 0;
  94. }
  95. SEC("varlen")
  96. int handle_ingress(struct __sk_buff *skb)
  97. {
  98. void *data = (void *)(long)skb->data;
  99. struct ethhdr *eth = data;
  100. void *data_end = (void *)(long)skb->data_end;
  101. uint64_t h_proto, nh_off;
  102. nh_off = sizeof(*eth);
  103. if (data + nh_off > data_end)
  104. return 0;
  105. h_proto = eth->h_proto;
  106. if (h_proto == ETH_P_8021Q || h_proto == ETH_P_8021AD) {
  107. struct vlan_hdr *vhdr;
  108. vhdr = data + nh_off;
  109. nh_off += sizeof(struct vlan_hdr);
  110. if (data + nh_off > data_end)
  111. return 0;
  112. h_proto = vhdr->h_vlan_encapsulated_proto;
  113. }
  114. if (h_proto == ETH_P_8021Q || h_proto == ETH_P_8021AD) {
  115. struct vlan_hdr *vhdr;
  116. vhdr = data + nh_off;
  117. nh_off += sizeof(struct vlan_hdr);
  118. if (data + nh_off > data_end)
  119. return 0;
  120. h_proto = vhdr->h_vlan_encapsulated_proto;
  121. }
  122. if (h_proto == htons(ETH_P_IP))
  123. return parse_ipv4(data, nh_off, data_end);
  124. else if (h_proto == htons(ETH_P_IPV6))
  125. return parse_ipv6(data, nh_off, data_end);
  126. return 0;
  127. }
  128. char _license[] SEC("license") = "GPL";