[vlan] Add support for IEEE 802.1Q VLANs

Originally-implemented-by: michael-dev@fami-braun.de
Signed-off-by: Michael Brown <mcb30@ipxe.org>

10 files changed, 725 insertions, 10 deletions

diff --git a/src/config/config.c b/src/config/config.c
index f9061d06..5d218881 100644
--- a/src/config/config.c
+++ b/src/config/config.c
@@ -234,6 +234,9 @@ REQUIRE_OBJECT ( pxe_cmd );
 #ifdef LOTEST_CMD
 REQUIRE_OBJECT ( lotest_cmd );
 #endif
+#ifdef VLAN_CMD
+REQUIRE_OBJECT ( vlan_cmd );
+#endif
 
 /*
  * Drag in miscellaneous objects
diff --git a/src/config/general.h b/src/config/general.h
index 652ecf75..b84707ab 100644
--- a/src/config/general.h
+++ b/src/config/general.h
@@ -123,6 +123,7 @@ FILE_LICENCE ( GPL2_OR_LATER );
 #undef	TIME_CMD		/* Time commands */
 #undef	DIGEST_CMD		/* Image crypto digest commands */
 #undef	LOTEST_CMD		/* Loopback testing commands */
+#undef	VLAN_CMD		/* VLAN commands */
 //#undef	PXE_CMD			/* PXE commands */
 
 /*
diff --git a/src/hci/commands/vlan_cmd.c b/src/hci/commands/vlan_cmd.c
new file mode 100644
index 00000000..d9e411fb
--- /dev/null
+++ b/src/hci/commands/vlan_cmd.c
@@ -0,0 +1,174 @@
+/*
+ * Copyright (C) 2010 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER );
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <ipxe/netdevice.h>
+#include <ipxe/command.h>
+#include <ipxe/vlan.h>
+
+/** @file
+ *
+ * VLAN commands
+ *
+ */
+
+static void vcreate_syntax ( char **argv ) {
+	printf ( "Usage:\n  %s --tag <tag> [--priority <priority] "
+		 "<trunk interface>\n", argv[0] );
+}
+
+static int vcreate_exec ( int argc, char **argv ) {
+	static struct option vcreate_opts[] = {
+		{ "help", 0, NULL, 'h' },
+		{ "tag", required_argument, NULL, 't' },
+		{ "priority", required_argument, NULL, 'p' },
+		{ NULL, 0, NULL, 0 },
+	};
+	const char *trunk_name;
+	const char *tag_text = NULL;
+	const char *priority_text = NULL;
+	struct net_device *trunk;
+	unsigned int tag;
+	unsigned int priority;
+	char *endp;
+	int c;
+	int rc;
+
+	/* Parse command line */
+	while ( ( c = getopt_long ( argc, argv, "ht:p:", vcreate_opts,
+				    NULL ) ) >= 0 ) {
+		switch ( c ) {
+		case 't':
+			tag_text = optarg;
+			break;
+		case 'p':
+			priority_text = optarg;
+			break;
+		case 'h':
+			/* Display help text */
+		default:
+			/* Unrecognised/invalid option */
+			vcreate_syntax ( argv );
+			return 1;
+		}
+	}
+	if ( optind != ( argc - 1 ) ) {
+		vcreate_syntax ( argv );
+		return 1;
+	}
+	trunk_name = argv[optind];
+	if ( ! tag_text ) {
+		vcreate_syntax ( argv );
+		return 1;
+	}
+
+	/* Identify network device */
+	trunk = find_netdev ( trunk_name );
+	if ( ! trunk ) {
+		printf ( "%s: no such interface\n", trunk_name );
+		return 1;
+	}
+	tag = strtoul ( tag_text, &endp, 10 );
+	if ( *endp ) {
+		printf ( "%s: invalid tag\n", tag_text );
+		return 1;
+	}
+	if ( priority_text ) {
+		priority = strtoul ( priority_text, &endp, 10 );
+		if ( *endp ) {
+			printf ( "%s: invalid priority\n", priority_text );
+			return 1;
+		}
+	} else {
+		priority = 0;
+	}
+
+	/* Create VLAN device */
+	if ( ( rc = vlan_create ( trunk, tag, priority ) ) != 0 ) {
+		printf ( "Could not create VLAN device: %s\n",
+			 strerror ( rc ) );
+		return 1;
+	}
+
+	return 0;
+}
+
+static void vdestroy_syntax ( char **argv ) {
+	printf ( "Usage:\n  %s <interface>\n", argv[0] );
+}
+
+static int vdestroy_exec ( int argc, char **argv ) {
+	static struct option vdestroy_opts[] = {
+		{ "help", 0, NULL, 'h' },
+		{ NULL, 0, NULL, 0 },
+	};
+	const char *netdev_name;
+	struct net_device *netdev;
+	int c;
+	int rc;
+
+	/* Parse command line */
+	while ( ( c = getopt_long ( argc, argv, "h", vdestroy_opts,
+				    NULL ) ) >= 0 ) {
+		switch ( c ) {
+		case 'h':
+			/* Display help text */
+		default:
+			/* Unrecognised/invalid option */
+			vdestroy_syntax ( argv );
+			return 1;
+		}
+	}
+	if ( optind != ( argc - 1 ) ) {
+		vdestroy_syntax ( argv );
+		return 1;
+	}
+	netdev_name = argv[optind];
+
+	/* Identify network device */
+	netdev = find_netdev ( netdev_name );
+	if ( ! netdev ) {
+		printf ( "%s: no such interface\n", netdev_name );
+		return 1;
+	}
+
+	/* Destroy VLAN device */
+	if ( ( rc = vlan_destroy ( netdev ) ) != 0 ) {
+		printf ( "Could not destroy VLAN device: %s\n",
+			 strerror ( rc ) );
+		return 1;
+	}
+
+	return 0;
+}
+
+struct command vlan_commands[] __command = {
+	{
+		.name = "vcreate",
+		.exec = vcreate_exec,
+	},
+	{
+		.name = "vdestroy",
+		.exec = vdestroy_exec,
+	},
+};
diff --git a/src/include/ipxe/errfile.h b/src/include/ipxe/errfile.h
index 2255f8a3..5f0f1661 100644
--- a/src/include/ipxe/errfile.h
+++ b/src/include/ipxe/errfile.h
@@ -190,6 +190,7 @@ FILE_LICENCE ( GPL2_OR_LATER );
 #define ERRFILE_fcp			( ERRFILE_NET | 0x002d0000 )
 #define ERRFILE_fcoe			( ERRFILE_NET | 0x002e0000 )
 #define ERRFILE_fcns			( ERRFILE_NET | 0x002f0000 )
