1 files changed, 23 insertions, 0 deletions
diff --git a/src/wireguard/router/ip.rs b/src/wireguard/router/ip.rs
index e66144f..532c512 100644
--- a/src/wireguard/router/ip.rs
+++ b/src/wireguard/router/ip.rs
@@ -1,5 +1,8 @@
+use std::mem;
+
 use byteorder::BigEndian;
 use zerocopy::byteorder::U16;
+use zerocopy::LayoutVerified;
 use zerocopy::{AsBytes, FromBytes};
 
 pub const VERSION_IP4: u8 = 4;
@@ -24,3 +27,23 @@ pub struct IPv6Header {
     pub f_source: [u8; 16],
     pub f_destination: [u8; 16],
 }
+
+#[inline(always)]
+pub fn inner_length(packet: &[u8]) -> Option<usize> {
+    match packet.get(0)? >> 4 {
+        VERSION_IP4 => {
+            let (header, _): (LayoutVerified<&[u8], IPv4Header>, _) =
+                LayoutVerified::new_from_prefix(packet)?;
+
+            Some(header.f_total_len.get() as usize)
+        }
+        VERSION_IP6 => {
+            // check length and cast to IPv6 header
+            let (header, _): (LayoutVerified<&[u8], IPv6Header>, _) =
+                LayoutVerified::new_from_prefix(packet)?;
+
+            Some(header.f_len.get() as usize + mem::size_of::<IPv6Header>())
+        }
+        _ => None,
+    }
+}