Add L2TP utils

5 files changed, 524 insertions, 112 deletions

diff --git a/src/packet/gre_utils.h b/src/packet/gre_utils.h
index 7050c35..a330a05 100644
--- a/src/packet/gre_utils.h
+++ b/src/packet/gre_utils.h
@@ -75,11 +75,7 @@ struct sre
 #define GRE_ROUTING 0x4000
 #define GRE_KEY 0x2000
 #define GRE_SEQUENCE 0x1000
-#define GRE_STRICTSOURCE 0x0800
-#define GRE_RECURSION 0x0700
 #define GRE_ACK 0x0080          /* only in special PPTPized GRE header */
-#define GRE_RESERVED_PPP 0x0078 /* only in special PPTPized GRE header */
-#define GRE_RESERVED 0x00F8
 
 /******************************************************************************
  * get
diff --git a/src/packet/l2tp_utils.h b/src/packet/l2tp_utils.h
new file mode 100644
index 0000000..35949d4
--- /dev/null
+++ b/src/packet/l2tp_utils.h
@@ -0,0 +1,264 @@
+#ifndef _L2TP_UTILS_H
+#define _L2TP_UTILS_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <arpa/inet.h>
+
+struct l2tp_hdr
+{
+    uint16_t flags;
+    // other option fields
+};
+
+#define L2TP_CONTROL_BIT 0x8000
+#define L2TP_LENGTH_BIT 0x4000
+#define L2TP_SEQUENCE_BIT 0x0800
+#define L2TP_OFFSET_BIT 0x0200
+#define L2TP_PRIORITY_BIT 0x0100
+#define L2TP_VERSION 0x000f
+
+/******************************************************************************
+ * get
+ ******************************************************************************/
+
+static inline uint8_t l2tp_hdr_get_ver(const struct l2tp_hdr *hdr)
+{
+    return ntohs(hdr->flags) & L2TP_VERSION;
+}
+
+// 1: control message
+// 0: data message
+static inline uint8_t l2tp_hdr_get_type(const struct l2tp_hdr *hdr)
+{
+    return (ntohs(hdr->flags) & L2TP_CONTROL_BIT) >> 15;
+}
+
+/*
+ * Layer Two Tunneling Protocol "L2TP" (V2)
+ *
+ *   0                   1                   2                   3
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |T|L|x|x|S|x|O|P|x|x|x|x|  Ver  |          Length (opt)         |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |           Tunnel ID           |           Session ID          |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |             Ns (opt)          |             Nr (opt)          |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |      Offset Size (opt)        |    Offset pad... (opt)
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * https://datatracker.ietf.org/doc/html/rfc2661
+ */
+
+static inline uint16_t calc_udp_l2tpv2_hdr_len(const char *data, uint16_t len)
+{
+    const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data;
+    uint16_t flags = ntohs(hdr->flags);
+
+    // ctrl message
+    if (flags & L2TP_CONTROL_BIT)
+    {
+        if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0 ||
+            (flags & L2TP_OFFSET_BIT) != 0 || (flags & L2TP_PRIORITY_BIT) != 0)
+        {
+            return 0;
+        }
+        else
+        {
+            return ntohs(*((uint16_t *)(data + 2)));
+        }
+    }
+    // data message
+    else
+    {
+        uint16_t skip_len = 2; // skip flags field
+        if (flags & L2TP_LENGTH_BIT)
+        {
+            skip_len += 2; // skip length field
+        }
+        skip_len += 2; // skip tunnel id field
+        skip_len += 2; // skip session id field
+        if (flags & L2TP_SEQUENCE_BIT)
+        {
+            skip_len += 2; // skip ns field
+            skip_len += 2; // skip nr field
+        }
+        if (flags & L2TP_OFFSET_BIT)
+        {
+            if (skip_len + 2 > len)
+            {
+                return 0;
+            }
+            uint16_t offset = ntohs(*((uint16_t *)(data + skip_len)));
+            if (offset == 0)
+            {
+                skip_len += 2; // skip offset field
+            }
+            else
+            {
+                skip_len = offset;
+            }
+        }
+        return skip_len;
+    }
+}
+
+/*
+ * Figure 4.1.1.1: L2TPv3 Session Header Over IP
+ *
+ *   0                   1                   2                   3
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                           Session ID                          |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |               Cookie (optional, maximum 64 bits)...
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *                                                                  |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * Figure 4.1.1.2: L2TPv3 Control Message Header Over IP
+ *
+ *   0                   1                   2                   3
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                      (32 bits of zeros)                       |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |T|L|x|x|S|x|x|x|x|x|x|x|  Ver  |             Length            |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                     Control Connection ID                     |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |               Ns              |               Nr              |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * Note: Unlike L2TP over UDP, which uses the T bit to distinguish between
+ * L2TP control and data packets, L2TP over IP uses the reserved Session
+ * ID of zero (0) when sending control messages.
+ *
+ * https://www.rfc-editor.org/rfc/rfc3931.html
+ */
+
+static inline uint16_t calc_ip_l2tpv3_hdr_len(const char *data, uint16_t len)
+{
+    if (len < 4)
+    {
+        return 0;
+    }
+    uint32_t session_id = ntohl(*((uint32_t *)data));
+    // data message
+    if (session_id)
+    {
+        // TODO The optional Cookie field contains a variable-length value (maximum 64 bits)
+        // TODO L2-Specific Sublayer 4 bytes
+        return 4 + 4;
+    }
+    // control message
+    else
+    {
+        if (len < 16)
+        {
+            return 0;
+        }
+        uint16_t flags = ntohs(*((uint16_t *)(data + 4)));
+        if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0)
+        {
+            return 0;
+        }
+        else
+        {
+            return ntohs(*((uint16_t *)(data + 4 + 2)));
+        }
+    }
+}
+
+/*
+ * Layer Two Tunneling Protocol - Version 3 (L2TPv3)
+ *
+ * Figure 3.2.1: L2TP Control Message Header
+ *
+ *   0                   1                   2                   3
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |T|L|x|x|S|x|x|x|x|x|x|x|  Ver  |             Length            |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                     Control Connection ID                     |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |               Ns              |               Nr              |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * Figure 4.1.2.1: L2TPv3 Session Header over UDP
+ *
+ *   0                   1                   2                   3
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |T|x|x|x|x|x|x|x|x|x|x|x|  Ver  |          Reserved             |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                           Session ID                          |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |               Cookie (optional, maximum 64 bits)...
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *                                                                  |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * https://www.rfc-editor.org/rfc/rfc3931.html
+ */
+
+static inline uint16_t calc_udp_l2tpv3_hdr_len(const char *data, uint16_t len)
+{
+    if (len < 8)
+    {
+        return 0;
+    }
+
+    const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data;
+    uint16_t flags = ntohs(hdr->flags);
+
+    // ctrl message
+    if (flags & L2TP_CONTROL_BIT)
+    {
+        if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0)
+        {
+            return 0;
+        }
+        else
+        {
+            return ntohs(*((uint16_t *)(data + 2)));
+        }
+    }
+    // data message
+    else
+    {
+        // TODO The optional Cookie field contains a variable-length value (maximum 64 bits)
+        // TODO L2-Specific Sublayer 4 bytes
+        return 8 + 4;
+    }
+}
+
+/******************************************************************************
+ * set
+ ******************************************************************************/
+
+// TODO
+
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
+static inline int l2tp_hdr_to_str(const struct l2tp_hdr *hdr, char *buf, size_t size)
+{
+    memset(buf, 0, size);
+    return snprintf(buf, size, "L2TP: type=%s version=%u",
+                    l2tp_hdr_get_type(hdr) ? "control" : "data", l2tp_hdr_get_ver(hdr));
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/packet/packet.cpp b/src/packet/packet.cpp
index 278f6ba..d94fb61 100644
--- a/src/packet/packet.cpp
+++ b/src/packet/packet.cpp
@@ -13,6 +13,7 @@
 #include "ipv4_utils.h"
 #include "ipv6_utils.h"
 #include "mpls_utils.h"
+#include "l2tp_utils.h"
 #include "vlan_utils.h"
 #include "vxlan_utils.h"
 
@@ -57,13 +58,12 @@ static inline void set_tuple6(const char *data, enum layer_type type, struct tup
 static inline struct packet_layer *get_free_layer(struct packet *pkt);
 
 static inline uint16_t get_gtp_hdr_len(const char *data, uint16_t len);
-static inline uint16_t get_l2tpv2_hdr_len(const char *data, uint16_t len);
 
 // 数据链路层
 static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len);
 static inline const char *parse_pweth(struct packet *pkt, const char *data, uint16_t len);
 static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16_t len);
