GRE: parse gre protocol

Author: BonnyAD9

input/headers.hpp

@@ -61,6 +61,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

@@ -238,6 +238,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.