VLAN dot1q and dot1ad parsing

framework/src/headers/mac.rs

@@ -66,24 +66,37 @@ impl Hash for MacAddress {
     }
 }
 
-/// A packet's MAC header.
+/// A packet's MAC header, with up to two VLAN tags.
 #[derive(Debug, Default)]
 #[repr(C, packed)]
 pub struct MacHeader {
     pub dst: MacAddress,
     pub src: MacAddress,
-    etype: u16,
+    etype0: u16, // 0x8100 (8021.Q), 0x88a8 (QinQ), or ethertype of the next header
+    tci1: u16,
+    etype1: u16, // if valid, 0x8100 (8021.Q) or ethertype of the next header
+    tci2: u16,
+    etype2: u16, // if valid, ethertype of next header
 }
 
 impl fmt::Display for MacHeader {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(
-            f,
-            "{} > {} 0x{:04x}",
-            self.src,
-            self.dst,
-            u16::from_be(self.etype)
-        )
+        let mut result: fmt::Result;
+        result =
+            write!(f,
+               "{} > {} etype=0x{:04x}",
+               self.src,
+               self.dst,
+               self.etype(),
+        );
+        if self.num_tags() >= 1 {
+            result = write!(f, " vid1=0x{:03x}", self.vid1());
+        }
+
+        if self.num_tags() >= 2 {
+            result = write!(f, " vid2=0x{:03x}", self.vid2())
+        }
+        return result;
     }
 }
 