+#define ERRFILE_vlan			( ERRFILE_NET | 0x00300000 )
 
 #define ERRFILE_image		      ( ERRFILE_IMAGE | 0x00000000 )
 #define ERRFILE_elf		      ( ERRFILE_IMAGE | 0x00010000 )
diff --git a/src/include/ipxe/features.h b/src/include/ipxe/features.h
index 76b59321..660015cd 100644
--- a/src/include/ipxe/features.h
+++ b/src/include/ipxe/features.h
@@ -51,6 +51,7 @@ FILE_LICENCE ( GPL2_OR_LATER );
 #define DHCP_EB_FEATURE_COMBOOT		0x23 /**< COMBOOT format */
 #define DHCP_EB_FEATURE_EFI		0x24 /**< EFI format */
 #define DHCP_EB_FEATURE_FCOE		0x25 /**< FCoE protocol */
+#define DHCP_EB_FEATURE_VLAN		0x26 /**< VLAN support */
 
 /** @} */
 
diff --git a/src/include/ipxe/if_ether.h b/src/include/ipxe/if_ether.h
index db6cb0df..a7e23734 100644
--- a/src/include/ipxe/if_ether.h
+++ b/src/include/ipxe/if_ether.h
@@ -18,6 +18,7 @@ FILE_LICENCE ( GPL2_OR_LATER );
 #define ETH_P_IP	0x0800	/* Internet Protocl Packet */
 #define ETH_P_ARP	0x0806	/* Address Resolution Protocol */
 #define ETH_P_RARP	0x8035	/* Reverse Address resolution Protocol */
