From 57ac099703c11b11a92790273fde9b6631008a03 Mon Sep 17 00:00:00 2001
From: Teodoro Vargas <tevaco19@outlook.com>
Date: Tue, 9 Oct 2018 15:02:01 -0600
Subject: [PATCH] Fix serialization for QinQ (#316)

* Add QinQ Frame

* Fix serialization for QinQ
---
 include/tins/pdu.h         |  1 +
 src/detail/pdu_helpers.cpp |  7 ++++++-
 src/dot1q.cpp              | 11 +++--------
 src/ethernetII.cpp         | 11 ++++++++++-
 src/utils/pdu_utils.cpp    |  1 +
 tests/src/dot1q_test.cpp   |  6 ++++++
 6 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/include/tins/pdu.h b/include/tins/pdu.h
index 62dc0f8..b2165c1 100644
--- a/include/tins/pdu.h
+++ b/include/tins/pdu.h
@@ -170,6 +170,7 @@ public:
         ICMPv6,
         SLL,
         DHCPv6,
+        DOT1AD,
         DOT1Q,
         PPPOE,
         STP,
diff --git a/src/detail/pdu_helpers.cpp b/src/detail/pdu_helpers.cpp
index 1b94595..bb8f4e5 100644
--- a/src/detail/pdu_helpers.cpp
+++ b/src/detail/pdu_helpers.cpp
@@ -211,6 +211,8 @@ Constants::Ethernet::e pdu_flag_to_ether_type(PDU::PDUType flag) {
             return Constants::Ethernet::ARP;
         case PDU::DOT1Q:
             return Constants::Ethernet::VLAN;
+        case PDU::DOT1AD:
+            return Constants::Ethernet::QINQ;
         case PDU::PPPOE:
             return Constants::Ethernet::PPPOED;
         case PDU::MPLS:
@@ -238,6 +240,9 @@ PDU::PDUType ether_type_to_pdu_flag(Constants::Ethernet::e flag) {
             return PDU::ARP;
         case Constants::Ethernet::VLAN:
             return PDU::DOT1Q;
+        case Constants::Ethernet::QINQ:
+        case Constants::Ethernet::OLD_QINQ:
+            return PDU::DOT1AD;
         case Constants::Ethernet::PPPOED:
             return PDU::PPPOE;
         //case PDU::RSNEAPOL
@@ -295,4 +300,4 @@ PDU::PDUType ip_type_to_pdu_flag(Constants::IP::e flag) {
 }
 
 } // Internals
-} // Tins
\ No newline at end of file
+} // Tins
diff --git a/src/dot1q.cpp b/src/dot1q.cpp
index c5522a6..462a4a3 100644
--- a/src/dot1q.cpp
+++ b/src/dot1q.cpp
@@ -107,15 +107,10 @@ uint32_t Dot1Q::trailer_size() const {
 void Dot1Q::write_serialization(uint8_t* buffer, uint32_t total_sz) {
     OutputMemoryStream stream(buffer, total_sz);
     if (inner_pdu()) {
-        Constants::Ethernet::e flag;
+        Constants::Ethernet::e flag = Constants::Ethernet::UNKNOWN;
         PDUType type = inner_pdu()->pdu_type();
-        if (type == PDU::DOT1Q) {
-            flag = Constants::Ethernet::QINQ;
-        }
-        else {
-            // Set the appropriate payload type flag
-            flag = Internals::pdu_flag_to_ether_type(type);
-        }
+        // Set the appropriate payload type flag
+        flag = Internals::pdu_flag_to_ether_type(type);
         if (flag != Constants::Ethernet::UNKNOWN) {
             payload_type(static_cast<uint16_t>(flag));
         }
diff --git a/src/ethernetII.cpp b/src/ethernetII.cpp
index 047e92c..91d2949 100644
--- a/src/ethernetII.cpp
+++ b/src/ethernetII.cpp
@@ -156,7 +156,7 @@ bool EthernetII::matches_response(const uint8_t* ptr, uint32_t total_sz) const {
 void EthernetII::write_serialization(uint8_t* buffer, uint32_t total_sz) {
     OutputMemoryStream stream(buffer, total_sz);
     if (inner_pdu()) {
-        Constants::Ethernet::e flag;
+        Constants::Ethernet::e flag = Constants::Ethernet::UNKNOWN;
         const PDUType type = inner_pdu()->pdu_type();
         // Dirty trick to successfully tag PPPoE session/discovery packets
         if (type == PDU::PPPOE) {
@@ -164,6 +164,15 @@ void EthernetII::write_serialization(uint8_t* buffer, uint32_t total_sz) {
             flag = (pppoe->code() == 0) ? Constants::Ethernet::PPPOES 
                                         : Constants::Ethernet::PPPOED;
         }
+        // Dirty trick: Double Dot1Q is interpreted as Dot1AD
+        else if (type == PDU::DOT1Q) {
+            if (inner_pdu()->inner_pdu()) {
+                const PDUType inner_type = inner_pdu()->inner_pdu()->pdu_type();
+                if (inner_type == PDU::DOT1Q) {
+                    flag = Constants::Ethernet::QINQ;
+                }
+            }
+        }
         else {
             flag = Internals::pdu_flag_to_ether_type(type);
         }
diff --git a/src/utils/pdu_utils.cpp b/src/utils/pdu_utils.cpp
index 7a89eba..3714e32 100644
--- a/src/utils/pdu_utils.cpp
+++ b/src/utils/pdu_utils.cpp
@@ -81,6 +81,7 @@ string to_string(PDU::PDUType pduType) {
         ENUM_TEXT(ICMPv6);
         ENUM_TEXT(SLL);
         ENUM_TEXT(DHCPv6);
+        ENUM_TEXT(DOT1AD);
         ENUM_TEXT(DOT1Q);
         ENUM_TEXT(PPPOE);
         ENUM_TEXT(STP);
diff --git a/tests/src/dot1q_test.cpp b/tests/src/dot1q_test.cpp
index 3075bce..2188586 100644
--- a/tests/src/dot1q_test.cpp
+++ b/tests/src/dot1q_test.cpp
@@ -86,6 +86,12 @@ TEST_F(Dot1QTest, QinQ) {
     EthernetII pkt = EthernetII() / Dot1Q(10) / Dot1Q(42) / IP("192.168.1.2") / 
                      TCP(23, 45) / RawPDU("asdasdasd");
     PDU::serialization_type buffer = pkt.serialize();
+    // First PID of the serialized packet should be Dot1AD = 0x88a8
+    EXPECT_EQ(buffer[12], 0x88);
+    EXPECT_EQ(buffer[13], 0xa8);
+    // Second PID of the serialized packet chouls be Dot1Q = 0x8100
+    EXPECT_EQ(buffer[16], 0x81);
+    EXPECT_EQ(buffer[17], 0x00);
     EthernetII pkt2(&buffer[0], buffer.size());
     const Dot1Q& q1 = pkt2.rfind_pdu<Dot1Q>();
     ASSERT_TRUE(q1.inner_pdu() != NULL);