diff --git a/Cargo.toml b/Cargo.toml
index a8ad41200..1420234fc 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -60,6 +60,7 @@ defmt = ["dep:defmt", "heapless/defmt-03"]
"proto-ipsec" = ["proto-ipsec-ah", "proto-ipsec-esp"]
"proto-ipsec-ah" = []
"proto-ipsec-esp" = []
+"proto-vlan" = ["medium-ethernet"]
"socket" = []
"socket-raw" = ["socket"]
diff --git a/ci.sh b/ci.sh
index ec20cc70e..6ec164a32 100755
--- a/ci.sh
+++ b/ci.sh
@@ -30,6 +30,7 @@ FEATURES_TEST=(
"std,medium-ieee802154,proto-rpl,proto-sixlowpan,proto-sixlowpan-fragmentation,socket-udp"
"std,medium-ip,proto-ipv4,proto-ipv6,socket-tcp,socket-udp"
"std,medium-ethernet,medium-ip,medium-ieee802154,proto-ipv4,proto-ipv6,socket-raw,socket-udp,socket-tcp,socket-icmp,socket-dns,async"
+ "std,medium-ethernet,medium-ip,medium-ieee802154,proto-ipv4,proto-ipv6,proto-vlan,socket-raw,socket-udp,socket-tcp,socket-icmp,socket-dns,async"
"std,medium-ieee802154,medium-ip,proto-ipv4,socket-raw"
"std,medium-ethernet,proto-ipv4,proto-ipsec,socket-raw"
)
@@ -39,7 +40,7 @@ FEATURES_TEST_NIGHTLY=(
)
FEATURES_CHECK=(
- "medium-ip,medium-ethernet,medium-ieee802154,proto-ipv6,proto-ipv6,proto-igmp,proto-dhcpv4,proto-ipsec,socket-raw,socket-udp,socket-tcp,socket-icmp,socket-dns,async"
+ "medium-ip,medium-ethernet,medium-ieee802154,proto-ipv6,proto-ipv6,proto-igmp,proto-dhcpv4,proto-ipsec,proto-vlan,socket-raw,socket-udp,socket-tcp,socket-icmp,socket-dns,async"
"defmt,medium-ip,medium-ethernet,proto-ipv6,proto-ipv6,proto-igmp,proto-dhcpv4,socket-raw,socket-udp,socket-tcp,socket-icmp,socket-dns,async"
"defmt,alloc,medium-ip,medium-ethernet,proto-ipv6,proto-ipv6,proto-igmp,proto-dhcpv4,socket-raw,socket-udp,socket-tcp,socket-icmp,socket-dns,async"
)
diff --git a/src/iface/interface/ethernet.rs b/src/iface/interface/ethernet.rs
index 4d29faa11..7aa9b3eb1 100644
--- a/src/iface/interface/ethernet.rs
+++ b/src/iface/interface/ethernet.rs
@@ -18,22 +18,80 @@ impl InterfaceInner {
return None;
}
- match eth_frame.ethertype() {
+ #[cfg(feature = "proto-vlan")]
+ if let Some(vlan_config) = &self.vlan_config {
+ // Drop all frames without VLAN header
+ match vlan_config.outer_vlan_id {
+ Some(_) if eth_frame.ethertype() != EthernetProtocol::VlanOuter => return None,
+ None if eth_frame.ethertype() != EthernetProtocol::VlanInner => return None,
+ _ => (),
+ }
+ }
+
+ self.handle_ethertype(
+ sockets,
+ meta,
+ eth_frame.payload(),
+ eth_frame.ethertype(),
+ fragments,
+ )
+ }
+
+ fn handle_ethertype<'frame>(
+ &mut self,
+ sockets: &mut SocketSet,
+ meta: crate::phy::PacketMeta,
+ payload: &'frame [u8],
+ ethertype: EthernetProtocol,
+ fragments: &'frame mut FragmentsBuffer,
+ ) -> Option<EthernetPacket<'frame>> {
+ match ethertype {
#[cfg(feature = "proto-ipv4")]
- EthernetProtocol::Arp => self.process_arp(self.now, ð_frame),
+ EthernetProtocol::Arp => self.process_arp(self.now, payload),
#[cfg(feature = "proto-ipv4")]
EthernetProtocol::Ipv4 => {
- let ipv4_packet = check!(Ipv4Packet::new_checked(eth_frame.payload()));
+ let ipv4_packet = check!(Ipv4Packet::new_checked(payload));
self.process_ipv4(sockets, meta, &ipv4_packet, fragments)
.map(EthernetPacket::Ip)
}
#[cfg(feature = "proto-ipv6")]
EthernetProtocol::Ipv6 => {
- let ipv6_packet = check!(Ipv6Packet::new_checked(eth_frame.payload()));
+ let ipv6_packet = check!(Ipv6Packet::new_checked(payload));
self.process_ipv6(sockets, meta, &ipv6_packet)
.map(EthernetPacket::Ip)
}
+ #[cfg(feature = "proto-vlan")]
+ EthernetProtocol::VlanInner | EthernetProtocol::VlanOuter => match &self.vlan_config {
+ Some(vlan_config) => {
+ let vlan_packet = check!(VlanPacket::new_checked(payload));
+ if ethertype == EthernetProtocol::VlanOuter
+ && (vlan_config.outer_vlan_id.is_none()
+ || !matches!(
+ vlan_config.outer_vlan_id,
+ Some(vid) if vid == vlan_packet.vlan_identifier()
+ )
+ || vlan_packet.ethertype() != EthernetProtocol::VlanInner)
+ {
+ return None;
+ }
+ if ethertype == EthernetProtocol::VlanInner
+ && (vlan_packet.ethertype() == EthernetProtocol::VlanInner
+ || vlan_packet.ethertype() == EthernetProtocol::VlanOuter
+ || vlan_packet.vlan_identifier() != vlan_config.inner_vlan_id)
+ {
+ return None;
+ }
+ return self.handle_ethertype(
+ sockets,
+ meta,
+ &payload[VlanPacket::<&[u8]>::header_len()..],
+ vlan_packet.ethertype(),
+ fragments,
+ );
+ }
+ None => None,
+ },
// Drop all other traffic.
_ => None,
}
diff --git a/src/iface/interface/ipv4.rs b/src/iface/interface/ipv4.rs
index 3a5a864ee..083cc9ecc 100644
--- a/src/iface/interface/ipv4.rs
+++ b/src/iface/interface/ipv4.rs
@@ -230,9 +230,9 @@ impl InterfaceInner {
pub(super) fn process_arp<'frame>(
&mut self,
timestamp: Instant,
- eth_frame: &EthernetFrame<&'frame [u8]>,
+ payload: &'frame [u8],
) -> Option<EthernetPacket<'frame>> {
- let arp_packet = check!(ArpPacket::new_checked(eth_frame.payload()));
+ let arp_packet = check!(ArpPacket::new_checked(payload));
let arp_repr = check!(ArpRepr::parse(&arp_packet));
match arp_repr {
@@ -407,6 +407,14 @@ impl InterfaceInner {
#[cfg(feature = "medium-ethernet")]
if matches!(caps.medium, Medium::Ethernet) {
tx_len += EthernetFrame::<&[u8]>::header_len();
+
+ #[cfg(feature = "proto-vlan")]
+ {
+ tx_len += self
+ .vlan_config
+ .map(|vlan_config| vlan_config.get_additional_header_length())
+ .unwrap_or(0);
+ }
}
// Emit function for the Ethernet header.
@@ -418,11 +426,23 @@ impl InterfaceInner {
frame.set_src_addr(src_addr);
frame.set_dst_addr(frag.ipv4.dst_hardware_addr);
- match repr.version() {
+ let ip_ethertype = match repr.version() {
#[cfg(feature = "proto-ipv4")]
- IpVersion::Ipv4 => frame.set_ethertype(EthernetProtocol::Ipv4),
+ IpVersion::Ipv4 => EthernetProtocol::Ipv4,
#[cfg(feature = "proto-ipv6")]
- IpVersion::Ipv6 => frame.set_ethertype(EthernetProtocol::Ipv6),
+ IpVersion::Ipv6 => EthernetProtocol::Ipv6,
+ };
+
+ #[cfg(feature = "proto-vlan")]
+ if let Some(vlan_config) = &self.vlan_config {
+ frame.set_ethertype(vlan_config.get_outer_ethertype());
+ vlan_config.emit_to_payload(frame.payload_mut(), ip_ethertype);
+ } else {
+ frame.set_ethertype(ip_ethertype);
+ }
+ #[cfg(not(feature = "proto-vlan"))]
+ {
+ frame.set_ethertype(ip_ethertype);
}
};
@@ -430,7 +450,19 @@ impl InterfaceInner {
#[cfg(feature = "medium-ethernet")]
if matches!(self.caps.medium, Medium::Ethernet) {
emit_ethernet(&IpRepr::Ipv4(frag.ipv4.repr), tx_buffer);
- tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()..];
+ #[cfg(not(feature = "proto-vlan"))]
+ {
+ tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()..];
+ }
+ #[cfg(feature = "proto-vlan")]
+ {
+ tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()
+ + self
+ .vlan_config
+ .as_ref()
+ .map(|vlan_config| vlan_config.get_additional_header_length())
+ .unwrap_or(0)..];
+ }
}
let mut packet =
diff --git a/src/iface/interface/mod.rs b/src/iface/interface/mod.rs
index 00f46d07d..fa23f3701 100644
--- a/src/iface/interface/mod.rs
+++ b/src/iface/interface/mod.rs
@@ -97,6 +97,8 @@ pub struct InterfaceInner {
#[cfg(any(feature = "medium-ethernet", feature = "medium-ieee802154"))]
neighbor_cache: NeighborCache,
hardware_addr: HardwareAddress,
+ #[cfg(feature = "proto-vlan")]
+ vlan_config: Option<VlanConfig>,
#[cfg(feature = "medium-ieee802154")]
sequence_no: u8,
#[cfg(feature = "medium-ieee802154")]
@@ -136,6 +138,9 @@ pub struct Config {
/// Creating the interface panics if the address is not unicast.
pub hardware_addr: HardwareAddress,
+ #[cfg(feature = "proto-vlan")]
+ pub vlan_config: Option<VlanConfig>,
+
/// Set the IEEE802.15.4 PAN ID the interface will use.
///
/// **NOTE**: we use the same PAN ID for destination and source.
@@ -148,6 +153,8 @@ impl Config {
Config {
random_seed: 0,
hardware_addr,
+ #[cfg(feature = "proto-vlan")]
+ vlan_config: None,
#[cfg(feature = "medium-ieee802154")]
pan_id: None,
}
@@ -220,6 +227,8 @@ impl Interface {
now,
caps,
hardware_addr: config.hardware_addr,
+ #[cfg(feature = "proto-vlan")]
+ vlan_config: config.vlan_config,
ip_addrs: Vec::new(),
#[cfg(feature = "proto-ipv4")]
any_ip: false,
@@ -859,12 +868,43 @@ impl InterfaceInner {
} => target_hardware_addr,
};
- self.dispatch_ethernet(tx_token, arp_repr.buffer_len(), |mut frame| {
+ #[cfg(feature = "proto-vlan")]
+ let vlan_config = self.vlan_config.as_ref().copied();
+
+ #[cfg(feature = "proto-vlan")]
+ let buffer_len = arp_repr.buffer_len()
+ + vlan_config
+ .as_ref()
+ .map(|vlan_config| vlan_config.get_additional_header_length())
+ .unwrap_or(0);
+ #[cfg(not(feature = "proto-vlan"))]
+ let buffer_len = arp_repr.buffer_len();
+
+ self.dispatch_ethernet(tx_token, buffer_len, |mut frame| {
frame.set_dst_addr(dst_hardware_addr);
- frame.set_ethertype(EthernetProtocol::Arp);
- let mut packet = ArpPacket::new_unchecked(frame.payload_mut());
- arp_repr.emit(&mut packet);
+ #[cfg(feature = "proto-vlan")]
+ {
+ let mut packet = if let Some(vlan_config) = vlan_config {
+ frame.set_ethertype(vlan_config.get_outer_ethertype());
+ vlan_config.emit_to_payload(frame.payload_mut(), EthernetProtocol::Arp);
+
+ ArpPacket::new_unchecked(
+ &mut frame.payload_mut()
+ [vlan_config.get_additional_header_length()..],
+ )
+ } else {
+ frame.set_ethertype(EthernetProtocol::Arp);
+ ArpPacket::new_unchecked(frame.payload_mut())
+ };
+ arp_repr.emit(&mut packet);
+ }
+ #[cfg(not(feature = "proto-vlan"))]
+ {
+ frame.set_ethertype(EthernetProtocol::Arp);
+ let mut packet = ArpPacket::new_unchecked(frame.payload_mut());
+ arp_repr.emit(&mut packet);
+ }
})
}
EthernetPacket::Ip(packet) => {
@@ -994,14 +1034,44 @@ impl InterfaceInner {
target_protocol_addr: dst_addr,
};
- if let Err(e) =
- self.dispatch_ethernet(tx_token, arp_repr.buffer_len(), |mut frame| {
- frame.set_dst_addr(EthernetAddress::BROADCAST);
- frame.set_ethertype(EthernetProtocol::Arp);
+ #[cfg(feature = "proto-vlan")]
+ let vlan_config = self.vlan_config.as_ref().copied();
- arp_repr.emit(&mut ArpPacket::new_unchecked(frame.payload_mut()))
- })
- {
+ #[cfg(feature = "proto-vlan")]
+ let buffer_len = arp_repr.buffer_len()
+ + vlan_config
+ .as_ref()
+ .map(|vlan_config| vlan_config.get_additional_header_length())
+ .unwrap_or(0);
+ #[cfg(not(feature = "proto-vlan"))]
+ let buffer_len = arp_repr.buffer_len();
+
+ if let Err(e) = self.dispatch_ethernet(tx_token, buffer_len, |mut frame| {
+ frame.set_dst_addr(EthernetAddress::BROADCAST);
+
+ #[cfg(feature = "proto-vlan")]
+ {
+ let mut packet = if let Some(vlan_config) = vlan_config {
+ frame.set_ethertype(vlan_config.get_outer_ethertype());
+ vlan_config.emit_to_payload(frame.payload_mut(), EthernetProtocol::Arp);
+
+ ArpPacket::new_unchecked(
+ &mut frame.payload_mut()
+ [vlan_config.get_additional_header_length()..],
+ )
+ } else {
+ frame.set_ethertype(EthernetProtocol::Arp);
+ ArpPacket::new_unchecked(frame.payload_mut())
+ };
+ arp_repr.emit(&mut packet);
+ }
+ #[cfg(not(feature = "proto-vlan"))]
+ {
+ frame.set_ethertype(EthernetProtocol::Arp);
+ let mut packet = ArpPacket::new_unchecked(frame.payload_mut());
+ arp_repr.emit(&mut packet);
+ }
+ }) {
net_debug!("Failed to dispatch ARP request: {:?}", e);
return Err(DispatchError::NeighborPending);
}
@@ -1094,6 +1164,14 @@ impl InterfaceInner {
#[cfg(feature = "medium-ethernet")]
if matches!(self.caps.medium, Medium::Ethernet) {
total_len = EthernetFrame::<&[u8]>::buffer_len(total_len);
+ #[cfg(feature = "proto-vlan")]
+ {
+ total_len += self
+ .vlan_config
+ .as_ref()
+ .map(|vlan_config| vlan_config.get_additional_header_length())
+ .unwrap_or(0);
+ }
}
// If the medium is Ethernet, then we need to retrieve the destination hardware address.
@@ -1122,11 +1200,23 @@ impl InterfaceInner {
frame.set_src_addr(src_addr);
frame.set_dst_addr(dst_hardware_addr);
- match repr.version() {
+ let ip_ethertype = match repr.version() {
#[cfg(feature = "proto-ipv4")]
- IpVersion::Ipv4 => frame.set_ethertype(EthernetProtocol::Ipv4),
+ IpVersion::Ipv4 => EthernetProtocol::Ipv4,
#[cfg(feature = "proto-ipv6")]
- IpVersion::Ipv6 => frame.set_ethertype(EthernetProtocol::Ipv6),
+ IpVersion::Ipv6 => EthernetProtocol::Ipv6,
+ };
+
+ #[cfg(feature = "proto-vlan")]
+ if let Some(vlan_config) = &self.vlan_config {
+ frame.set_ethertype(vlan_config.get_outer_ethertype());
+ vlan_config.emit_to_payload(frame.payload_mut(), ip_ethertype);
+ } else {
+ frame.set_ethertype(ip_ethertype);
+ }
+ #[cfg(not(feature = "proto-vlan"))]
+ {
+ frame.set_ethertype(ip_ethertype);
}
Ok(())
@@ -1202,7 +1292,22 @@ impl InterfaceInner {
#[cfg(feature = "medium-ethernet")]
if matches!(self.caps.medium, Medium::Ethernet) {
emit_ethernet(&ip_repr, tx_buffer)?;
- tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()..];
+ #[cfg(not(feature = "proto-vlan"))]
+ {
+ tx_buffer =
+ &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()..];
+ }
+ #[cfg(feature = "proto-vlan")]
+ {
+ tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()
+ + self
+ .vlan_config
+ .as_ref()
+ .map(|vlan_config| {
+ vlan_config.get_additional_header_length()
+ })
+ .unwrap_or(0)..];
+ }
}
// Change the offset for the next packet.
@@ -1229,7 +1334,21 @@ impl InterfaceInner {
#[cfg(feature = "medium-ethernet")]
if matches!(self.caps.medium, Medium::Ethernet) {
emit_ethernet(&ip_repr, tx_buffer)?;
- tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()..];
+ #[cfg(not(feature = "proto-vlan"))]
+ {
+ tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()..];
+ }
+ #[cfg(feature = "proto-vlan")]
+ {
+ tx_buffer = &mut tx_buffer[EthernetFrame::<&[u8]>::header_len()
+ + self
+ .vlan_config
+ .as_ref()
+ .map(|vlan_config| {
+ vlan_config.get_additional_header_length()
+ })
+ .unwrap_or(0)..];
+ }
}
emit_ip(&ip_repr, tx_buffer);
diff --git a/src/iface/interface/tests/ipv4.rs b/src/iface/interface/tests/ipv4.rs
index 316a9d58a..39fcfddb4 100644
--- a/src/iface/interface/tests/ipv4.rs
+++ b/src/iface/interface/tests/ipv4.rs
@@ -2,7 +2,7 @@ use super::*;
#[rstest]
#[case(Medium::Ethernet)]
-#[cfg(feature = "medium-ethernet")]
+#[cfg(all(feature = "medium-ethernet", not(feature = "proto-vlan")))]
fn test_any_ip_accept_arp(#[case] medium: Medium) {
let mut buffer = [0u8; 64];
#[allow(non_snake_case)]
@@ -58,6 +58,91 @@ fn test_any_ip_accept_arp(#[case] medium: Medium) {
.is_some());
}
+#[rstest]
+#[case(Medium::Ethernet)]
+#[cfg(feature = "proto-vlan")]
+fn test_any_ip_accept_arp_vlan(#[case] medium: Medium) {
+ let mut buffer = [0u8; 64];
+ #[allow(non_snake_case)]
+ fn ETHERNET_FRAME_ARP(buffer: &mut [u8]) -> &[u8] {
+ let ethernet_repr = EthernetRepr {
+ src_addr: EthernetAddress::from_bytes(&[0x02, 0x02, 0x02, 0x02, 0x02, 0x03]),
+ dst_addr: EthernetAddress::from_bytes(&[0x02, 0x02, 0x02, 0x02, 0x02, 0x02]),
+ ethertype: EthernetProtocol::VlanOuter,
+ };
+ let outer_vlan_repr = VlanRepr {
+ vlan_identifier: 200,
+ drop_eligible_indicator: false,
+ priority_code_point: Pcp::Be,
+ ethertype: EthernetProtocol::VlanInner,
+ };
+ let inner_vlan_repr = VlanRepr {
+ vlan_identifier: 100,
+ drop_eligible_indicator: false,
+ priority_code_point: Pcp::Be,
+ ethertype: EthernetProtocol::Arp,
+ };
+ let frame_repr = ArpRepr::EthernetIpv4 {
+ operation: ArpOperation::Request,
+ source_hardware_addr: EthernetAddress::from_bytes(&[
+ 0x02, 0x02, 0x02, 0x02, 0x02, 0x03,
+ ]),
+ source_protocol_addr: Ipv4Address::from_bytes(&[192, 168, 1, 2]),
+ target_hardware_addr: EthernetAddress::from_bytes(&[
+ 0x02, 0x02, 0x02, 0x02, 0x02, 0x02,
+ ]),
+ target_protocol_addr: Ipv4Address::from_bytes(&[192, 168, 1, 3]),
+ };
+ let mut frame = EthernetFrame::new_unchecked(&mut buffer[..]);
+ ethernet_repr.emit(&mut frame);
+
+ let mut frame = VlanPacket::new_unchecked(&mut buffer[ethernet_repr.buffer_len()..]);
+ outer_vlan_repr.emit(&mut frame);
+
+ let mut frame = VlanPacket::new_unchecked(
+ &mut buffer[ethernet_repr.buffer_len() + outer_vlan_repr.buffer_len()..],
+ );
+ inner_vlan_repr.emit(&mut frame);
+
+ let mut frame = ArpPacket::new_unchecked(
+ &mut buffer[ethernet_repr.buffer_len()
+ + outer_vlan_repr.buffer_len()
+ + inner_vlan_repr.buffer_len()..],
+ );
+ frame_repr.emit(&mut frame);
+
+ &buffer[..ethernet_repr.buffer_len()
+ + frame_repr.buffer_len()
+ + outer_vlan_repr.buffer_len()
+ + inner_vlan_repr.buffer_len()]
+ }
+
+ let (mut iface, mut sockets, _) = setup(medium);
+
+ assert!(iface
+ .inner
+ .process_ethernet(
+ &mut sockets,
+ PacketMeta::default(),
+ ETHERNET_FRAME_ARP(buffer.as_mut()),
+ &mut iface.fragments,
+ )
+ .is_none());
+
+ // Accept any IP address
+ iface.set_any_ip(true);
+
+ assert!(iface
+ .inner
+ .process_ethernet(
+ &mut sockets,
+ PacketMeta::default(),
+ ETHERNET_FRAME_ARP(buffer.as_mut()),
+ &mut iface.fragments,
+ )
+ .is_some());
+}
+
#[rstest]
#[case(Medium::Ip)]
#[cfg(feature = "medium-ip")]
@@ -406,7 +491,7 @@ fn test_handle_ipv4_broadcast(#[case] medium: Medium) {
#[rstest]
#[case(Medium::Ethernet)]
-#[cfg(feature = "medium-ethernet")]
+#[cfg(all(feature = "medium-ethernet", not(feature = "proto-vlan")))]
fn test_handle_valid_arp_request(#[case] medium: Medium) {
let (mut iface, mut sockets, _device) = setup(medium);
@@ -512,7 +597,7 @@ fn test_handle_other_arp_request(#[case] medium: Medium) {
#[rstest]
#[case(Medium::Ethernet)]
-#[cfg(feature = "medium-ethernet")]
+#[cfg(all(feature = "medium-ethernet", not(feature = "proto-vlan")))]
fn test_arp_flush_after_update_ip(#[case] medium: Medium) {
let (mut iface, mut sockets, _device) = setup(medium);
@@ -676,7 +761,22 @@ fn test_handle_igmp(#[case] medium: Medium) {
#[cfg(feature = "medium-ethernet")]
Medium::Ethernet => {
let eth_frame = EthernetFrame::new_checked(frame).ok()?;
- Ipv4Packet::new_checked(eth_frame.payload()).ok()?
+ #[cfg(feature = "proto-vlan")]
+ let ipv4_payload = {
+ let vlan_packet = VlanPacket::new_checked(eth_frame.payload()).ok()?;
+ let num_vlan_headers =
+ if vlan_packet.ethertype() == EthernetProtocol::VlanInner {
+ 2
+ } else {
+ 1
+ };
+ ð_frame.payload()
+ [VlanPacket::<&[u8]>::header_len() * num_vlan_headers..]
+ };
+ #[cfg(not(feature = "proto-vlan"))]
+ let ipv4_payload = eth_frame.payload();
+
+ Ipv4Packet::new_checked(ipv4_payload).ok()?
}
#[cfg(feature = "medium-ip")]
Medium::Ip => Ipv4Packet::new_checked(&frame[..]).ok()?,
diff --git a/src/iface/interface/tests/ipv6.rs b/src/iface/interface/tests/ipv6.rs
index f7debda9f..97cbe0789 100644
--- a/src/iface/interface/tests/ipv6.rs
+++ b/src/iface/interface/tests/ipv6.rs
@@ -678,7 +678,7 @@ fn ndsic_neighbor_advertisement_ieee802154(#[case] medium: Medium) {
#[rstest]
#[case(Medium::Ethernet)]
-#[cfg(feature = "medium-ethernet")]
+#[cfg(all(feature = "medium-ethernet", not(feature = "proto-vlan")))]
fn test_handle_valid_ndisc_request(#[case] medium: Medium) {
let (mut iface, mut sockets, _device) = setup(medium);
diff --git a/src/tests.rs b/src/tests.rs
index ec026ab64..a4279b8cd 100644
--- a/src/tests.rs
+++ b/src/tests.rs
@@ -4,7 +4,8 @@ use crate::wire::*;
pub(crate) fn setup<'a>(medium: Medium) -> (Interface, SocketSet<'a>, TestingDevice) {
let mut device = TestingDevice::new(medium);
- let config = Config::new(match medium {
+ #[allow(unused_mut)]
+ let mut config = Config::new(match medium {
#[cfg(feature = "medium-ethernet")]
Medium::Ethernet => {
HardwareAddress::Ethernet(EthernetAddress([0x02, 0x02, 0x02, 0x02, 0x02, 0x02]))
@@ -17,6 +18,14 @@ pub(crate) fn setup<'a>(medium: Medium) -> (Interface, SocketSet<'a>, TestingDev
])),
});
+ #[cfg(feature = "proto-vlan")]
+ {
+ config.vlan_config = Some(VlanConfig {
+ inner_vlan_id: 100,
+ outer_vlan_id: Some(200),
+ });
+ }
+
let mut iface = Interface::new(config, &mut device, Instant::ZERO);
#[cfg(feature = "proto-ipv4")]
diff --git a/src/wire/ethernet.rs b/src/wire/ethernet.rs
index 110e3d37b..3c58e469f 100644
--- a/src/wire/ethernet.rs
+++ b/src/wire/ethernet.rs
@@ -8,7 +8,9 @@ enum_with_unknown! {
pub enum EtherType(u16) {
Ipv4 = 0x0800,
Arp = 0x0806,
- Ipv6 = 0x86DD
+ Ipv6 = 0x86DD,
+ VlanInner = 0x8100,
+ VlanOuter = 0x88A8
}
}
@@ -18,6 +20,8 @@ impl fmt::Display for EtherType {
EtherType::Ipv4 => write!(f, "IPv4"),
EtherType::Ipv6 => write!(f, "IPv6"),
EtherType::Arp => write!(f, "ARP"),
+ EtherType::VlanInner => write!(f, "Inner VLAN"),
+ EtherType::VlanOuter => write!(f, "Outer VLAN"),
EtherType::Unknown(id) => write!(f, "0x{id:04x}"),
}
}
@@ -223,39 +227,62 @@ impl<T: AsRef<[u8]>> fmt::Display for Frame<T> {
use crate::wire::pretty_print::{PrettyIndent, PrettyPrint};
-impl<T: AsRef<[u8]>> PrettyPrint for Frame<T> {
- fn pretty_print(
- buffer: &dyn AsRef<[u8]>,
+impl<T: AsRef<[u8]>> Frame<T> {
+ fn pretty_print_ethertype(
+ payload: &dyn AsRef<[u8]>,
f: &mut fmt::Formatter,
indent: &mut PrettyIndent,
+ ethertype: EtherType,
) -> fmt::Result {
- let frame = match Frame::new_checked(buffer) {
- Err(err) => return write!(f, "{indent}({err})"),
- Ok(frame) => frame,
- };
- write!(f, "{indent}{frame}")?;
-
- match frame.ethertype() {
+ match ethertype {
#[cfg(feature = "proto-ipv4")]
EtherType::Arp => {
indent.increase(f)?;
- super::ArpPacket::<&[u8]>::pretty_print(&frame.payload(), f, indent)
+ super::ArpPacket::<&[u8]>::pretty_print(&payload, f, indent)
}
#[cfg(feature = "proto-ipv4")]
EtherType::Ipv4 => {
indent.increase(f)?;
- super::Ipv4Packet::<&[u8]>::pretty_print(&frame.payload(), f, indent)
+ super::Ipv4Packet::<&[u8]>::pretty_print(&payload, f, indent)
}
#[cfg(feature = "proto-ipv6")]
EtherType::Ipv6 => {
indent.increase(f)?;
- super::Ipv6Packet::<&[u8]>::pretty_print(&frame.payload(), f, indent)
+ super::Ipv6Packet::<&[u8]>::pretty_print(&payload, f, indent)
+ }
+ #[cfg(feature = "proto-vlan")]
+ EtherType::VlanOuter | EtherType::VlanInner => {
+ indent.increase(f)?;
+ super::VlanPacket::<&[u8]>::pretty_print(&payload, f, indent)?;
+ let vlan_packet = super::VlanPacket::new_unchecked(&payload);
+ Frame::<&[u8]>::pretty_print_ethertype(
+ &&payload.as_ref()[super::VlanPacket::<&[u8]>::header_len()..],
+ f,
+ indent,
+ vlan_packet.ethertype(),
+ )
}
_ => Ok(()),
}
}
}
+impl<T: AsRef<[u8]>> PrettyPrint for Frame<T> {
+ fn pretty_print(
+ buffer: &dyn AsRef<[u8]>,
+ f: &mut fmt::Formatter,
+ indent: &mut PrettyIndent,
+ ) -> fmt::Result {
+ let frame = match Frame::new_checked(buffer) {
+ Err(err) => return write!(f, "{indent}({err})"),
+ Ok(frame) => frame,
+ };
+ write!(f, "{indent}{frame}")?;
+
+ Frame::<&[u8]>::pretty_print_ethertype(&frame.payload(), f, indent, frame.ethertype())
+ }
+}
+
/// A high-level representation of an Internet Protocol version 4 packet header.
#[derive(Debug, PartialEq, Eq, Clone, Copy)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
diff --git a/src/wire/mod.rs b/src/wire/mod.rs
index e73e2305f..6bd534bcd 100644
--- a/src/wire/mod.rs
+++ b/src/wire/mod.rs
@@ -128,6 +128,8 @@ mod rpl;
mod sixlowpan;
mod tcp;
mod udp;
+#[cfg(feature = "proto-vlan")]
+mod vlan;
#[cfg(feature = "proto-ipsec-ah")]
mod ipsec_ah;
@@ -272,6 +274,9 @@ pub use self::dhcpv4::{
MAX_DNS_SERVER_COUNT as DHCP_MAX_DNS_SERVER_COUNT, SERVER_PORT as DHCP_SERVER_PORT,
};
+#[cfg(feature = "proto-vlan")]
+pub use self::vlan::{Packet as VlanPacket, Pcp, Repr as VlanRepr, VlanConfig};
+
#[cfg(feature = "proto-dns")]
pub use self::dns::{
Flags as DnsFlags, Opcode as DnsOpcode, Packet as DnsPacket, Question as DnsQuestion,
diff --git a/src/wire/vlan.rs b/src/wire/vlan.rs
new file mode 100644
index 000000000..1393b3c02
--- /dev/null
+++ b/src/wire/vlan.rs
@@ -0,0 +1,375 @@
+use byteorder::{ByteOrder, NetworkEndian};
+use core::fmt;
+
+use super::ethernet::EtherType;
+use super::{Error, Result};
+
+enum_with_unknown! {
+ /// Priority code point.
+ pub enum Pcp(u8) {
+ Bk = 1, // lowest
+ Be = 0, // default
+ Ee = 2,
+ Ca = 3,
+ Vi = 4,
+ Vo = 5,
+ Ic = 6,
+ NC = 7, // highest
+ }
+}
+
+/// A struct holding VLAN configuration parameters
+#[derive(Debug, PartialEq, Eq, Copy, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct VlanConfig {
+ pub inner_vlan_id: u16,
+ pub outer_vlan_id: Option<u16>,
+}
+
+impl VlanConfig {
+ pub fn get_additional_header_length(&self) -> usize {
+ if self.outer_vlan_id.is_some() {
+ 2 * HEADER_LEN
+ } else {
+ HEADER_LEN
+ }
+ }
+
+ pub(crate) fn emit_to_payload(&self, payload: &mut [u8], ethertype: EtherType) {
+ let mut inner_header = if let Some(outer_vlan_id) = self.outer_vlan_id {
+ let mut outer_header = Packet::new_unchecked(&mut payload[..]);
+ let outer_header_repr = Repr {
+ vlan_identifier: outer_vlan_id,
+ drop_eligible_indicator: false,
+ priority_code_point: Pcp::Be,
+ ethertype: EtherType::VlanInner,
+ };
+ outer_header_repr.emit(&mut outer_header);
+ Packet::new_unchecked(&mut payload[HEADER_LEN..])
+ } else {
+ Packet::new_unchecked(payload)
+ };
+
+ let inner_header_repr = Repr {
+ vlan_identifier: self.inner_vlan_id,
+ drop_eligible_indicator: false,
+ priority_code_point: Pcp::Be,
+ ethertype,
+ };
+ inner_header_repr.emit(&mut inner_header);
+ }
+
+ pub(crate) fn get_outer_ethertype(&self) -> EtherType {
+ if self.outer_vlan_id.is_some() {
+ EtherType::VlanOuter
+ } else {
+ EtherType::VlanInner
+ }
+ }
+}
+
+/// A read/write wrapper around a VLAN header.
+#[derive(Debug, PartialEq, Eq, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct Packet<T: AsRef<[u8]>> {
+ buffer: T,
+}
+
+// VLAN according to IEEE 802.1Q adds 4 bytes after the source MAC address and EtherType of an
+// Ethernet frame as follows:
+//
+// ```txt
+// 0 1 2 3
+// 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+// | TPID (0x8100) | PCP |D| VLAN ID |
+// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+// ```
+//
+// The first two bytes are the Tag Protocol Identifier (TPID) and the last two are the
+// Tag Control Information (TCI).
+//
+// IEEE 802.1ad adds the concept of double tagging which allows an outer header with a
+// TPID of 0x88A8 in front of the IEEE 802.1Q header.
+//
+// For simplicity it is practical to treat the TPID as EtherType of the standard Ethernet
+// header. One can then handle VLAN as a normal Ethernet protocol with the TCI as first field
+// followed by the EtherType of the next protocol:
+//
+// ```txt
+// 0 1 2 3
+// 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+// | PCP |D| VLAN ID | EtherType |
+// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+// ```
+//
+mod field {
+ #![allow(non_snake_case)]
+
+ use crate::wire::field::*;
+
+ pub const TCI: Field = 0..2;
+ pub const ETHERTYPE: Field = 2..4;
+ pub const PAYLOAD: Rest = 4..;
+}
+
+/// The VLAN header length
+pub const HEADER_LEN: usize = field::PAYLOAD.start;
+
+impl<T: AsRef<[u8]>> Packet<T> {
+ /// Imbue a raw octet buffer with VLAN header structure.
+ pub const fn new_unchecked(buffer: T) -> Packet<T> {
+ Packet { buffer }
+ }
+
+ /// Shorthand for a combination of [new_unchecked] and [check_len].
+ ///
+ /// [new_unchecked]: #method.new_unchecked
+ /// [check_len]: #method.check_len
+ pub fn new_checked(buffer: T) -> Result<Packet<T>> {
+ let packet = Self::new_unchecked(buffer);
+ packet.check_len()?;
+ Ok(packet)
+ }
+
+ /// Ensure that no accessor method will panic if called.
+ /// Returns `Err(Error)` if the buffer is shorter than four bytes.
+ ///
+ pub fn check_len(&self) -> Result<()> {
+ let len = self.buffer.as_ref().len();
+ if len < HEADER_LEN {
+ Err(Error)
+ } else {
+ Ok(())
+ }
+ }
+
+ /// Return the length of a VLAN header.
+ pub const fn header_len() -> usize {
+ HEADER_LEN
+ }
+
+ /// Consume the packet, returning the underlying buffer.
+ pub fn into_inner(self) -> T {
+ self.buffer
+ }
+
+ /// Return the TCI field.
+ #[inline]
+ pub fn tag_control_information(&self) -> u16 {
+ let data = self.buffer.as_ref();
+ NetworkEndian::read_u16(&data[field::TCI])
+ }
+
+ /// Return the VID field.
+ #[inline]
+ pub fn vlan_identifier(&self) -> u16 {
+ let data = self.buffer.as_ref();
+ NetworkEndian::read_u16(&data[field::TCI]) & 0xfff
+ }
+
+ /// Return the DEI flag.
+ #[inline]
+ pub fn drop_eligible_indicator(&self) -> bool {
+ let data = self.buffer.as_ref();
+ NetworkEndian::read_u16(&data[field::TCI]) & 0x1000 != 0
+ }
+
+ /// Return the PCP field.
+ #[inline]
+ pub fn priority_code_point(&self) -> Pcp {
+ let data = self.buffer.as_ref();
+ let raw = data[field::TCI.start] & (0xe0_u8 >> 5);
+ Pcp::from(raw)
+ }
+
+ /// Return the EtherType field
+ #[inline]
+ pub fn ethertype(&self) -> EtherType {
+ let data = self.buffer.as_ref();
+ let raw = NetworkEndian::read_u16(&data[field::ETHERTYPE]);
+ EtherType::from(raw)
+ }
+}
+
+impl<T: AsRef<[u8]> + AsMut<[u8]>> Packet<T> {
+ /// Set the TCI field.
+ #[inline]
+ pub fn set_tag_control_information(&mut self, value: u16) {
+ let data = self.buffer.as_mut();
+ NetworkEndian::write_u16(&mut data[field::TCI], value);
+ }
+
+ /// Set the VID field.
+ #[inline]
+ pub fn set_vlan_identifier(&mut self, value: u16) {
+ let data = self.buffer.as_mut();
+ let tci = NetworkEndian::read_u16(&data[field::TCI]);
+ let raw = (tci & 0xf000) | (value & !0xf000);
+ NetworkEndian::write_u16(&mut data[field::TCI], raw)
+ }
+
+ /// Set the DEI flag.
+ #[inline]
+ pub fn set_drop_eligible_indicator(&mut self, value: bool) {
+ let data = self.buffer.as_mut();
+ let raw = data[field::TCI.start];
+ data[field::TCI.start] = if value { raw | 0x10 } else { raw & !0x10 };
+ }
+
+ /// Set the PCP field.
+ #[inline]
+ pub fn set_priority_code_point(&mut self, value: Pcp) {
+ let data = self.buffer.as_mut();
+ let raw = data[field::TCI.start];
+ data[field::TCI.start] = (raw & 0x1f) | (u8::from(value) << 5);
+ }
+
+ /// Set the EtherType field.
+ #[inline]
+ pub fn set_ethertype(&mut self, value: EtherType) {
+ let data = self.buffer.as_mut();
+ NetworkEndian::write_u16(&mut data[field::ETHERTYPE], value.into())
+ }
+}
+
+impl<T: AsRef<[u8]>> AsRef<[u8]> for Packet<T> {
+ fn as_ref(&self) -> &[u8] {
+ self.buffer.as_ref()
+ }
+}
+
+/// A high-level representation of a VLAN header.
+#[derive(Debug, PartialEq, Eq, Clone, Copy)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct Repr {
+ pub vlan_identifier: u16,
+ pub drop_eligible_indicator: bool,
+ pub priority_code_point: Pcp,
+ pub ethertype: EtherType,
+}
+
+impl Repr {
+ /// Parse a VLAN header and return a high-level representation,
+ /// or return `Err(Error)` if the packet is not recognized.
+ pub fn parse<T: AsRef<[u8]>>(packet: &Packet<T>) -> Result<Repr> {
+ packet.check_len()?;
+
+ Ok(Repr {
+ vlan_identifier: packet.vlan_identifier(),
+ drop_eligible_indicator: packet.drop_eligible_indicator(),
+ priority_code_point: packet.priority_code_point(),
+ ethertype: packet.ethertype(),
+ })
+ }
+
+ /// Return the length of a packet that will be emitted from this high-level representation.
+ pub const fn buffer_len(&self) -> usize {
+ HEADER_LEN
+ }
+
+ /// Emit a high-level representation into an VLAN header.
+ pub fn emit<T: AsRef<[u8]> + AsMut<[u8]>>(&self, packet: &mut Packet<T>) {
+ assert!(packet.buffer.as_ref().len() >= self.buffer_len());
+ packet.set_vlan_identifier(self.vlan_identifier);
+ packet.set_drop_eligible_indicator(self.drop_eligible_indicator);
+ packet.set_priority_code_point(self.priority_code_point);
+ packet.set_ethertype(self.ethertype);
+ }
+}
+
+impl<T: AsRef<[u8]>> fmt::Display for Packet<T> {
+ fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+ match Repr::parse(self) {
+ Ok(repr) => write!(f, "{repr}"),
+ _ => {
+ write!(f, "VLAN (unrecognized)")?;
+ write!(
+ f,
+ " vid={:?} dei={:?} pcp={:?} ethetype={:?}",
+ self.vlan_identifier(),
+ self.drop_eligible_indicator(),
+ self.priority_code_point(),
+ self.ethertype(),
+ )?;
+ Ok(())
+ }
+ }
+ }
+}
+
+impl fmt::Display for Repr {
+ fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+ write!(
+ f,
+ "VLAN vid={} ethertype={}",
+ self.vlan_identifier, self.ethertype,
+ )
+ }
+}
+
+use crate::wire::pretty_print::{PrettyIndent, PrettyPrint};
+
+impl<T: AsRef<[u8]>> PrettyPrint for Packet<T> {
+ fn pretty_print(
+ buffer: &dyn AsRef<[u8]>,
+ f: &mut fmt::Formatter,
+ indent: &mut PrettyIndent,
+ ) -> fmt::Result {
+ match Packet::new_checked(buffer) {
+ Err(err) => write!(f, "{indent}({err})"),
+ Ok(packet) => write!(f, "{indent}{packet}"),
+ }
+ }
+}
+
+#[cfg(test)]
+mod test {
+ use super::*;
+
+ static PACKET_BYTES: [u8; 4] = [0x00, 0x64, 0x08, 0x06];
+
+ #[test]
+ fn test_deconstruct() {
+ let packet = Packet::new_unchecked(&PACKET_BYTES[..]);
+ assert_eq!(packet.priority_code_point(), Pcp::Be);
+ assert!(!packet.drop_eligible_indicator());
+ assert_eq!(packet.vlan_identifier(), 100);
+ }
+
+ #[test]
+ fn test_construct() {
+ let mut bytes = vec![0xa5; 4];
+ let mut packet = Packet::new_unchecked(&mut bytes);
+ packet.set_priority_code_point(Pcp::Be);
+ packet.set_drop_eligible_indicator(false);
+ packet.set_vlan_identifier(100);
+ packet.set_ethertype(EtherType::Arp);
+ assert_eq!(&*packet.into_inner(), &PACKET_BYTES[..]);
+ }
+
+ fn packet_repr() -> Repr {
+ Repr {
+ vlan_identifier: 100,
+ drop_eligible_indicator: false,
+ priority_code_point: Pcp::Be,
+ ethertype: EtherType::Arp,
+ }
+ }
+
+ #[test]
+ fn test_parse() {
+ let packet = Packet::new_unchecked(&PACKET_BYTES[..]);
+ let repr = Repr::parse(&packet).unwrap();
+ assert_eq!(repr, packet_repr());
+ }
+
+ #[test]
+ fn test_emit() {
+ let mut bytes = vec![0xa5; 4];
+ let mut packet = Packet::new_unchecked(&mut bytes);
+ packet_repr().emit(&mut packet);
+ assert_eq!(&*packet.into_inner(), &PACKET_BYTES[..HEADER_LEN]);
+ }
+}