-static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len);
+static inline const char *parse_l2tpv2_over_udp(struct packet *pkt, const char *data, uint16_t len);
 static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len);
 static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *data, uint16_t len);
 
@@ -354,99 +354,6 @@ static inline uint16_t get_gtp_hdr_len(const char *data, uint16_t len)
     return hdr_offset;
 }
 
-static inline uint16_t get_l2tpv2_hdr_len(const char *data, uint16_t len)
-{
-    /*
-     * Layer Two Tunneling Protocol "L2TP"
-     * https://datatracker.ietf.org/doc/html/rfc2661
-     *
-     * 0                   1                   2                   3
-     * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
-     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-     * |T|L|x|x|S|x|O|P|x|x|x|x|  Ver  |          Length (opt)         |
-     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-     * |           Tunnel ID           |           Session ID          |
-     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-     * |             Ns (opt)          |             Nr (opt)          |
-     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-     * |      Offset Size (opt)        |    Offset pad... (opt)
-     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-     *
-     * Figure 3.1 L2TP Message Header
-     */
-
-#define CONTROL_BIT(msg_info) (msg_info & 0x8000)  // Type bit control = 1 data = 0
-#define LENGTH_BIT(msg_info) (msg_info & 0x4000)   // Length bit = 1
-#define SEQUENCE_BIT(msg_info) (msg_info & 0x0800) // SEQUENCE bit = 1 Ns and Nr fields
-#define OFFSET_BIT(msg_info) (msg_info & 0x0200)   // Offset
-#define PRIORITY_BIT(msg_info) (msg_info & 0x0100) // Priority
-#define L2TP_VERSION(msg_info) (msg_info & 0x000f) // Version of l2tp
-
-    if (unlikely(len < 6))
-    {
-        return 0;
-    }
-
-    uint16_t control = ntohs(*((uint16_t *)data));
-    if (L2TP_VERSION(control) != 2)
-    {
-        return 0;
-    }
-
-    if (CONTROL_BIT(control))
-    {
-        if (LENGTH_BIT(control) == 0 || SEQUENCE_BIT(control) == 0 || OFFSET_BIT(control) != 0 || PRIORITY_BIT(control) != 0)
-        {
-            return 0;
-        }
-        else
-        {
-            return ntohs(*((uint16_t *)(data + 2)));
-        }
-    }
-    else
-    {
-        uint16_t skip_len = 2;
-        if (LENGTH_BIT(control))
-        {
-            skip_len += 2; // skip length field
-        }
-        skip_len += 2; // skip tunnel id field
-        skip_len += 2; // skip session id field
-        if (SEQUENCE_BIT(control))
-        {
-            skip_len += 2; // skip ns field
-            skip_len += 2; // skip nr field
-        }
-        if (OFFSET_BIT(control))
-        {
-            if (skip_len + 2 > len)
-            {
-                return 0;
-            }
-            uint16_t offset = ntohs(*((uint16_t *)(data + skip_len)));
-            if (offset == 0)
-            {
-                return skip_len + 2;
-            }
-
-            // invalid offset
-            if (offset > len)
-            {
-                return 0;
-            }
-            else
-            {
-                return offset;
-            }
-        }
-        else
-        {
-            return skip_len;
-        }
-    }
-}
-
 static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len)
 {
     if (unlikely(len < sizeof(struct ethhdr)))
@@ -594,11 +501,9 @@ success:
     }
 }
 
