[feature] Support QinQ Decode

1 files changed, 204 insertions, 5 deletions

diff --git a/src/packet/packet.rs b/src/packet/packet.rs
index 57d871e..23bbc6f 100644
--- a/src/packet/packet.rs
+++ b/src/packet/packet.rs
@@ -353,13 +353,13 @@ impl Packet<'_> {
 }
 
 fn handle_l2<'a>(packet: &mut Packet<'a>, input: &'a [u8]) -> Result<(), PacketError> {
-    packet.event.push(PacketEvent::L2_EVENT);
     let result = EthernetFrame::decode(input);
     if let Ok((payload, header)) = result {
         dbg!(&header);
         packet
             .encapsulation
             .push(Encapsulation::L2_ETH(header, payload));
+        packet.event.push(PacketEvent::L2_EVENT);
         return handle_l3(packet, payload, header.ether_type);
     } else {
         return Err(PacketError::IncompleteEthernetFrame);
@@ -371,9 +371,8 @@ fn handle_l3<'a>(
     input: &'a [u8],
     next_proto: EtherType,
 ) -> Result<(), PacketError> {
-    packet.event.push(PacketEvent::L3_EVENT);
     match next_proto {
-        EtherType::VLAN => {
+        EtherType::QinQ | EtherType::VLAN => {
             let result = VlanHeader::decode(input);
             if let Ok((payload, header)) = result {
                 dbg!(&header);
@@ -381,7 +380,7 @@ fn handle_l3<'a>(
                 packet
                     .encapsulation
                     .push(Encapsulation::L3_VLAN(header, payload));
-
+                packet.event.push(PacketEvent::L3_EVENT);
                 return handle_l3(packet, payload, header.ether_type);
             } else {
                 return Err(PacketError::IncompleteEthernetFrame);
@@ -402,6 +401,7 @@ fn handle_l3<'a>(
 
                 // TODO IPv4 Fragment
 
+                packet.event.push(PacketEvent::L3_EVENT);
                 packet.event.push(PacketEvent::IPV4_EVENT);
                 return handle_l4(packet, payload, header.protocol);
             } else {
@@ -423,6 +423,7 @@ fn handle_l3<'a>(
 
                 // TODO IPv6 Fragment
 
+                packet.event.push(PacketEvent::L3_EVENT);
                 packet.event.push(PacketEvent::IPV6_EVENT);
                 return handle_l4(packet, payload, header.next_header);
             } else {
@@ -440,7 +441,6 @@ fn handle_l4<'a>(
     input: &'a [u8],
     next_proto: IPProtocol,
 ) -> Result<(), PacketError> {
-    packet.event.push(PacketEvent::L4_EVENT);
     match next_proto {
         IPProtocol::UDP => {
             let result = UdpHeader::decode(input);
@@ -450,6 +450,7 @@ fn handle_l4<'a>(
                     .encapsulation
                     .push(Encapsulation::L4_UDP(header, payload));
 
+                packet.event.push(PacketEvent::L4_EVENT);
                 packet.event.push(PacketEvent::UDP_EVENT);
                 return Ok(());
             } else {
@@ -467,6 +468,7 @@ fn handle_l4<'a>(
 
                 // TODO TCP Reassembly
 
+                packet.event.push(PacketEvent::L4_EVENT);
                 packet.event.push(PacketEvent::TCP_EVENT);
                 return Ok(());
             } else {
@@ -824,4 +826,201 @@ mod tests {
         );
         // assert_eq!(1, 0);
     }
+
+    #[test]
+    fn test_packet_handle_eth_qinq_van_ipv4_tcp() {
+        /*
+         * Frame 3: 62 bytes on wire (496 bits), 62 bytes captured (496 bits)
+         *     Encapsulation type: Ethernet (1)
+         *     Arrival Time: Nov 19, 2020 11:56:10.266735000 CST
+         *     [Time shift for this packet: 0.000000000 seconds]
+         *     Epoch Time: 1605758170.266735000 seconds
+         *     [Time delta from previous captured frame: 0.006757000 seconds]
+         *     [Time delta from previous displayed frame: 0.006757000 seconds]
+         *     [Time since reference or first frame: 0.008335000 seconds]
+         *     Frame Number: 3
+         *     Frame Length: 62 bytes (496 bits)
+         *     Capture Length: 62 bytes (496 bits)
+         *     [Frame is marked: False]
+         *     [Frame is ignored: False]
+         *     [Protocols in frame: eth:ethertype:ieee8021ad:ethertype:vlan:ethertype:ip:tcp]
+         *     [Coloring Rule Name: Checksum Errors]
+         *     [Coloring Rule String [truncated]: eth.fcs.status=="Bad" || ip.checksum.status=="Bad" || tcp.checksum.status=="Bad" || udp.checksum.status=="Bad" || sctp.checksum.status=="Bad" || mstp.checksum.status=="Bad" || cdp.checksum.status=="Bad" |]
+         * Ethernet II, Src: a2:c1:12:03:02:03 (a2:c1:12:03:02:03), Dst: a2:c1:12:03:01:64 (a2:c1:12:03:01:64)
+         *     Destination: a2:c1:12:03:01:64 (a2:c1:12:03:01:64)
+         *         Address: a2:c1:12:03:01:64 (a2:c1:12:03:01:64)
+         *         .... ..1. .... .... .... .... = LG bit: Locally administered address (this is NOT the factory default)
+         *         .... ...0 .... .... .... .... = IG bit: Individual address (unicast)
+         *     Source: a2:c1:12:03:02:03 (a2:c1:12:03:02:03)
+         *         Address: a2:c1:12:03:02:03 (a2:c1:12:03:02:03)
+         *         .... ..1. .... .... .... .... = LG bit: Locally administered address (this is NOT the factory default)
+         *         .... ...0 .... .... .... .... = IG bit: Individual address (unicast)
+         *     Type: 802.1ad Provider Bridge (Q-in-Q) (0x88a8)
+         * IEEE 802.1ad, ID: 1
+         *     000. .... .... .... = Priority: 0
+         *     ...0 .... .... .... = DEI: 0
+         *     .... 0000 0000 0001 = ID: 1
+         *     Type: 802.1Q Virtual LAN (0x8100)
+         * 802.1Q Virtual LAN, PRI: 0, DEI: 0, ID: 1
+         *     000. .... .... .... = Priority: Best Effort (default) (0)
+         *     ...0 .... .... .... = DEI: Ineligible
+         *     .... 0000 0000 0001 = ID: 1
+         *     Type: IPv4 (0x0800)
+         * Internet Protocol Version 4, Src: 18.3.2.3, Dst: 18.3.1.100
+         *     0100 .... = Version: 4
+         *     .... 0101 = Header Length: 20 bytes (5)
+         *     Differentiated Services Field: 0x00 (DSCP: CS0, ECN: Not-ECT)
+         *         0000 00.. = Differentiated Services Codepoint: Default (0)
+         *         .... ..00 = Explicit Congestion Notification: Not ECN-Capable Transport (0)
+         *     Total Length: 40
+         *     Identification: 0x2706 (9990)
+         *     000. .... = Flags: 0x0
+         *         0... .... = Reserved bit: Not set
+         *         .0.. .... = Don't fragment: Not set
+         *         ..0. .... = More fragments: Not set
+         *     ...0 0000 0000 0000 = Fragment Offset: 0
+         *     Time to Live: 255
+         *     Protocol: TCP (6)
+         *     Header Checksum: 0x0000 incorrect, should be 0x6d5d(may be caused by "IP checksum offload"?)
+         *         [Expert Info (Error/Checksum): Bad checksum [should be 0x6d5d]]
+         *             [Bad checksum [should be 0x6d5d]]
+         *             [Severity level: Error]
+         *             [Group: Checksum]
+         *     [Header checksum status: Bad]
+         *     [Calculated Checksum: 0x6d5d]
+         *     Source Address: 18.3.2.3
+         *     Destination Address: 18.3.1.100
+         * Transmission Control Protocol, Src Port: 10000, Dst Port: 80, Seq: 108, Ack: 2921, Len: 0
+         *     Source Port: 10000
+         *     Destination Port: 80
+         *     [Stream index: 0]
+         *     [Conversation completeness: Incomplete (13)]
+         *     [TCP Segment Len: 0]
+         *     Sequence Number: 108    (relative sequence number)
+         *     Sequence Number (raw): 2180528890
+         *     [Next Sequence Number: 108    (relative sequence number)]
+         *     Acknowledgment Number: 2921    (relative ack number)
+         *     Acknowledgment number (raw): 3488984520
+         *     0101 .... = Header Length: 20 bytes (5)
+         *     Flags: 0x010 (ACK)
+         *         000. .... .... = Reserved: Not set
+         *         ...0 .... .... = Accurate ECN: Not set
+         *         .... 0... .... = Congestion Window Reduced: Not set
+         *         .... .0.. .... = ECN-Echo: Not set
+         *         .... ..0. .... = Urgent: Not set
+         *         .... ...1 .... = Acknowledgment: Set
+         *         .... .... 0... = Push: Not set
+         *         .... .... .0.. = Reset: Not set
+         *         .... .... ..0. = Syn: Not set
+         *         .... .... ...0 = Fin: Not set
+         *         [TCP Flags: ·······A····]
+         *     Window: 8192
+         *     [Calculated window size: 8192]
+         *     [Window size scaling factor: -2 (no window scaling used)]
+         *     Checksum: 0x2787 incorrect, should be 0x0657(maybe caused by "TCP checksum offload"?)
+         *         [Expert Info (Error/Checksum): Bad checksum [should be 0x0657]]
+         *             [Bad checksum [should be 0x0657]]
+         *             [Severity level: Error]
+         *             [Group: Checksum]
+         *     [Checksum Status: Bad]
+         *     [Calculated Checksum: 0x0657]
+         *     Urgent Pointer: 0
+         *     [Timestamps]
+         *         [Time since first frame in this TCP stream: 0.008335000 seconds]
+         *         [Time since previous frame in this TCP stream: 0.006757000 seconds]
+         */
+
+        let bytes = [
+            0xa2, 0xc1, 0x12, 0x03, 0x01, 0x64, 0xa2, 0xc1, 0x12, 0x03, 0x02, 0x03, 0x88, 0xa8,
+            0x00, 0x01, 0x81, 0x00, 0x00, 0x01, 0x08, 0x00, 0x45, 0x00, 0x00, 0x28, 0x27, 0x06,
+            0x00, 0x00, 0xff, 0x06, 0x00, 0x00, 0x12, 0x03, 0x02, 0x03, 0x12, 0x03, 0x01, 0x64,
+            0x27, 0x10, 0x00, 0x50, 0x81, 0xf8, 0x3a, 0xfa, 0xcf, 0xf5, 0xad, 0xc8, 0x50, 0x10,
+            0x20, 0x00, 0x27, 0x87, 0x00, 0x00,
+        ];
+
+        let mut packet = Packet::new(&bytes, bytes.len() as u32);
+        let result = packet.handle();
+        assert_eq!(result.is_ok(), true);
+
+        assert_eq!(packet.encapsulation.len(), 5);
+        assert_eq!(
+            packet.encapsulation[0],
+            Encapsulation::L2_ETH(
+                EthernetFrame {
+                    source_mac: MacAddress([0xa2, 0xc1, 0x12, 0x03, 0x02, 0x03]),
+                    dest_mac: MacAddress([0xa2, 0xc1, 0x12, 0x03, 0x01, 0x64]),
+                    ether_type: EtherType::QinQ,
+                },
+                &bytes[14..]
+            )
+        );
+        assert_eq!(
+            packet.encapsulation[1],
+            Encapsulation::L3_VLAN(
+                VlanHeader {
+                    priority_code_point: 0,
+                    drop_eligible_indicator: false,
+                    vlan_identifier: 1,
+                    ether_type: EtherType::VLAN,
+                },
+                &bytes[18..]
+            )
+        );
+        assert_eq!(
+            packet.encapsulation[2],
+            Encapsulation::L3_VLAN(
+                VlanHeader {
+                    priority_code_point: 0,
+                    drop_eligible_indicator: false,
+                    vlan_identifier: 1,
+                    ether_type: EtherType::IPv4,
+                },
+                &bytes[22..]
+            )
+        );
+        assert_eq!(
+            packet.encapsulation[3],
+            Encapsulation::L3_IPV4(
+                IPv4Header {
+                    version: 4,
+                    ihl: 20,
+                    tos: 0,
+                    length: 40,
+                    id: 9990,
+                    flags: 0x0,
+                    frag_offset: 0,
+                    ttl: 255,
+                    protocol: IPProtocol::TCP,
+                    checksum: 0x0000,
+                    source_address: Ipv4Addr::new(18, 3, 2, 3),
+                    dest_address: Ipv4Addr::new(18, 3, 1, 100),
+                },
+                &bytes[42..]
+            )
+        );
+        assert_eq!(
+            packet.encapsulation[4],
+            Encapsulation::L4_TCP(
+                TcpHeader {
+                    source_port: 10000,
+                    dest_port: 80,
+                    seq_num: 2180528890,
+                    ack_num: 3488984520,
+                    data_offset: 20,
+                    reserved: 0,
+                    flag_urg: false,
+                    flag_ack: true,
+                    flag_psh: false,
+                    flag_rst: false,
+                    flag_syn: false,
+                    flag_fin: false,
+                    window: 8192,
+                    checksum: 0x2787,
+                    urgent_ptr: 0,
+                    options: None,
+                },
+                &bytes[62..]
+            )
+        )
+    }
 }