+#define ETH_P_8021Q	0x8100	/* 802.1Q VLAN Extended Header */
 #define ETH_P_IPV6	0x86DD	/* IPv6 over blueblook */
 #define ETH_P_SLOW	0x8809	/* Ethernet slow protocols */
 #define ETH_P_EAPOL	0x888E	/* 802.1X EAP over LANs */
diff --git a/src/include/ipxe/netdevice.h b/src/include/ipxe/netdevice.h
index 26e2ab89..6986233e 100644
--- a/src/include/ipxe/netdevice.h
+++ b/src/include/ipxe/netdevice.h
@@ -36,11 +36,12 @@ struct device;
 /** Maximum length of a link-layer header
  *
  * The longest currently-supported link-layer header is for 802.11: a
- * 24-byte frame header plus an 8-byte 802.3 LLC/SNAP header.  (The
- * IPoIB link-layer pseudo-header doesn't actually include link-layer
- * addresses; see ipoib.c for details).
+ * 24-byte frame header plus an 8-byte 802.3 LLC/SNAP header, plus a
+ * possible 4-byte VLAN header.  (The IPoIB link-layer pseudo-header
+ * doesn't actually include link-layer addresses; see ipoib.c for
+ * details.)
  */
-#define MAX_LL_HEADER_LEN 32
+#define MAX_LL_HEADER_LEN 36
 
 /** Maximum length of a network-layer address */
 #define MAX_NET_ADDR_LEN 4
