2 files changed, 43 insertions, 3 deletions
diff --git a/src/packet/error.rs b/src/packet/error.rs
index f5dbf0a..b6b054b 100644
--- a/src/packet/error.rs
+++ b/src/packet/error.rs
@@ -26,7 +26,10 @@ pub enum PacketError {
 
     // L TUNNEL
     IncompleteGtpv1Header,
-    UnsupportGtpProtocol,
+    UnsupportGtpVersion,
+
+    IncompleteL2tpHeader,
+    UnsupportL2tpVersion,
 }
 
 impl core::fmt::Display for PacketError {
@@ -52,7 +55,9 @@ impl core::fmt::Display for PacketError {
             PacketError::IncompleteIcmpv6Header => write!(f, "Incomplete ICMPv6 Header"),
             // L TUNNEL
             PacketError::IncompleteGtpv1Header => write!(f, "Incomplete GTPv1 Header"),
-            PacketError::UnsupportGtpProtocol => write!(f, "Unsupport GTP Protocol"),
+            PacketError::UnsupportGtpVersion => write!(f, "Unsupport GTP Version"),
+            PacketError::IncompleteL2tpHeader => write!(f, "Incomplete L2TP Header"),
+            PacketError::UnsupportL2tpVersion => write!(f, "Unsupport L2TP Version"),
         }
     }
 }
diff --git a/src/packet/packet.rs b/src/packet/packet.rs
index e44968f..781d89b 100644
--- a/src/packet/packet.rs
+++ b/src/packet/packet.rs
@@ -8,6 +8,8 @@ use crate::protocol::icmpv6::Icmpv6Header;
 use crate::protocol::ip::IPProtocol;
 use crate::protocol::ipv4::Ipv4Header;
 use crate::protocol::ipv6::Ipv6Header;
+use crate::protocol::l2tp::L2tpHeader;
+use crate::protocol::l2tp::L2tpType;
 use crate::protocol::mpls::MplsHeader;
 use crate::protocol::mpls::PwEthHeader;
 use crate::protocol::tcp::TcpHeader;
@@ -32,6 +34,7 @@ pub enum Encapsulation<'a> {
     L4_ICMPV6(Icmpv6Header, &'a [u8]),
 
     LTUN_GTPV1_C(Gtpv1Header, &'a [u8]),
+    LTUN_L2TP(L2tpHeader, &'a [u8]),
 
     UNSUPPORTED(&'a [u8]),
 }
@@ -499,6 +502,8 @@ fn handle_udp<'a>(packet: &mut Packet<'a>, input: &'a [u8]) -> Result<(), Packet
         match dest_port {
             // GTP-U
             2152 => handle_gtpv1_c(packet, payload),
+            // L2TPv2
+            1701 => handle_l2tp(packet, payload),
             _ => Ok(()),
         }
     } else {
@@ -561,7 +566,37 @@ fn handle_gtpv1_c<'a>(packet: &mut Packet<'a>, input: &'a [u8]) -> Result<(), Pa
             return Err(PacketError::IncompleteGtpv1Header);
         }
         _ => {
-            return Err(PacketError::UnsupportGtpProtocol);
+            return Err(PacketError::UnsupportGtpVersion);
+        }
+    }
+}
+
+fn handle_l2tp<'a>(packet: &mut Packet<'a>, input: &'a [u8]) -> Result<(), PacketError> {
+    let result = L2tpHeader::decode(input);
+    match result {
+        Ok((payload, header)) => {
+            dbg!(&header);
+
+            let l2tp_type = header.flag_type;
+            packet
+                .encapsulation
+                .push(Encapsulation::LTUN_L2TP(header, payload));
+
+            match l2tp_type {
+                L2tpType::Control => {
+                    return Ok(());
+                }
+                L2tpType::Data => {
+                    // TODO handle PPP
+                    return Ok(());
+                }
+            }
+        }
+        Err(Incomplete(_)) => {
+            return Err(PacketError::IncompleteL2tpHeader);
+        }
+        _ => {
+            return Err(PacketError::UnsupportL2tpVersion);
         }
     }
 }