Merge pull request #144 from BonnyAD9/support-gre-protocol

Support gre protocol

Author: SiskaPavel

input/headers.hpp

@@ -46,6 +46,14 @@
 
 namespace ipxp {
 
+struct grehdr {
+   uint16_t flags;
+#define GRE_CHECKSUM 0x8000
+#define GRE_KEY      0x2000
+#define GRE_SEQNUM   0x1000
+   uint16_t type;
+};
+
 // Copied protocol headers from netinet/* files, which may not be present on other platforms
 
 struct ethhdr {

input/parser.cpp

@@ -231,6 +231,59 @@ inline uint16_t parse_trill(const u_char *data_ptr, uint16_t data_len, Packet *p
    return sizeof(trill_hdr) + op_len_bytes;
 }
 
+inline uint16_t parse_ipv4_hdr(const u_char *data_ptr, uint16_t data_len, Packet *pkt);
+inline uint16_t parse_ipv6_hdr(const u_char *data_ptr, uint16_t data_len, Packet *pkt);
+uint16_t process_mpls(const u_char *data_ptr, uint16_t data_len, Packet *pkt);
+inline uint16_t process_pppoe(const u_char *data_ptr, uint16_t data_len, Packet *pkt);
+
+inline uint16_t parse_gre(const u_char *data_ptr, uint16_t data_len, Packet *pkt)
+{
+   int gre_len = sizeof(struct grehdr);
+   if (data_len < gre_len) {
+       throw "Parser detected malformed packet";
+   }
+      throw "Parser detected malformed packet";
+
+   auto gre = (struct grehdr *)data_ptr;
+   auto flags = ntohs(gre->flags);
+   auto type = ntohs(gre->type);
+
+   // skip optional gre fields
+   if (flags & GRE_CHECKSUM) {
+      gre_len += 4;
+      DEBUG_MSG("GRE has checksum\n");
+   }
+   if (flags & GRE_KEY) {
+      gre_len += 4;
+      DEBUG_MSG("GRE has key\n");
+   }
+   if (flags & GRE_SEQNUM) {
+      gre_len += 4;
+      DEBUG_MSG("GRE has sequence number\n");
+   }
+
+   if (data_len < gre_len) {
+       throw "Parser detected malformed packet";
+   }
+
+   data_ptr += gre_len;
+   data_len -= gre_len;
+
+   switch (type) {
+   case ETH_P_IP:
+      return parse_ipv4_hdr(data_ptr, data_len, pkt) + gre_len;
+   case ETH_P_IPV6:
+      return parse_ipv6_hdr(data_ptr, data_len, pkt) + gre_len;
+   case ETH_P_MPLS_UC: case ETH_P_MPLS_MC:
+      return process_mpls(data_ptr, data_len, pkt) + gre_len;
+   case ETH_P_PPP_SES:
+      return process_pppoe(data_ptr, data_len, pkt) + gre_len;
+   default:
+      pkt->ip_proto = IPPROTO_GRE;
+      return 0;
+   }
+}
+
 /**
  * \brief Parse specific fields from IPv4 header.
  * \param [in] data_ptr Pointer to begin of header.
@@ -245,11 +298,21 @@ inline uint16_t parse_ipv4_hdr(const u_char *data_ptr, uint16_t data_len, Packet
       throw "Parser detected malformed packet";
    }
 
+   const int ihl = ip->ihl << 2;
+
+   if (ip->protocol == IPPROTO_GRE) {
+      DEBUG_MSG("Parse GRE in ipv4 header\n");
+      if (data_len < ihl) {
+          throw "Parser detected malformed packet";
+      }
+      return parse_gre(data_ptr + ihl, data_len - ihl, pkt) + ihl;
+   }
+
    pkt->ip_version = IP::v4;
    pkt->ip_proto = ip->protocol;
    pkt->ip_tos = ip->tos;
    pkt->ip_len = ntohs(ip->tot_len);
-   pkt->ip_payload_len = pkt->ip_len - (ip->ihl << 2);
+   pkt->ip_payload_len = pkt->ip_len - ihl;
    pkt->ip_ttl = ip->ttl;
    pkt->ip_flags = (ntohs(ip->frag_off) & 0xE000) >> 13;
    pkt->src_ip.v4 = ip->saddr;
@@ -269,7 +332,7 @@ inline uint16_t parse_ipv4_hdr(const u_char *data_ptr, uint16_t data_len, Packet
    DEBUG_MSG("\tSrc addr:\t%s\n",      inet_ntoa(*(struct in_addr *) (&ip->saddr)));
    DEBUG_MSG("\tDest addr:\t%s\n",     inet_ntoa(*(struct in_addr *) (&ip->daddr)));
 
-   return (ip->ihl << 2);
+   return ihl;
 }
 
 /**