@@ -278,7 +279,7 @@ struct net_device {
 	/** List of open network devices */
 	struct list_head open_list;
 	/** Name of this network device */
-	char name[8];
+	char name[12];
 	/** Underlying hardware device */
 	struct device *dev;
 
diff --git a/src/include/ipxe/vlan.h b/src/include/ipxe/vlan.h
new file mode 100644
index 00000000..86d78bed
--- /dev/null
+++ b/src/include/ipxe/vlan.h
@@ -0,0 +1,66 @@
+#ifndef _IPXE_VLAN_H
+#define _IPXE_VLAN_H
+
+/**
+ * @file
+ *
+ * Virtual LANs
+ *
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER );
+
+/** A VLAN header */
+struct vlan_header {
+	/** Tag control information */
+	uint16_t tci;
+	/** Encapsulated protocol */
+	uint16_t net_proto;
+} __attribute__ (( packed ));
+
+/**
+ * Extract VLAN tag from tag control information
+ *
+ * @v tci		Tag control information
+ * @ret tag		VLAN tag
+ */
+#define VLAN_TAG( tci ) ( (tci) & 0x0fff )
+
+/**
+ * Extract VLAN priority from tag control information
+ *
+ * @v tci		Tag control information
+ * @ret priority	Priority
+ */
+#define VLAN_PRIORITY( tci ) ( (tci) >> 13 )
+
+/**
+ * Construct VLAN tag control information
+ *
+ * @v tag		VLAN tag
+ * @v priority		Priority
+ * @ret tci		Tag control information
+ */
+#define VLAN_TCI( tag, priority ) ( ( (priority) << 13 ) | (tag) )
+
+/**
+ * Check VLAN tag is valid
+ *
+ * @v tag		VLAN tag
+ * @ret is_valid	VLAN tag is valid
+ */
+#define VLAN_TAG_IS_VALID( tag ) ( ( (tag) >= 1 ) && ( (tag) < 0xfff ) )
+
+/**
+ * Check VLAN priority is valid
+ *
+ * @v priority		VLAN priority
+ * @ret is_valid	VLAN priority is valid
+ */
+#define VLAN_PRIORITY_IS_VALID( priority ) ( (priority) <= 7 )
+
+extern int vlan_create ( struct net_device *trunk, unsigned int tag,
+			 unsigned int priority );
+extern int vlan_destroy ( struct net_device *netdev );
+
+#endif /* _IPXE_VLAN_H */
diff --git a/src/net/netdevice.c b/src/net/netdevice.c
index 90dab8fb..37887073 100644
--- a/src/net/netdevice.c
+++ b/src/net/netdevice.c
@@ -405,8 +405,10 @@ int register_netdev ( struct net_device *netdev ) {
 	int rc;
 
 	/* Create device name */
-	snprintf ( netdev->name, sizeof ( netdev->name ), "net%d",
-		   ifindex++ );
+	if ( netdev->name[0] == '\0' ) {
+		snprintf ( netdev->name, sizeof ( netdev->name ), "net%d",
+			   ifindex++ );
+	}
 
 	/* Set initial link-layer address */
 	netdev->ll_protocol->init_addr ( netdev->hw_addr, netdev->ll_addr );
@@ -461,13 +463,13 @@ int netdev_open ( struct net_device *netdev ) {
 
 	DBGC ( netdev, "NETDEV %s opening\n", netdev->name );
 
+	/* Mark as opened */
+	netdev->state |= NETDEV_OPEN;
+
 	/* Open the device */
 	if ( ( rc = netdev->op->open ( netdev ) ) != 0 )
 		return rc;
 
-	/* Mark as opened */
-	netdev->state |= NETDEV_OPEN;
-
 	/* Add to head of open devices list */
 	list_add ( &netdev->open_list, &open_net_devices );
 
diff --git a/src/net/vlan.c b/src/net/vlan.c
new file mode 100644
index 00000000..a764f305
--- /dev/null
+++ b/src/net/vlan.c
@@ -0,0 +1,465 @@
+/*
+ * Copyright (C) 2010 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER );
+
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+#include <errno.h>
+#include <byteswap.h>
+#include <ipxe/features.h>
+#include <ipxe/if_ether.h>
+#include <ipxe/ethernet.h>
+#include <ipxe/netdevice.h>
+#include <ipxe/iobuf.h>
+#include <ipxe/vlan.h>
+
+/** @file
+ *
+ * Virtual LANs
+ *
+ */
+
+FEATURE ( FEATURE_PROTOCOL, "VLAN", DHCP_EB_FEATURE_VLAN, 1 );
+
+struct net_protocol vlan_protocol __net_protocol;
+
+/** VLAN device private data */
+struct vlan_device {
+	/** Trunk network device */
+	struct net_device *trunk;
+	/** VLAN tag */
+	unsigned int tag;
+	/** Default priority */
+	unsigned int priority;
+};
+
+/**
+ * Open VLAN device
+ *
+ * @v netdev		Network device
+ * @ret rc		Return status code
+ */
+static int vlan_open ( struct net_device *netdev ) {
+	struct vlan_device *vlan = netdev->priv;
+
+	return netdev_open ( vlan->trunk );
+}
+
+/**
+ * Close VLAN device
+ *
+ * @v netdev		Network device
+ */
+static void vlan_close ( struct net_device *netdev ) {
+	struct vlan_device *vlan = netdev->priv;
+
+	netdev_close ( vlan->trunk );
+}
+
+/**
+ * Transmit packet on VLAN device
+ *
+ * @v netdev		Network device
+ * @v iobuf		I/O buffer
+ * @ret rc		Return status code
+ */
+static int vlan_transmit ( struct net_device *netdev,
+			   struct io_buffer *iobuf ) {
+	struct vlan_device *vlan = netdev->priv;
+	struct net_device *trunk = vlan->trunk;
+	struct ll_protocol *ll_protocol;
+	struct vlan_header *vlanhdr;
+	uint8_t ll_dest_copy[ETH_ALEN];
+	uint8_t ll_source_copy[ETH_ALEN];
+	const void *ll_dest;
+	const void *ll_source;
+	uint16_t net_proto;
+	int rc;
+
+	/* Strip link-layer header and preserve link-layer header fields */
+	ll_protocol = netdev->ll_protocol;
+	if ( ( rc = ll_protocol->pull ( netdev, iobuf, &ll_dest, &ll_source,
+					&net_proto ) ) != 0 ) {
+		DBGC ( netdev, "VLAN %s could not parse link-layer header: "
+		       "%s\n", netdev->name, strerror ( rc ) );
+		return rc;
+	}
+	memcpy ( ll_dest_copy, ll_dest, ETH_ALEN );
+	memcpy ( ll_source_copy, ll_source, ETH_ALEN );
+
+	/* Construct VLAN header */
+	vlanhdr = iob_push ( iobuf, sizeof ( *vlanhdr ) );
+	vlanhdr->tci = htons ( VLAN_TCI ( vlan->tag, vlan->priority ) );
+	vlanhdr->net_proto = net_proto;
+
+	/* Reclaim I/O buffer from VLAN device's TX queue */
+	list_del ( &iobuf->list );
+
+	/* Transmit packet on trunk device */
+	if ( ( rc = net_tx ( iob_disown ( iobuf ), trunk, &vlan_protocol,
+			     ll_dest_copy, ll_source_copy ) ) != 0 ) {
+		DBGC ( netdev, "VLAN %s could not transmit: %s\n",
+		       netdev->name, strerror ( rc ) );
+		/* Cannot return an error status, since that would
+		 * cause the I/O buffer to be double-freed.
+		 */
+		return 0;
+	}
+
+	return 0;
+}
+
+/**
+ * Poll VLAN device
+ *
+ * @v netdev		Network device
+ */
+static void vlan_poll ( struct net_device *netdev ) {
+	struct vlan_device *vlan = netdev->priv;
+
+	/* Poll trunk device */
+	netdev_poll ( vlan->trunk );
+}
+
+/**
+ * Enable/disable interrupts on VLAN device
+ *
+ * @v netdev		Network device
+ * @v enable		Interrupts should be enabled
+ */
+static void vlan_irq ( struct net_device *netdev, int enable ) {
+	struct vlan_device *vlan = netdev->priv;
+
+	/* Enable/disable interrupts on trunk device.  This is not at
+	 * all robust, but there is no sensible course of action
+	 * available.
+	 */
+	netdev_irq ( vlan->trunk, enable );
+}
+
+/** VLAN device operations */
+static struct net_device_operations vlan_operations = {
+	.open		= vlan_open,
+	.close		= vlan_close,
+	.transmit	= vlan_transmit,
+	.poll		= vlan_poll,
+	.irq		= vlan_irq,
+};
+
+/**
+ * Synchronise VLAN device
+ *
+ * @v netdev		Network device
+ */
+static void vlan_sync ( struct net_device *netdev ) {
+	struct vlan_device *vlan = netdev->priv;
+	struct net_device *trunk = vlan->trunk;
+
+	/* Synchronise link status */
+	if ( netdev->link_rc != trunk->link_rc )
+		netdev_link_err ( netdev, trunk->link_rc );
+
+	/* Synchronise open/closed status */
+	if ( netdev_is_open ( trunk ) ) {
+		if ( ! netdev_is_open ( netdev ) )
+			netdev_open ( netdev );
+	} else {
+		if ( netdev_is_open ( netdev ) )
+			netdev_close ( netdev );
+	}
+}
+
+/**
+ * Identify VLAN device
+ *
+ * @v trunk		Trunk network device
+ * @v tag		VLAN tag
+ * @ret netdev		VLAN device, if any
+ */
+static struct net_device * vlan_find ( struct net_device *trunk,
+				       uint16_t tag ) {
+	struct net_device *netdev;
+	struct vlan_device *vlan;
+
+	for_each_netdev ( netdev ) {
+		if ( netdev->op != &vlan_operations )
+			continue;
+		vlan = netdev->priv;
+		if ( ( vlan->trunk == trunk ) && ( vlan->tag == tag ) )
+			return netdev;
+	}
+	return NULL;
+}
+
+/**
+ * Process incoming VLAN packet
+ *
+ * @v iobuf		I/O buffer
+ * @v trunk		Trunk network device
+ * @v ll_dest		Link-layer destination address
+ * @v ll_source		Link-layer source address
+ * @ret rc		Return status code
+ */
+static int vlan_rx ( struct io_buffer *iobuf, struct net_device *trunk,
+		     const void *ll_dest, const void *ll_source ) {
+	struct vlan_header *vlanhdr = iobuf->data;
+	struct net_device *netdev;
+	struct ll_protocol *ll_protocol;
+	uint8_t ll_dest_copy[ETH_ALEN];
+	uint8_t ll_source_copy[ETH_ALEN];
+	uint16_t tag;
+	int rc;
+
+	/* Sanity check */
+	if ( iob_len ( iobuf ) < sizeof ( *vlanhdr ) ) {
+		DBGC ( trunk, "VLAN %s received underlength packet (%zd "
+		       "bytes)\n", trunk->name, iob_len ( iobuf ) );
+		rc = -EINVAL;
+		goto err_sanity;
+	}
+
+	/* Identify VLAN device */
+	tag = VLAN_TAG ( ntohs ( vlanhdr->tci ) );
+	netdev = vlan_find ( trunk, tag );
+	if ( ! netdev ) {
+		DBGC2 ( trunk, "VLAN %s received packet for unknown VLAN "
+			"%d\n", trunk->name, tag );
+		rc = -EPIPE;
+		goto err_no_vlan;
+	}
+
+	/* Strip VLAN header and preserve original link-layer header fields */
+	iob_pull ( iobuf, sizeof ( *vlanhdr ) );
+	ll_protocol = trunk->ll_protocol;
+	memcpy ( ll_dest_copy, ll_dest, ETH_ALEN );
+	memcpy ( ll_source_copy, ll_source, ETH_ALEN );
+
+	/* Reconstruct link-layer header for VLAN device */
+	ll_protocol = netdev->ll_protocol;
+	if ( ( rc = ll_protocol->push ( netdev, iobuf, ll_dest_copy,
+					ll_source_copy,
+					vlanhdr->net_proto ) ) != 0 ) {
+		DBGC ( netdev, "VLAN %s could not reconstruct link-layer "
+		       "header: %s\n", netdev->name, strerror ( rc ) );
+		goto err_ll_push;
+	}
+
+	/* Enqueue packet on VLAN device */
+	netdev_rx ( netdev, iob_disown ( iobuf ) );
+	return 0;
+
+ err_ll_push:
+ err_no_vlan:
+ err_sanity:
+	free_iob ( iobuf );
+	return rc;
+}
+
+/** VLAN protocol */
+struct net_protocol vlan_protocol __net_protocol = {
+	.name = "VLAN",
+	.net_proto = htons ( ETH_P_8021Q ),
+	.rx = vlan_rx,
+};
+
+/**
+ * Create VLAN device
+ *
+ * @v trunk		Trunk network device
+ * @v tag		VLAN tag
+ * @v priority		Default VLAN priority
+ * @ret rc		Return status code
+ *
+ * The VLAN device will be created as an Ethernet device.  (We cannot
+ * simply clone the link layer of the trunk network device, because
+ * this link layer may expect the network device structure to contain
+ * some link-layer-private data.)  The trunk network device must
+ * therefore have a link layer that is in some sense 'compatible' with
+ * Ethernet; specifically, it must have link-layer addresses that are
+ * the same length as Ethernet link-layer addresses.
+ */
+int vlan_create ( struct net_device *trunk, unsigned int tag,
+		  unsigned int priority ) {
+	struct net_device *netdev;
+	struct vlan_device *vlan;
+	int rc;
+
+	/* Sanity checks */
+	if ( trunk->ll_protocol->ll_addr_len != ETH_ALEN ) {
+		DBGC ( trunk, "VLAN %s cannot create VLAN for %s device\n",
+		       trunk->name, trunk->ll_protocol->name );
+		rc = -ENOTTY;
+		goto err_sanity;
+	}
+	if ( ! VLAN_TAG_IS_VALID ( tag ) ) {
+		DBGC ( trunk, "VLAN %s cannot create VLAN with invalid tag "
+		       "%d\n", trunk->name, tag );
+		rc = -EINVAL;
+		goto err_sanity;
+	}
+	if ( ! VLAN_PRIORITY_IS_VALID ( priority ) ) {
+		DBGC ( trunk, "VLAN %s cannot create VLAN with invalid "
+		       "priority %d\n", trunk->name, priority );
+		rc = -EINVAL;
+		goto err_sanity;
+	}
+	if ( ( netdev = vlan_find ( trunk, tag ) ) != NULL ) {
+		DBGC ( netdev, "VLAN %s already exists\n", netdev->name );
+		rc = -EEXIST;
+		goto err_sanity;
+	}
+
+	/* Allocate and initialise structure */
+	netdev = alloc_etherdev ( sizeof ( *vlan ) );
+	if ( ! netdev ) {
+		rc = -ENOMEM;
+		goto err_alloc_etherdev;
+	}
+	netdev_init ( netdev, &vlan_operations );
+	netdev->dev = trunk->dev;
+	memcpy ( netdev->hw_addr, trunk->ll_addr, ETH_ALEN );
+	vlan = netdev->priv;
+	vlan->trunk = netdev_get ( trunk );
+	vlan->tag = tag;
+	vlan->priority = priority;
+
+	/* Construct VLAN device name */
+	snprintf ( netdev->name, sizeof ( netdev->name ), "%s.%d",
+		   trunk->name, vlan->tag );
+
+	/* Register VLAN device */
+	if ( ( rc = register_netdev ( netdev ) ) != 0 ) {
+		DBGC ( netdev, "VLAN %s could not register: %s\n",
+		       netdev->name, strerror ( rc ) );
+		goto err_register;
+	}
+
+	/* Synchronise with trunk device */
+	vlan_sync ( netdev );
+
+	return 0;
+
+	unregister_netdev ( netdev );
+ err_register:
+	netdev_nullify ( netdev );
+	netdev_put ( netdev );
+	netdev_put ( trunk );
+ err_alloc_etherdev:
+ err_sanity:
+	return rc;
+}
+
+/**
+ * Destroy VLAN device
+ *
+ * @v netdev		Network device
+ * @ret rc		Return status code
+ */
+int vlan_destroy ( struct net_device *netdev ) {
+	struct vlan_device *vlan = netdev->priv;
+	struct net_device *trunk;
+
+	/* Sanity check */
+	if ( netdev->op != &vlan_operations ) {
+		DBGC ( netdev, "VLAN %s cannot destroy non-VLAN device\n",
+		       netdev->name );
+		return -ENOTTY;
+	}
+
+	/* Remove VLAN device */
+	unregister_netdev ( netdev );
+	trunk = vlan->trunk;
+	netdev_nullify ( netdev );
+	netdev_put ( netdev );
+	netdev_put ( trunk );
+
+	return 0;
+}
+
+/**
+ * Do nothing
+ *
+ * @v trunk		Trunk network device
+ * @ret rc		Return status code
+ */
+static int vlan_probe ( struct net_device *trunk __unused ) {
+	return 0;
+}
+
+/**
+ * Handle trunk network device link state change
+ *
+ * @v trunk		Trunk network device
+ */
+static void vlan_notify ( struct net_device *trunk ) {
+	struct net_device *netdev;
+	struct vlan_device *vlan;
+
+	for_each_netdev ( netdev ) {
+		if ( netdev->op != &vlan_operations )
+			continue;
+		vlan = netdev->priv;
+		if ( vlan->trunk == trunk )
+			vlan_sync ( netdev );
+	}
+}
+
+/**
+ * Destroy first VLAN device for a given trunk
+ *
+ * @v trunk		Trunk network device
+ * @ret found		A VLAN device was found
+ */
+static int vlan_remove_first ( struct net_device *trunk ) {
+	struct net_device *netdev;
+	struct vlan_device *vlan;
+
+	for_each_netdev ( netdev ) {
+		if ( netdev->op != &vlan_operations )
+			continue;
+		vlan = netdev->priv;
+		if ( vlan->trunk == trunk ) {
+			vlan_destroy ( netdev );
+			return 1;
+		}
+	}
+	return 0;
+}
+
+/**
+ * Destroy all VLAN devices for a given trunk
+ *
+ * @v trunk		Trunk network device
+ */
+static void vlan_remove ( struct net_device *trunk ) {
+
+	/* Remove all VLAN devices attached to this trunk, safe
+	 * against arbitrary net device removal.
+	 */
+	while ( vlan_remove_first ( trunk ) ) {}
+}
+
+/** VLAN driver */
+struct net_driver vlan_driver __net_driver = {
+	.name = "VLAN",
+	.probe = vlan_probe,
+	.notify = vlan_notify,
+	.remove = vlan_remove,
+};