From 2766a4783cd56a941132c62230bd58014d04bf1b Mon Sep 17 00:00:00 2001 From: "Steven H. Wang" Date: Fri, 23 Jun 2017 22:43:55 +0000 Subject: [PATCH] Add VLAN get/set to Mac header --- framework/src/headers/mac.rs | 185 ++++++++++++++++++++++++++++++++--- 1 file changed, 172 insertions(+), 13 deletions(-) diff --git a/framework/src/headers/mac.rs b/framework/src/headers/mac.rs index e0daa9d155..da5bbb5d42 100644 --- a/framework/src/headers/mac.rs +++ b/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; + } + } }