@@ -98,7 +111,7 @@ impl EndOffset for MacHeader {
         if cfg!(feature = "performance") {
             HDR_SIZE
         } else {
-            match self.etype {
+            match self.etype0 {
                 0x8100 => HDR_SIZE_802_1Q,
                 0x9100 => HDR_SIZE_802_1AD,
                 _ => HDR_SIZE,
@@ -107,7 +120,6 @@ impl EndOffset for MacHeader {
     }
     #[inline]
     fn size() -> usize {
-        // The struct itself is always 20 bytes.
         HDR_SIZE
     }
 
@@ -127,14 +139,24 @@ impl MacHeader {
         Default::default()
     }
 
+    // Return the ethernet type of the next header.
     #[inline]
     pub fn etype(&self) -> u16 {
-        u16::from_be(self.etype)
+        match u16::from_be(self.etype0) {
+            0x8100 => u16::from_be(self.etype1),
+            0x88a8 => u16::from_be(self.etype2),
+            _ => u16::from_be(self.etype0),
+        }
     }
 
+    // Set the ethernet type of the next header.
     #[inline]
     pub fn set_etype(&mut self, etype: u16) {
-        self.etype = u16::to_be(etype)
+        match u16::from_be(self.etype0) {
+            0x8100 => self.etype1 = u16::to_be(etype),
+            0x88a8 => self.etype2 = u16::to_be(etype),
+            _ => self.etype0 = u16::to_be(etype),
+        }
     }
 
     #[inline]
@@ -144,4 +166,141 @@ impl MacHeader {
         self.src.copy_address(&self.dst);
         self.dst.copy_address(&src);
     }
+
+    // Return the number of VLAN tags.
+    #[inline]
+    pub fn num_tags(&self) -> u32 {
+        match u16::from_be(self.etype0) {
+            0x8100 => 1,
+            0x88a8 => 2,
+            _ => 0,
+        }
+    }
+
+    // Return the first VLAN tag.
+    #[inline]
+    pub fn vid1(&self) -> u16 {
+        assert!(self.num_tags() >= 1);
+        return u16::from_be(self.tci1) & 0x0FFF;
+    }
+
+    // Return the second VLAN tag.
+    #[inline]
+    pub fn vid2(&self) -> u16 {
+        assert!(self.num_tags() >= 2);
+        return u16::from_be(self.tci2) & 0x0FFF;
+    }
+
+    // Set the first VLAN tag.
+    #[inline]
+    pub fn set_vid1(&mut self, vid: u16) {
+        assert!(self.num_tags() >= 1);
+        assert!(vid <= 0xFFF);
+        let pri = (self.tci1 & 0x0F) << 4;
+        self.tci1 = pri | u16::to_be(vid);
+    }
+
+    // Set the second VLAN tag
+    #[inline]
+    pub fn set_vid2(&mut self, vid: u16) {
+        assert!(self.num_tags() >= 1);
+        assert!(vid <= 0xFFF);
+        let pri = (self.tci2 & 0x0F) << 4;
+        self.tci2 = pri | u16::to_be(vid);
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn build_1tagged(pri: u16, vid: u16) -> MacHeader {
+        assert!(pri <= 0xF, "test config failure");
+        assert!(vid <= 0xFFF, "test config failure");
+        let tci = u16::to_be((pri << 12) | vid);
+        MacHeader {
+            dst: MacAddress { addr: [0, 1, 2, 3, 4, 5] },
+            src: MacAddress { addr: [5, 4, 3, 2, 1, 0] },
+            etype0: u16::to_be(0x8100),
+            tci1: tci,
+            etype1: u16::to_be(0x8000), // IPv4
+            tci2: 0,
+            etype2: 0,
+        }
+    }
+
+    #[test]
+    fn test_1tagged() {
+        let pairs: Vec<(u16, u16)> = vec![(3, 13), (6, 100), (0, 0), (0, 0xFFF), (0xF, 0xFFF)];
+        let mut new_vid: u16 = 0x00E;
+        for &(pri, vid) in &pairs {
+            let mut mac = build_1tagged(pri, vid);
+            assert_eq!(1, mac.num_tags());
+            assert_eq!(vid, mac.vid1());
+            assert_eq!(0x8000, mac.etype());
+
+            mac.set_vid1(new_vid);
+            assert_eq!(new_vid, mac.vid1());
+            assert_eq!(0x8000, mac.etype());
+
+            mac.set_etype(0x1131);
+            assert_eq!(new_vid, mac.vid1());
+            assert_eq!(0x1131, mac.etype());
+            assert_eq!(0x1131, u16::from_be(mac.etype1));
+            assert_eq!(1, mac.num_tags());
+
+            new_vid += 0x031;
+        }
+    }
+
+    fn build_2tagged(pri: u16, vid: u16, pri2: u16, vid2: u16) -> MacHeader {
+        assert!(pri <= 0xF, "test config failure");
+        assert!(vid <= 0xFFF, "test config failure");
+        assert!(pri2 <= 0xF, "test config failure");
+        assert!(vid2 <= 0xFFF, "test config failure");
+        let tci = u16::to_be((pri << 12) | vid);
+        let tci2 = u16::to_be((pri2 << 12) | vid2);
+        MacHeader {
+            dst: MacAddress { addr: [0, 1, 2, 3, 4, 5] },
+            src: MacAddress { addr: [5, 4, 3, 2, 1, 0] },
+            etype0: u16::to_be(0x88a8),
+            tci1: tci,
+            etype1: u16::to_be(0x8100),
+            tci2: tci2,
+            etype2: u16::to_be(0x8000), // IPv4
+        }
+    }
+
+    #[test]
+    fn test_2tagged() {
+        let quads: Vec<(u16, u16, u16, u16)> = vec![
+            (3, 13, 5, 16),
+            (6, 100, 3, 13),
+            (0, 0, 1, 1),
+            (0, 0xFFF, 3, 0xFFF),
+            (0xF, 0xFFF, 0xF, 0xFFF),
+        ];
+        let mut new_vid: u16 = 0x00E;
+        for &(pri, vid, pri2, vid2) in &quads {
+            let mut mac = build_2tagged(pri, vid, pri2, vid2);
+            assert_eq!(2, mac.num_tags());
+            assert_eq!(vid, mac.vid1());
+            assert_eq!(vid2, mac.vid2());
+            assert_eq!(0x8000, mac.etype());
+
+            mac.set_vid1(new_vid);
+            mac.set_vid2(new_vid + 3);
+            assert_eq!(new_vid, mac.vid1());
+            assert_eq!(new_vid + 3, mac.vid2());
+            assert_eq!(0x8000, mac.etype());
+
+            mac.set_etype(0x1131);
+            assert_eq!(new_vid, mac.vid1());
+            assert_eq!(0x1131, mac.etype());
+            assert_eq!(0x1131, u16::from_be(mac.etype2));
+            assert_eq!(2, mac.num_tags());
+
+            new_vid += 0x031;
+        }
+    }
 }