Add VLAN support

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"]

ci.sh

@@ -29,7 +29,7 @@ FEATURES_TEST=(
     "std,medium-ieee802154,proto-sixlowpan,proto-sixlowpan-fragmentation,socket-udp"
     "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 +39,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"
 )

src/iface/interface/ethernet.rs

@@ -18,22 +18,70 @@ impl InterfaceInner {
             return None;
         }
 
-        match eth_frame.ethertype() {
+        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, &eth_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,
         }

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,19 +426,43 @@ 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);
             }
         };
 
         tx_token.consume(tx_len, |mut tx_buffer| {
             #[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 =