1 files changed, 84 insertions, 10 deletions

diff --git a/src/openvpn/proto.c b/src/openvpn/proto.c
index 87c18e8..6f4d929 100644
--- a/src/openvpn/proto.c
+++ b/src/openvpn/proto.c
@@ -38,17 +38,17 @@
  * If raw tunnel packet is IPv<X>, return true and increment
  * buffer offset to start of IP header.
  */
-static
-bool
-is_ipv_X( int tunnel_type, struct buffer *buf, int ip_ver )
+static bool
+is_ipv_X(int tunnel_type, struct buffer *buf, int ip_ver)
 {
     int offset;
+    uint16_t proto;
     const struct openvpn_iphdr *ih;
 
     verify_align_4(buf);
     if (tunnel_type == DEV_TYPE_TUN)
     {
-        if (BLEN(buf) < (int) sizeof(struct openvpn_iphdr))
+        if (BLEN(buf) < sizeof(struct openvpn_iphdr))
         {
             return false;
         }
@@ -57,24 +57,46 @@ is_ipv_X( int tunnel_type, struct buffer *buf, int ip_ver )
     else if (tunnel_type == DEV_TYPE_TAP)
     {
         const struct openvpn_ethhdr *eh;
-        if (BLEN(buf) < (int)(sizeof(struct openvpn_ethhdr)
-                              + sizeof(struct openvpn_iphdr)))
+        if (BLEN(buf) < (sizeof(struct openvpn_ethhdr)
+                         + sizeof(struct openvpn_iphdr)))
         {
             return false;
         }
-        eh = (const struct openvpn_ethhdr *) BPTR(buf);
-        if (ntohs(eh->proto) != (ip_ver == 6 ? OPENVPN_ETH_P_IPV6 : OPENVPN_ETH_P_IPV4))
+        eh = (const struct openvpn_ethhdr *)BPTR(buf);
+
+        /* start by assuming this is a standard Eth fram */
+        proto = eh->proto;
+        offset = sizeof(struct openvpn_ethhdr);
+
+        /* if this is a 802.1q frame, parse the header using the according
+         * format
+         */
+        if (proto == htons(OPENVPN_ETH_P_8021Q))
+        {
+            const struct openvpn_8021qhdr *evh;
+            if (BLEN(buf) < (sizeof(struct openvpn_ethhdr)
+                             + sizeof(struct openvpn_iphdr)))
+            {
+                return false;
+            }
+
+            evh = (const struct openvpn_8021qhdr *)BPTR(buf);
+
+            proto = evh->proto;
+            offset = sizeof(struct openvpn_8021qhdr);
+        }
+
+        if (ntohs(proto) != (ip_ver == 6 ? OPENVPN_ETH_P_IPV6 : OPENVPN_ETH_P_IPV4))
         {
             return false;
         }
-        offset = sizeof(struct openvpn_ethhdr);
     }
     else
     {
         return false;
     }
 
-    ih = (const struct openvpn_iphdr *) (BPTR(buf) + offset);
+    ih = (const struct openvpn_iphdr *)(BPTR(buf) + offset);
 
     /* IP version is stored in the same bits for IPv4 or IPv6 header */
     if (OPENVPN_IPH_GET_VER(ih->version_len) == ip_ver)
@@ -98,6 +120,58 @@ is_ipv6(int tunnel_type, struct buffer *buf)
     return is_ipv_X( tunnel_type, buf, 6 );
 }
 
+
+uint16_t
+ip_checksum(const sa_family_t af, const uint8_t *payload, const int len_payload,
+            const uint8_t *src_addr, const uint8_t *dest_addr, const int proto)
+{
+    uint32_t sum = 0;
+    int addr_len = (af == AF_INET) ? 4 : 16;
+
+    /*
+     * make 16 bit words out of every two adjacent 8 bit words and  */
+    /* calculate the sum of all 16 bit words
+     */
+    for (int i = 0; i < len_payload; i += 2)
+    {
+        sum +=  (uint16_t)(((payload[i] << 8) & 0xFF00)
+                           +((i + 1 < len_payload) ? (payload[i + 1] & 0xFF) : 0));
+
+    }
+
+    /*
+     * add the pseudo header which contains the IP source and destination
+     * addresses
+     */
+    for (int i = 0; i < addr_len; i += 2)
+    {
+        sum += (uint16_t)((src_addr[i] << 8) & 0xFF00) + (src_addr[i + 1] & 0xFF);
+
+    }
+    for (int i = 0; i < addr_len; i += 2)
+    {
+        sum += (uint16_t)((dest_addr[i] << 8) & 0xFF00) + (dest_addr[i + 1] & 0xFF);
+    }
+
+    /* the length of the payload */
+    sum += (uint16_t)len_payload;
+
+    /* The next header or proto field*/
+    sum += (uint16_t)proto;
+
+    /*
+     * keep only the last 16 bits of the 32 bit calculated sum and add
+     * the carries
+     */
+    while (sum >> 16)
+    {
+        sum = (sum & 0xFFFF) + (sum >> 16);
+    }
+
+    /* Take the one's complement of sum */
+    return ((uint16_t) ~sum);
+}
+
 #ifdef PACKET_TRUNCATION_CHECK
 
 void