-static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len)
+static inline const char *parse_l2tpv2_over_udp(struct packet *pkt, const char *data, uint16_t len)
 {
-#define CONTROL_BIT(msg_info) (msg_info & 0x8000) // Type bit control = 1 data = 0
-
-    uint16_t hdr_len = get_l2tpv2_hdr_len(data, len);
+    uint16_t hdr_len = calc_udp_l2tpv2_hdr_len(data, len);
     if (unlikely(hdr_len == 0 || hdr_len > len))
     {
         PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_TYPE_L2TP);
@@ -612,11 +517,12 @@ static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uin
     }
     SET_LAYER(pkt, layer, LAYER_TYPE_L2TP, hdr_len, data, len, 0);
 
-    uint16_t control = ntohs(*((uint16_t *)data));
-    if (CONTROL_BIT(control))
+    // control message
+    if (l2tp_hdr_get_type((const struct l2tp_hdr *)data))
     {
         return layer->pld_ptr;
     }
+    // data message
     else
     {
         return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
@@ -625,14 +531,60 @@ static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uin
 
 static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len)
 {
-    // TODO
-    return data;
+    uint16_t hdr_len = calc_udp_l2tpv3_hdr_len(data, len);
+    if (unlikely(hdr_len == 0 || hdr_len > len))
+    {
+        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_TYPE_L2TP);
+        return data;
+    }
+
+    struct packet_layer *layer = get_free_layer(pkt);
+    if (unlikely(layer == NULL))
+    {
+        return data;
+    }
+    SET_LAYER(pkt, layer, LAYER_TYPE_L2TP, hdr_len, data, len, 0);
+
+    // control message
+    if (l2tp_hdr_get_type((const struct l2tp_hdr *)data))
+    {
+        return layer->pld_ptr;
+    }
+    // data message
+    else
+    {
+        // TOOD
+        return layer->pld_ptr;
+    }
 }
 
 static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *data, uint16_t len)
 {
-    // TODO
-    return data;
+    uint16_t hdr_len = calc_ip_l2tpv3_hdr_len(data, len);
+    if (unlikely(hdr_len == 0 || hdr_len > len))
+    {
+        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_TYPE_L2TP);
+        return data;
+    }
+
+    struct packet_layer *layer = get_free_layer(pkt);
+    if (unlikely(layer == NULL))
+    {
+        return data;
+    }
+    SET_LAYER(pkt, layer, LAYER_TYPE_L2TP, hdr_len, data, len, 0);
+
+    // data message
+    if (ntohl(*((uint32_t *)data)))
+    {
+        // TOOD
+        return layer->pld_ptr;
+    }
+    // control message
+    else
+    {
+        return layer->pld_ptr;
+    }
 }
 
 static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len)
