4 files changed, 85 insertions, 22 deletions

diff --git a/src/drivers/infiniband/hermon.c b/src/drivers/infiniband/hermon.c
index a1d2a3bd5..9675c156b 100644
--- a/src/drivers/infiniband/hermon.c
+++ b/src/drivers/infiniband/hermon.c
@@ -3207,22 +3207,16 @@ static void hermon_eth_complete_recv ( struct ib_device *ibdev __unused,
 				       struct ib_address_vector *source,
 				       struct io_buffer *iobuf, int rc ) {
 	struct net_device *netdev = ib_qp_get_ownerdata ( qp );
-	struct net_device *vlan;
-
-	/* Find VLAN device, if applicable */
-	if ( source->vlan_present ) {
-		if ( ( vlan = vlan_find ( netdev, source->vlan ) ) != NULL ) {
-			netdev = vlan;
-		} else if ( rc == 0 ) {
-			rc = -ENODEV;
-		}
-	}
+	unsigned int tag;
+
+	/* Identify VLAN tag, if applicable */
+	tag = ( source->vlan_present ? source->vlan : 0 );
 
 	/* Hand off to network layer */
 	if ( rc == 0 ) {
-		netdev_rx ( netdev, iobuf );
+		vlan_netdev_rx ( netdev, tag, iobuf );
 	} else {
-		netdev_rx_err ( netdev, iobuf, rc );
+		vlan_netdev_rx_err ( netdev, tag, iobuf, rc );
 	}
 }
 
diff --git a/src/include/ipxe/vlan.h b/src/include/ipxe/vlan.h
index 439e0c16d..7f93439b3 100644
--- a/src/include/ipxe/vlan.h
+++ b/src/include/ipxe/vlan.h
@@ -10,6 +10,8 @@
 
 FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 
+#include <ipxe/netdevice.h>
+
 /** A VLAN header */
 struct vlan_header {
 	/** Tag control information */
@@ -59,12 +61,14 @@ struct vlan_header {
  */
 #define VLAN_PRIORITY_IS_VALID( priority ) ( (priority) <= 7 )
 
-extern struct net_device * vlan_find ( struct net_device *trunk,
-				       unsigned int tag );
 extern unsigned int vlan_tag ( struct net_device *netdev );
 extern int vlan_can_be_trunk ( struct net_device *trunk );
 extern int vlan_create ( struct net_device *trunk, unsigned int tag,
 			 unsigned int priority );
 extern int vlan_destroy ( struct net_device *netdev );
+extern void vlan_netdev_rx ( struct net_device *netdev, unsigned int tag,
+			     struct io_buffer *iobuf );
+extern void vlan_netdev_rx_err ( struct net_device *netdev, unsigned int tag,
+				 struct io_buffer *iobuf, int rc );
 
 #endif /* _IPXE_VLAN_H */
diff --git a/src/net/netdevice.c b/src/net/netdevice.c
index 71a37eccc..3b02e64bd 100644
--- a/src/net/netdevice.c
+++ b/src/net/netdevice.c
@@ -1126,15 +1126,35 @@ __weak unsigned int vlan_tag ( struct net_device *netdev __unused ) {
 }
 
 /**
- * Identify VLAN device (when VLAN support is not present)
+ * Add VLAN tag-stripped packet to queue (when VLAN support is not present)
  *
- * @v trunk		Trunk network device
- * @v tag		VLAN tag
- * @ret netdev		VLAN device, if any
+ * @v netdev		Network device
+ * @v tag		VLAN tag, or zero
+ * @v iobuf		I/O buffer
  */
-__weak struct net_device * vlan_find ( struct net_device *trunk __unused,
-				       unsigned int tag __unused ) {
-	return NULL;
+__weak void vlan_netdev_rx ( struct net_device *netdev, unsigned int tag,
+			     struct io_buffer *iobuf ) {
+
+	if ( tag == 0 ) {
+		netdev_rx ( netdev, iobuf );
+	} else {
+		netdev_rx_err ( netdev, iobuf, -ENODEV );
+	}
+}
+
+/**
+ * Discard received VLAN tag-stripped packet (when VLAN support is not present)
+ *
+ * @v netdev		Network device
+ * @v tag		VLAN tag, or zero
+ * @v iobuf		I/O buffer, or NULL
+ * @v rc		Packet status code
+ */
+__weak void vlan_netdev_rx_err ( struct net_device *netdev,
+				 unsigned int tag __unused,
+				 struct io_buffer *iobuf, int rc ) {
+
+	netdev_rx_err ( netdev, iobuf, rc );
 }
 
 /** Networking stack process */
diff --git a/src/net/vlan.c b/src/net/vlan.c
index f515c2dc9..90f2934de 100644
--- a/src/net/vlan.c
+++ b/src/net/vlan.c
@@ -199,7 +199,8 @@ static void vlan_sync ( struct net_device *netdev ) {
  * @v tag		VLAN tag
  * @ret netdev		VLAN device, if any
  */
-struct net_device * vlan_find ( struct net_device *trunk, unsigned int tag ) {
+static struct net_device * vlan_find ( struct net_device *trunk,
+				       unsigned int tag ) {
 	struct net_device *netdev;
 	struct vlan_device *vlan;
 
@@ -506,3 +507,47 @@ struct net_driver vlan_driver __net_driver = {
 	.notify = vlan_notify,
 	.remove = vlan_remove,
 };
+
+/**
+ * Add VLAN tag-stripped packet to receive queue
+ *
+ * @v netdev		Network device
+ * @v tag		VLAN tag, or zero
+ * @v iobuf		I/O buffer
+ */
+void vlan_netdev_rx ( struct net_device *netdev, unsigned int tag,
+		      struct io_buffer *iobuf ) {
+	struct net_device *vlan;
+
+	/* Identify VLAN device, if applicable */
+	if ( tag ) {
+		if ( ( vlan = vlan_find ( netdev, tag ) ) == NULL ) {
+			netdev_rx_err ( netdev, iobuf, -ENODEV );
+			return;
+		}
+		netdev = vlan;
+	}
+
+	/* Hand off to network device */
+	netdev_rx ( netdev, iobuf );
+}
+
+/**
+ * Discard received VLAN tag-stripped packet
+ *
+ * @v netdev		Network device
+ * @v tag		VLAN tag, or zero
+ * @v iobuf		I/O buffer, or NULL
+ * @v rc		Packet status code
+ */
+void vlan_netdev_rx_err ( struct net_device *netdev, unsigned int tag,
+			  struct io_buffer *iobuf, int rc ) {
+	struct net_device *vlan;
+
+	/* Identify VLAN device, if applicable */
+	if ( tag && ( ( vlan = vlan_find ( netdev, tag ) ) != NULL ) )
+		netdev = vlan;
+
+	/* Hand off to network device */
+	netdev_rx_err ( netdev, iobuf, rc );
+}