@@ -978,11 +930,10 @@ static inline const char *parse_udp(struct packet *pkt, const char *data, uint16
             return layer->pld_ptr;
         }
 
-        uint16_t control = ntohs(*((uint16_t *)layer->pld_ptr));
-        switch (L2TP_VERSION(control))
+        switch (l2tp_hdr_get_ver((const struct l2tp_hdr *)layer->pld_ptr))
         {
         case 2:
-            return parse_l2tpv2(pkt, layer->pld_ptr, layer->pld_len);
+            return parse_l2tpv2_over_udp(pkt, layer->pld_ptr, layer->pld_len);
         case 3:
             return parse_l2tpv3_over_udp(pkt, layer->pld_ptr, layer->pld_len);
         default:
@@ -1214,6 +1165,7 @@ void packet_print_str(const struct packet *pkt)
         case LAYER_TYPE_HDLC:
             break;
         case LAYER_TYPE_L2TP:
+            used = l2tp_hdr_to_str((const struct l2tp_hdr *)layer->hdr_ptr, buffer, sizeof(buffer));
             break;
         case LAYER_TYPE_VLAN:
             used = vlan_hdr_to_str((const struct vlan_hdr *)layer->hdr_ptr, buffer, sizeof(buffer));
diff --git a/src/packet/test/CMakeLists.txt b/src/packet/test/CMakeLists.txt
index b93656d..c5783ab 100644
--- a/src/packet/test/CMakeLists.txt
+++ b/src/packet/test/CMakeLists.txt
@@ -28,6 +28,9 @@ target_link_libraries(gtest_vxlan_utils packet gtest)
 add_executable(gtest_gre_utils gtest_gre_utils.cpp)
 target_link_libraries(gtest_gre_utils packet gtest)
 
+add_executable(gtest_l2tp_utils gtest_l2tp_utils.cpp)
+target_link_libraries(gtest_l2tp_utils packet gtest)
+
 add_executable(gtest_packet_frag gtest_packet_frag.cpp)
 target_link_libraries(gtest_packet_frag packet gtest)
 
@@ -42,4 +45,5 @@ gtest_discover_tests(gtest_eth_utils)
 gtest_discover_tests(gtest_vlan_utils)
 gtest_discover_tests(gtest_vxlan_utils)
 gtest_discover_tests(gtest_gre_utils)
+gtest_discover_tests(gtest_l2tp_utils)
 gtest_discover_tests(gtest_packet_frag)
\ No newline at end of file
diff --git a/src/packet/test/gtest_l2tp_utils.cpp b/src/packet/test/gtest_l2tp_utils.cpp
new file mode 100644
index 0000000..d971379
--- /dev/null
+++ b/src/packet/test/gtest_l2tp_utils.cpp
@@ -0,0 +1,196 @@
+#include <gtest/gtest.h>
+
+#include "l2tp_utils.h"
+
+/*
+ * Layer 2 Tunneling Protocol
+ *     Flags: 0xc802, Type: Control Message, Length Bit, Sequence Bit
+ *         1... .... .... .... = Type: Control Message (1)
+ *         .1.. .... .... .... = Length Bit: Length field is present
+ *         .... 1... .... .... = Sequence Bit: Ns and Nr fields are present
+ *         .... ..0. .... .... = Offset bit: Offset size field is not present
+ *         .... ...0 .... .... = Priority: No priority
+ *         .... .... .... 0010 = Version: 2
+ *     Length: 105
+ *     Tunnel ID: 0
+ *     Session ID: 0
+ *     Ns: 0
+ *     Nr: 0
+ *     Control Message AVP
+ *         1... .... .... .... = Mandatory: True
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1000 = Length: 8
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Control Message (0)
+ *         Message Type: Start_Control_Request (1)
+ *     Protocol Version AVP
+ *         1... .... .... .... = Mandatory: True
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1000 = Length: 8
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Protocol Version (2)
+ *         Version: 1
+ *         Revision: 0
+ *     Framing Capabilities AVP
+ *         1... .... .... .... = Mandatory: True
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1010 = Length: 10
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Framing Capabilities (3)
+ *         .... .... .... .... .... .... .... ..0. = Async Framing Supported: False
+ *         .... .... .... .... .... .... .... ...1 = Sync Framing Supported: True
+ *     Bearer Capabilities AVP
+ *         1... .... .... .... = Mandatory: True
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1010 = Length: 10
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Bearer Capabilities (4)
+ *         .... .... .... .... .... .... .... ..0. = Analog Access Supported: False
+ *         .... .... .... .... .... .... .... ...0 = Digital Access Supported: False
+ *     Firmware Revision AVP
+ *         0... .... .... .... = Mandatory: False
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1000 = Length: 8
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Firmware Revision (6)
+ *         Firmware Revision: 1537 (0x0601)
+ *     Host Name AVP
+ *         1... .... .... .... = Mandatory: True
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0001 0010 = Length: 18
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Host Name (7)
+ *         Host Name: IIE-SM-THINK
+ *     Vendor Name AVP
+ *         0... .... .... .... = Mandatory: False
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1111 = Length: 15
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Vendor Name (8)
+ *         Vendor Name: Microsoft
+ *     Assigned Tunnel ID AVP
+ *         1... .... .... .... = Mandatory: True
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1000 = Length: 8
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Assigned Tunnel ID (9)
+ *         Assigned Tunnel ID: 1
+ *     Receive Window Size AVP
+ *         1... .... .... .... = Mandatory: True
+ *         .0.. .... .... .... = Hidden: False
+ *         .... ..00 0000 1000 = Length: 8
+ *         Vendor ID: Reserved (0)
+ *         AVP Type: Receive Window Size (10)
+ *         Receive Window Size: 8
+ */
+
+unsigned char v2_over_udp_ctrl_msg[] = {
+    0xc8, 0x02, 0x00, 0x69, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x80, 0x08, 0x00, 0x00, 0x00, 0x02,
+    0x01, 0x00, 0x80, 0x0a, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x80, 0x0a, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00,
+    0x00, 0x06, 0x06, 0x01, 0x80, 0x12, 0x00, 0x00, 0x00, 0x07, 0x49, 0x49, 0x45, 0x2d, 0x53, 0x4d, 0x2d, 0x54, 0x48, 0x49, 0x4e, 0x4b, 0x00, 0x0f, 0x00, 0x00,
+    0x00, 0x08, 0x4d, 0x69, 0x63, 0x72, 0x6f, 0x73, 0x6f, 0x66, 0x74, 0x80, 0x08, 0x00, 0x00, 0x00, 0x09, 0x00, 0x01, 0x80, 0x08, 0x00, 0x00, 0x00, 0x0a, 0x00,
+    0x08};
+
+TEST(L2TP_V2_OVER_UDP_UTILS, CRTL_MSG)
+{
+    const struct l2tp_hdr *hdr = (struct l2tp_hdr *)v2_over_udp_ctrl_msg;
+
+    EXPECT_TRUE(l2tp_hdr_get_ver(hdr) == 2);
+    EXPECT_TRUE(l2tp_hdr_get_type(hdr) == 1);
+    EXPECT_TRUE(calc_udp_l2tpv2_hdr_len((const char *)v2_over_udp_ctrl_msg, sizeof(v2_over_udp_ctrl_msg)) == 105);
+}
+
+/*
+ * Layer 2 Tunneling Protocol
+ *     Flags: 0x4002, Type: Data Message, Length Bit
+ *         0... .... .... .... = Type: Data Message (0)
+ *         .1.. .... .... .... = Length Bit: Length field is present
+ *         .... 0... .... .... = Sequence Bit: Ns and Nr fields are not present
+ *         .... ..0. .... .... = Offset bit: Offset size field is not present
+ *         .... ...0 .... .... = Priority: No priority
+ *         .... .... .... 0010 = Version: 2
+ *     Length: 78
+ *     Tunnel ID: 28998
+ *     Session ID: 2
+ */
+
+unsigned char v2_over_udp_data_msg[] = {
+    0x40, 0x02, 0x00, 0x4e, 0x71, 0x46, 0x00, 0x02};
+
+TEST(L2TP_V2_OVER_UDP_UTILS, DATA_MSG)
+{
+    const struct l2tp_hdr *hdr = (struct l2tp_hdr *)v2_over_udp_data_msg;
+
+    EXPECT_TRUE(l2tp_hdr_get_ver(hdr) == 2);
+    EXPECT_TRUE(l2tp_hdr_get_type(hdr) == 0);
+    EXPECT_TRUE(calc_udp_l2tpv2_hdr_len((const char *)v2_over_udp_data_msg, sizeof(v2_over_udp_data_msg)) == 8);
+}
+
+/*
+ * TODO
+ */
+
+unsigned char v3_over_udp_ctrl_msg[] = {};
+
+TEST(L2TP_V3_OVER_UDP_UTILS, CRTL_MSG)
+{
+    // TODO
+}
+
+/*
+ * Layer 2 Tunneling Protocol version 3
+ *     Flags: 0x0003, Type: Data Message
+ *         0... .... .... .... = Type: Data Message (0)
+ *         .0.. .... .... .... = Length Bit: Length field is not present
+ *         .... 0... .... .... = Sequence Bit: Ns and Nr fields are not present
+ *         .... .... .... 0011 = Version: 3
+ *     Reserved: 0x0000
+ *     Session ID: 0x00000fa0
+ *     [Pseudowire Type: Unknown (0)]
+ *     Cookie: 00000000
+ */
+
+unsigned char v3_over_udp_data_msg[] = {
+    0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xa0, 0x00, 0x00, 0x00, 0x00};
+
+TEST(L2TP_V3_OVER_UDP_UTILS, DATA_MSG)
+{
+    const struct l2tp_hdr *hdr = (struct l2tp_hdr *)v3_over_udp_data_msg;
+
+    EXPECT_TRUE(l2tp_hdr_get_ver(hdr) == 3);
+    EXPECT_TRUE(l2tp_hdr_get_type(hdr) == 0);
+    EXPECT_TRUE(calc_udp_l2tpv3_hdr_len((const char *)v3_over_udp_data_msg, sizeof(v3_over_udp_data_msg)) == 12);
+}
+
+/*
+ * TODO
+ */
+
+unsigned char v3_over_ip_ctrl_msg[] = {};
+
+TEST(L2TP_V3_OVER_IP_UTILS, CRTL_MSG)
+{
+    // TODO
+}
+
+/*
+ * Layer 2 Tunneling Protocol version 3
+ *     Session ID: 0x00009652
+ *     [Pseudowire Type: Unknown (0)]
+ *     Cookie: ca031078
+ */
+
+unsigned char v3_over_ip_data_msg[] = {
+    0x00, 0x00, 0x96, 0x52, 0xca, 0x03, 0x10, 0x78};
+
+TEST(L2TP_V3_OVER_IP_UTILS, DATA_MSG)
+{
+    EXPECT_TRUE(ntohl(*((uint32_t *)v3_over_ip_data_msg)) != 0); // data message
+    EXPECT_TRUE(calc_ip_l2tpv3_hdr_len((const char *)v3_over_ip_data_msg, sizeof(v3_over_ip_data_msg)) == 8);
+}
+
+int main(int argc, char **argv)
+{
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}