Patches by Pantelis Antoniou, 30 Mar 2004:

add networking support for VLANs (802.1q), and CDP (Cisco Discovery Protocol)

14 files changed, 817 insertions, 52 deletions

diff --git a/CHANGELOG b/CHANGELOG
index 8ababc72c9e..9445ee3a1ac 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -20,6 +20,13 @@ Changes for U-Boot 1.1.1:
        interface.
     3. We now correctly match the MII/RMII interface
        configuration to what the PHY reports.
+  - Fix problem when readingthe MII status register. Due to the
+    internal design of many PHYs you have to read the register
+    twice. The problem is more apparent in 10Mbit mode.
+  - add new mode ".jffs2s" for reading from a NAND device: it just
+    skips over bad blocks.
+  - add networking support for VLANs (802.1q), and CDP (Cisco
+    Discovery Protocol)
 
 * Patch by Yuli Barcohen, 28 Mar 2004:
   - Add support for MPC8272 family including MPC8247/8248/8271/8272
diff --git a/README b/README
index 4d7145d2953..d1816262c9c 100644
--- a/README
+++ b/README
@@ -582,6 +582,7 @@ The following options need to be configured:
 		CFG_CMD_USB	* USB support
 		CFG_CMD_VFD	* VFD support (TRAB)
 		CFG_CMD_BSP	* Board SPecific functions
+		CFG_CMD_CDP	* Cisco Discover Protocol support
 		-----------------------------------------------
 		CFG_CMD_ALL	all
 
@@ -950,6 +951,48 @@ The following options need to be configured:
 		environment variable is passed as option 12 to
 		the DHCP server.
 
+ - CDP Options:
+ 		CONFIG_CDP_DEVICE_ID
+
+		The device id used in CDP trigger frames.
+
+		CONFIG_CDP_DEVICE_ID_PREFIX
+
+		A two character string which is prefixed to the MAC address
+		of the device.
+
+		CONFIG_CDP_PORT_ID
+
+		A printf format string which contains the ascii name of
+		the port. Normally is set to "eth%d" which sets
+		eth0 for the first ethernet, eth1 for the second etc.
+
+		CONFIG_CDP_CAPABILITIES
+
+		A 32bit integer which indicates the device capabilities;
+		0x00000010 for a normal host which does not forwards.
+
+		CONFIG_CDP_VERSION
+
+		An ascii string containing the version of the software.
+
+		CONFIG_CDP_PLATFORM
+
+		An ascii string containing the name of the platform.
+
+		CONFIG_CDP_TRIGGER
+
+		A 32bit integer sent on the trigger.
+
+		CONFIG_CDP_POWER_CONSUMPTION
+
+		A 16bit integer containing the power consumption of the
+		device in .1 of milliwatts.
+
+		CONFIG_CDP_APPLIANCE_VLAN_TYPE
+
+		A byte containing the id of the VLAN.
+
 - Status LED:	CONFIG_STATUS_LED
 
 		Several configurations allow to display the current
@@ -2187,6 +2230,26 @@ Some configuration options can be set using Environment Variables:
 
   bootstopkey	- see CONFIG_AUTOBOOT_STOP_STR
 
+  ethprime	- When CONFIG_NET_MULTI is enabled controls which
+  		  interface is used first.
+
+  ethact	- When CONFIG_NET_MULTI is enabled controls which
+  		  interface is currently active. For example you
+		  can do the following
+
+		  => setenv ethact FEC ETHERNET
+		  => ping 192.168.0.1 # traffic sent on FEC ETHERNET
+		  => setenv ethact SCC ETHERNET
+		  => ping 10.0.0.1 # traffic sent on SCC ETHERNET
+
+   netretry	- When set to "no" each network operation will
+   		  either succeed or fail without retrying.
+		  Useful on scripts which control the retry operation
+		  themselves.
+
+   vlan		- When set to a value < 4095 the traffic over
+   		  ethernet is encapsulated/received over 802.1q
+		  VLAN tagged frames.
 
 The following environment variables may be used and automatically
 updated by the network boot commands ("bootp" and "rarpboot"),
diff --git a/common/cmd_nand.c b/common/cmd_nand.c
index 0e49e9f08da..c2e67abed54 100644
--- a/common/cmd_nand.c
+++ b/common/cmd_nand.c
@@ -9,6 +9,7 @@
 #include <command.h>
 #include <malloc.h>
 #include <asm/io.h>
+#include <watchdog.h>
 
 #ifdef CONFIG_SHOW_BOOT_PROGRESS
 # include <status_led.h>
@@ -63,6 +64,7 @@ struct nand_oob_config {
 #define NANDRW_READ	0x01
 #define NANDRW_WRITE	0x00
 #define NANDRW_JFFS2	0x02
+#define NANDRW_JFFS2_SKIP	0x04
 
 /*
  * Function Prototypes
@@ -207,6 +209,11 @@ int do_nand (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 		}
 		else if (cmdtail && !strncmp(cmdtail, ".jffs2", 2))
 			cmd |= NANDRW_JFFS2;	/* skip bad blocks */
+		else if (cmdtail && !strncmp(cmdtail, ".jffs2s", 2)) {
+			cmd |= NANDRW_JFFS2;	/* skip bad blocks (on read too) */
+			if (cmd & NANDRW_READ)
+				cmd |= NANDRW_JFFS2_SKIP;	/* skip bad blocks (on read too) */
+		}
 #ifdef SXNI855T
 		/* need ".e" same as ".j" for compatibility with older units */
 		else if (cmdtail && !strcmp(cmdtail, ".e"))
@@ -258,7 +265,7 @@ U_BOOT_CMD(
 	"nand    - NAND sub-system\n",
 	"info  - show available NAND devices\n"
 	"nand device [dev] - show or set current device\n"
-	"nand read[.jffs2]  addr off size\n"
+	"nand read[.jffs2[s]]  addr off size\n"
 	"nand write[.jffs2] addr off size - read/write `size' bytes starting\n"
 	"    at offset `off' to/from memory address `addr'\n"
 	"nand erase [clean] [off size] - erase `size' bytes from\n"
@@ -420,6 +427,7 @@ static void nand_print_bad(struct nand_chip* nand)
  *	1: NANDRW_READ			read, fail on bad block
  *	2: NANDRW_WRITE | NANDRW_JFFS2	write, skip bad blocks
  *	3: NANDRW_READ | NANDRW_JFFS2	read, data all 0xff for bad blocks
+ *      7: NANDRW_READ | NANDRW_JFFS2 | NANDRW_JFFS2_SKIP read, skip bad blocks
  */
 static int nand_rw (struct nand_chip* nand, int cmd,
 	    size_t start, size_t len,
@@ -450,6 +458,10 @@ static int nand_rw (struct nand_chip* nand, int cmd,
 					}
 					continue;
 				}
+				else if (cmd == (NANDRW_READ | NANDRW_JFFS2 | NANDRW_JFFS2_SKIP)) {
+					start += erasesize;
+					continue;
+				}
 				else if (cmd == (NANDRW_WRITE | NANDRW_JFFS2)) {
 					/* skip bad block */
 					start += erasesize;
diff --git a/common/cmd_net.c b/common/cmd_net.c
index f13e9d493c3..85a90237342 100644
--- a/common/cmd_net.c
+++ b/common/cmd_net.c
@@ -96,7 +96,7 @@ U_BOOT_CMD(
 
 static void netboot_update_env(void)
 {
-    char tmp[16] ;
+    char tmp[22] ;
 
     if (NetOurGatewayIP) {
 	ip_to_string (NetOurGatewayIP, tmp);
@@ -139,6 +139,16 @@ static void netboot_update_env(void)
     if (NetOurNISDomain[0])
 	setenv("domain", NetOurNISDomain);
 
+    if (ntohs(NetOurVLAN) != (ushort)-1) {
+	    VLAN_to_string(NetOurVLAN, tmp);
+	    setenv("vlan", tmp);
+    }
+
+    if (ntohs(NetOurNativeVLAN) != (ushort)-1) {
+	    VLAN_to_string(NetOurNativeVLAN, tmp);
+	    setenv("vlan", tmp);
+    }
+
 }
 static int
 netboot_common (int proto, cmd_tbl_t *cmdtp, int argc, char *argv[])
@@ -238,4 +248,47 @@ U_BOOT_CMD(
 );
 #endif	/* CFG_CMD_PING */
 
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+
+static void cdp_update_env(void)
+{
+	char tmp[16];
+
+	if (CDPApplianceVLAN != htons(-1)) {
+		printf("CDP offered appliance VLAN %d\n", ntohs(CDPApplianceVLAN));
+		VLAN_to_string(CDPApplianceVLAN, tmp);
+		setenv("vlan", tmp);
+		NetOurVLAN = CDPApplianceVLAN;
+	}
+
+	if (CDPNativeVLAN != htons(-1)) {
+		printf("CDP offered native VLAN %d\n", ntohs(CDPNativeVLAN));
+		VLAN_to_string(CDPNativeVLAN, tmp);
+		setenv("nvlan", tmp);
+		NetOurNativeVLAN = CDPNativeVLAN;
+	}
+
+}
+
+int do_cdp (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
+{
+	int r;
+
+	r = NetLoop(CDP);
+	if (r < 0) {
+		printf("cdp failed; perhaps not a CISCO switch?\n");
+		return 1;
+	}
+
+	cdp_update_env();
+
+	return 0;
+}
+
+U_BOOT_CMD(
+	cdp,	1,	1,	do_cdp,
+	"cdp     - Perform CDP network configuration\n",
+);
+#endif	/* CFG_CMD_CDP */
+
 #endif	/* CFG_CMD_NET */
diff --git a/common/miiphyutil.c b/common/miiphyutil.c
index 919de3ebc5e..721d109a132 100644
--- a/common/miiphyutil.c
+++ b/common/miiphyutil.c
@@ -203,6 +203,8 @@ int miiphy_link (unsigned char addr)
 {
 	unsigned short reg;
 
+	/* dummy read; needed to latch some phys */
+	(void)miiphy_read(addr, PHY_BMSR, &reg);
 	if (miiphy_read (addr, PHY_BMSR, &reg)) {
 		puts ("PHY_BMSR read failed, assuming no link\n");
 		return (0);
diff --git a/doc/README.VLAN b/doc/README.VLAN
new file mode 100644
index 00000000000..4f86d55ea1f
--- /dev/null
+++ b/doc/README.VLAN
@@ -0,0 +1,15 @@
+U-Boot has networking support for VLANs (802.1q), and CDP (Cisco
+Discovery Protocol).
+
+You control the sending/receiving of VLAN tagged packets with the
+"vlan" environmental variable. When not present no tagging is
+performed.
+
+CDP is used mainly to discover your device VLAN(s) when connected to
+a Cisco switch.
+
+Note: In order to enable CDP support a small change is needed in the
+networking driver. You have to enable reception of the
+01:00:0c:cc:cc:cc MAC address which is a multicast address.
+
+Various defines control CDP; see the README section.
diff --git a/include/cmd_confdefs.h b/include/cmd_confdefs.h
index c805970026d..7a4dfe1ec8c 100644
--- a/include/cmd_confdefs.h
+++ b/include/cmd_confdefs.h
@@ -88,6 +88,7 @@
 #define CFG_CMD_ITEST	0x0040000000000000U	/* Integer (and string) test	*/
 #define CFG_CMD_NFS	0x0080000000000000U	/* NFS support			*/
 #define CFG_CMD_REISER  0x0100000000000000U     /* Reiserfs support		*/
+#define CFG_CMD_CDP	0x0200000000000000U	/* Cisco Discovery Protocol 	*/
 
 #define CFG_CMD_ALL	0xFFFFFFFFFFFFFFFFU	/* ALL commands			*/
 
@@ -131,7 +132,8 @@
 			CFG_CMD_SDRAM	| \
 			CFG_CMD_SPI	| \
 			CFG_CMD_USB	| \
-			CFG_CMD_VFD	)
+			CFG_CMD_VFD	| \
+			CFG_CMD_CDP 	)
 
 /* Default configuration
  */
diff --git a/include/net.h b/include/net.h
index 1b2b347f86a..68f5fea3e3e 100644
--- a/include/net.h
+++ b/include/net.h
@@ -110,7 +110,11 @@ struct eth_device {
 extern int eth_initialize(bd_t *bis);		/* Initialize network subsystem */
 extern int eth_register(struct eth_device* dev);/* Register network device	*/
 extern void eth_try_another(int first_restart);	/* Change the device		*/
+#ifdef CONFIG_NET_MULTI
+extern void eth_set_current(void);		/* set nterface to ethcur var.  */
+#endif
 extern struct eth_device *eth_get_dev(void);	/* get the current device MAC	*/
+extern int eth_get_dev_index (void);		/* get the device index         */
 extern void eth_set_enetaddr(int num, char* a);	/* Set new MAC address		*/
 
 extern int eth_init(bd_t *bis);			/* Initialize the device	*/
@@ -143,9 +147,24 @@ typedef struct {
 
 #define ETHER_HDR_SIZE	14		/* Ethernet header size		*/
 #define E802_HDR_SIZE	22		/* 802 ethernet header size	*/
+
+/*
+ *	Ethernet header
+ */
+typedef struct {
+	uchar		vet_dest[6];	/* Destination node		*/
+	uchar		vet_src[6];	/* Source node			*/
+	ushort		vet_vlan_type;	/* PROT_VLAN			*/
+	ushort		vet_tag;	/* TAG of VLAN			*/
+	ushort		vet_type;	/* protocol type		*/
+} VLAN_Ethernet_t;
+
+#define VLAN_ETHER_HDR_SIZE	18	/* VLAN Ethernet header size	*/
+
 #define PROT_IP		0x0800		/* IP protocol			*/
 #define PROT_ARP	0x0806		/* IP ARP protocol		*/
 #define PROT_RARP	0x8035		/* IP ARP protocol		*/
+#define PROT_VLAN	0x8100		/* IEEE 802.1q protocol		*/
 
 #define IPPROTO_ICMP	 1	/* Internet Control Message Protocol	*/
 #define IPPROTO_UDP	17	/* User Datagram Protocol		*/
@@ -296,6 +315,15 @@ extern int		NetRxPktLen;		/* Current rx packet length	*/
 extern unsigned		NetIPID;		/* IP ID (counting)		*/
 extern uchar		NetBcastAddr[6];	/* Ethernet boardcast address	*/
 
+#define VLAN_NONE	4095			/* untagged 			*/
+#define VLAN_IDMASK	0x0fff			/* mask of valid vlan id 	*/
+extern ushort		NetOurVLAN;		/* Our VLAN 			*/
+extern ushort		NetOurNativeVLAN;	/* Our Native VLAN 		*/
+
+extern uchar		NetCDPAddr[6]; 		/* Ethernet CDP address		*/
+extern ushort		CDPNativeVLAN;		/* CDP returned native VLAN	*/
+extern ushort		CDPApplianceVLAN;	/* CDP returned appliance VLAN	*/
+
 extern int		NetState;		/* Network loop state		*/
 #define NETLOOP_CONTINUE	1
 #define NETLOOP_RESTART		2
@@ -306,7 +334,7 @@ extern int		NetState;		/* Network loop state		*/
 extern int		NetRestartWrap;		/* Tried all network devices	*/
 #endif
 
-typedef enum { BOOTP, RARP, ARP, TFTP, DHCP, PING, DNS, NFS } proto_t;
+typedef enum { BOOTP, RARP, ARP, TFTP, DHCP, PING, DNS, NFS, CDP } proto_t;
 
 /* from net/net.c */
 extern char	BootFile[128];			/* Boot File name		*/
@@ -315,6 +343,12 @@ extern char	BootFile[128];			/* Boot File name		*/
 extern IPaddr_t	NetPingIP;			/* the ip address to ping 		*/
 #endif
 
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+/* when CDP completes these hold the return values */
+extern ushort CDPNativeVLAN;
+extern ushort CDPApplianceVLAN;
+#endif
+
 /* Initialize the network adapter */
 extern int	NetLoop(proto_t);
 
@@ -324,8 +358,11 @@ extern void	NetStop(void);
 /* Load failed.	 Start again. */
 extern void	NetStartAgain(void);
 
-/* Set ethernet header */
-extern void	NetSetEther(volatile uchar *, uchar *, uint);
+/* Get size of the ethernet header when we send */
+extern int 	NetEthHdrSize(void);
+
+/* Set ethernet header; returns the size of the header */
+extern int	NetSetEther(volatile uchar *, uchar *, uint);
 
 /* Set IP header */
 extern void	NetSetIP(volatile uchar *, IPaddr_t, int, int, int);
@@ -397,9 +434,18 @@ extern void	ip_to_string (IPaddr_t x, char *s);
 /* Convert a string to ip address */
 extern IPaddr_t string_to_ip(char *s);
 
+/* Convert a VLAN id to a string */
+extern void	VLAN_to_string (ushort x, char *s);
+
+/* Convert a string to a vlan id */
+extern ushort string_to_VLAN(char *s);
+
 /* read an IP address from a environment variable */
 extern IPaddr_t getenv_IPaddr (char *);
 
+/* read a VLAN id from an environment variable */
+extern ushort getenv_VLAN(char *);
+
 /* copy a filename (allow for "..." notation, limit length) */
 extern void	copy_filename (uchar *dst, uchar *src, int size);
 
diff --git a/net/bootp.c b/net/bootp.c
index 21ce7b22844..4bca50d75d7 100644
--- a/net/bootp.c
+++ b/net/bootp.c
@@ -644,8 +644,7 @@ BootpRequest (void)
 	pkt = NetTxPacket;
 	memset ((void*)pkt, 0, PKTSIZE);
 
-	NetSetEther(pkt, NetBcastAddr, PROT_IP);
-	pkt += ETHER_HDR_SIZE;
+	pkt += NetSetEther(pkt, NetBcastAddr, PROT_IP);
 
 	/*
 	 * Next line results in incorrect packet size being transmitted, resulting
@@ -791,8 +790,7 @@ static void DhcpSendRequestPkt(Bootp_t *bp_offer)
 	pkt = NetTxPacket;
 	memset ((void*)pkt, 0, PKTSIZE);
 
-	NetSetEther(pkt, NetBcastAddr, PROT_IP);
-	pkt += ETHER_HDR_SIZE;
+	pkt += NetSetEther(pkt, NetBcastAddr, PROT_IP);
 
 	iphdr = pkt;		/* We'll need this later to set proper pkt size */
 	pkt += IP_HDR_SIZE;
diff --git a/net/eth.c b/net/eth.c
index 7eae469684b..7f8afe7bcff 100644
--- a/net/eth.c
+++ b/net/eth.c
@@ -87,6 +87,14 @@ int eth_register(struct eth_device* dev)
 
 	if (!eth_devices) {
 		eth_current = eth_devices = dev;
+#ifdef CONFIG_NET_MULTI
+		/* update current ethernet name */
+		{
+			char *act = getenv("ethact");
+			if (act == NULL || strcmp(act, eth_current->name) != 0)
+				setenv("ethact", eth_current->name);
+		}
+#endif
 	} else {
 		for (d=eth_devices; d->next!=eth_devices; d=d->next);
 		d->next = dev;
@@ -221,6 +229,16 @@ int eth_initialize(bd_t *bis)
 			dev = dev->next;
 		} while(dev != eth_devices);
 
+#ifdef CONFIG_NET_MULTI
+		/* update current ethernet name */
+		if (eth_current) {
+			char *act = getenv("ethact");
+			if (act == NULL || strcmp(act, eth_current->name) != 0)
+				setenv("ethact", eth_current->name);
+		} else
+			setenv("ethact", NULL);
+#endif
+
 		putc ('\n');
 	}
 
@@ -326,12 +344,44 @@ void eth_try_another(int first_restart)
 
 	eth_current = eth_current->next;
 
+#ifdef CONFIG_NET_MULTI
+	/* update current ethernet name */
+	{
+		char *act = getenv("ethact");
+		if (act == NULL || strcmp(act, eth_current->name) != 0)
+			setenv("ethact", eth_current->name);
+	}
+#endif
+
 	if (first_failed == eth_current)
 	{
 		NetRestartWrap = 1;
 	}
 }
 
+#ifdef CONFIG_NET_MULTI
+void eth_set_current(void)
+{
+	char *act;
+	struct eth_device* old_current;
+
+	if (!eth_current)	/* XXX no current */
+		return;
+
+	act = getenv("ethact");
+	if (act != NULL) {
+		old_current = eth_current;
+		do {
+			if (strcmp(eth_current->name, act) == 0)
+				return;
+			eth_current = eth_current->next;
+		} while (old_current != eth_current);
+	}
+
+	setenv("ethact", eth_current->name);
+}
+#endif
+
 char *eth_get_name (void)
 {
 	return (eth_current ? eth_current->name : "unknown");
diff --git a/net/net.c b/net/net.c
index eaab5622a68..80d047519f3 100644
--- a/net/net.c
+++ b/net/net.c
@@ -121,6 +121,10 @@ uchar		NetBcastAddr[6] =	/* Ethernet bcast address		*/
 			{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 uchar		NetEtherNullAddr[6] =
 			{ 0, 0, 0, 0, 0, 0 };
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+uchar		NetCDPAddr[6] =	/* Ethernet bcast address		*/
+			{ 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };
+#endif
 int		NetState;		/* Network loop state			*/
 #ifdef CONFIG_NET_MULTI
 int		NetRestartWrap = 0;	/* Tried all network devices		*/
@@ -128,6 +132,9 @@ static int	NetRestarted = 0;	/* Network loop restarted		*/
 static int	NetDevExists = 0;	/* At least one device configured	*/
 #endif
 
+ushort		NetOurVLAN = ntohs(-1);	/* default is without VLAN		*/
+ushort		NetOurNativeVLAN = htons(-1);	/* dido				*/
+
 char		BootFile[128];		/* Boot File name			*/
 
 #if (CONFIG_COMMANDS & CFG_CMD_PING)
@@ -136,6 +143,10 @@ IPaddr_t	NetPingIP;		/* the ip address to ping 		*/
 static void PingStart(void);
 #endif
 
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+static void CDPStart(void);
+#endif
+
 volatile uchar	PktBuf[(PKTBUFSRX+1) * PKTSIZE_ALIGN + PKTALIGN];
 
 volatile uchar *NetRxPackets[PKTBUFSRX]; /* Receive packets			*/
@@ -170,8 +181,7 @@ void ArpRequest(void)
 #endif
 	pkt = NetTxPacket;
 
-	NetSetEther(pkt, NetBcastAddr, PROT_ARP);
-	pkt += ETHER_HDR_SIZE;
+	pkt += NetSetEther(pkt, NetBcastAddr, PROT_ARP);
 
 	arp = (ARP_t *)pkt;
 
@@ -196,7 +206,7 @@ void ArpRequest(void)
 	    NetArpWaitReplyIP = NetArpWaitPacketIP;
 
 	NetWriteIP((uchar*)&arp->ar_data[16], NetArpWaitReplyIP);
-	(void) eth_send(NetTxPacket, ETHER_HDR_SIZE + ARP_HDR_SIZE);
+	(void) eth_send(NetTxPacket, (pkt - NetTxPacket) + ARP_HDR_SIZE);
 }
 
 void ArpTimeoutCheck(void)
@@ -269,6 +279,7 @@ NetLoop(proto_t protocol)
 	}
 
 	eth_halt();
+	eth_set_current();
 	if(eth_init(bd) < 0)
 	    return(-1);
 
@@ -298,6 +309,8 @@ restart:
 		NetCopyIP(&NetOurIP, &bd->bi_ip_addr);
 		NetOurGatewayIP = getenv_IPaddr ("gatewayip");
 		NetOurSubnetMask= getenv_IPaddr ("netmask");
+		NetOurVLAN = getenv_VLAN("vlan");
+		NetOurNativeVLAN = getenv_VLAN("nvlan");
 
 		switch (protocol) {
 #if (CONFIG_COMMANDS & CFG_CMD_NFS)
@@ -324,6 +337,11 @@ restart:
 		 */
 		NetOurIP = 0;
 		NetServerIP = getenv_IPaddr ("serverip");
+ 		NetOurVLAN = getenv_VLAN("vlan");	/* VLANs must be read */
+ 		NetOurNativeVLAN = getenv_VLAN("nvlan");
+ 	case CDP:
+ 		NetOurVLAN = getenv_VLAN("vlan");	/* VLANs must be read */
+ 		NetOurNativeVLAN = getenv_VLAN("nvlan");
 		break;
 	default:
 		break;
@@ -378,6 +396,11 @@ restart:
 			NfsStart();
 			break;
 #endif
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+		case CDP:
+			CDPStart();
+			break;
+#endif
 		default:
 			break;
 		}
@@ -469,6 +492,9 @@ restart:
 					NetBootFileXferSize);
 				sprintf(buf, "%lx", NetBootFileXferSize);
 				setenv("filesize", buf);
+
+				sprintf(buf, "%lX", (unsigned long)load_addr);
+				setenv("fileaddr", buf);
 			}
 			eth_halt();
 			return NetBootFileXferSize;
@@ -496,12 +522,19 @@ startAgainHandler(uchar * pkt, unsigned dest, unsigned src, unsigned len)
 void
 NetStartAgain(void)
 {
+	DECLARE_GLOBAL_DATA_PTR;
+	char *s;
+
+	if ((s = getenv("netretry")) != NULL && *s == 'n') {
+		eth_halt();
+		NetState = NETLOOP_FAIL;
+		return;
+	}
+
 #ifndef CONFIG_NET_MULTI
 	NetSetTimeout(10 * CFG_HZ, startAgainTimeout);
 	NetSetHandler(startAgainHandler);
 #else
-	DECLARE_GLOBAL_DATA_PTR;
-
 	eth_halt();
 	eth_try_another(!NetRestarted);
 	eth_init(gd->bd);
@@ -559,6 +592,8 @@ NetSendPacket(volatile uchar * pkt, int len)
 int
 NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
 {
+	uchar *pkt;
+
 	/* convert to new style broadcast */
 	if (dest == 0)
 		dest = 0xFFFFFFFF;
@@ -573,16 +608,17 @@ NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
 #ifdef ET_DEBUG
 		printf("sending ARP for %08lx\n", dest);
 #endif
-
 		NetArpWaitPacketIP = dest;
 		NetArpWaitPacketMAC = ether;
-		NetSetEther (NetArpWaitTxPacket, NetArpWaitPacketMAC, PROT_IP);
-		NetSetIP (NetArpWaitTxPacket + ETHER_HDR_SIZE, dest, dport, sport, len);
-		memcpy(NetArpWaitTxPacket + ETHER_HDR_SIZE + IP_HDR_SIZE,
-			(uchar *)NetTxPacket + ETHER_HDR_SIZE + IP_HDR_SIZE, len);
+
+		pkt = NetArpWaitTxPacket;
+		pkt += NetSetEther (pkt, NetArpWaitPacketMAC, PROT_IP);
+
+		NetSetIP (pkt, dest, dport, sport, len);
+		memcpy(pkt + IP_HDR_SIZE, (uchar *)NetTxPacket + (pkt - (uchar *)NetArpWaitTxPacket) + IP_HDR_SIZE, len);
 
 		/* size of the waiting packet */
-		NetArpWaitTxPacketSize = ETHER_HDR_SIZE + IP_HDR_SIZE + len;
+		NetArpWaitTxPacketSize = (pkt - NetArpWaitTxPacket) + IP_HDR_SIZE + len;
 
 		/* and do the ARP request */
 		NetArpWaitTry = 1;
@@ -596,9 +632,10 @@ NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
 			dest, ether[0], ether[1], ether[2], ether[3], ether[4], ether[5]);
 #endif
 
-	NetSetEther (NetTxPacket, ether, PROT_IP);
-	NetSetIP (NetTxPacket + ETHER_HDR_SIZE, dest, dport, sport, len);
-	(void) eth_send(NetTxPacket, ETHER_HDR_SIZE + IP_HDR_SIZE + len);
+	pkt = (uchar *)NetTxPacket;
+	pkt += NetSetEther (pkt, ether, PROT_IP);
+	NetSetIP (pkt, dest, dport, sport, len);
+	(void) eth_send(NetTxPacket, (pkt - NetTxPacket) + IP_HDR_SIZE + len);
 
 	return 0;	/* transmited */
 }
@@ -611,6 +648,7 @@ int PingSend(void)
 	static uchar mac[6];
 	volatile IP_t *ip;
 	volatile ushort *s;
+	uchar *pkt;
 
 	/* XXX always send arp request */
 
@@ -623,9 +661,10 @@ int PingSend(void)
 	NetArpWaitPacketIP = NetPingIP;
 	NetArpWaitPacketMAC = mac;
 
-	NetSetEther(NetArpWaitTxPacket, mac, PROT_IP);
+	pkt = NetArpWaitTxPacket;
+	pkt += NetSetEther(pkt, mac, PROT_IP);
 
-	ip = (volatile IP_t *)(NetArpWaitTxPacket + ETHER_HDR_SIZE);
+	ip = (volatile IP_t *)pkt;
 
 	/*
 	 *	Construct an IP and ICMP header.  (need to set no fragment bit - XXX)
@@ -650,7 +689,7 @@ int PingSend(void)
 	s[1] = ~NetCksum((uchar *)s, 8/2);
 
 	/* size of the waiting packet */
-	NetArpWaitTxPacketSize = ETHER_HDR_SIZE + IP_HDR_SIZE_NO_UDP + 8;
+	NetArpWaitTxPacketSize = (pkt - NetArpWaitTxPacket) + IP_HDR_SIZE_NO_UDP + 8;
 
 	/* and do the ARP request */
 	NetArpWaitTry = 1;
@@ -681,6 +720,9 @@ PingHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 
 static void PingStart(void)
 {
+#if defined(CONFIG_NET_MULTI)
+	printf ("Using %s device\n", eth_get_name());
+#endif
 	NetSetTimeout (10 * CFG_HZ, PingTimeout);
 	NetSetHandler (PingHandler);
 
@@ -689,37 +731,454 @@ static void PingStart(void)
 
 #endif
 
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+
+#define CDP_DEVICE_ID_TLV		0x0001
+#define CDP_ADDRESS_TLV			0x0002
+#define CDP_PORT_ID_TLV			0x0003
+#define CDP_CAPABILITIES_TLV		0x0004
+#define CDP_VERSION_TLV			0x0005
+#define CDP_PLATFORM_TLV		0x0006
+#define CDP_NATIVE_VLAN_TLV		0x000a
+#define CDP_APPLIANCE_VLAN_TLV		0x000e
+#define CDP_TRIGGER_TLV			0x000f
+#define CDP_POWER_CONSUMPTION_TLV	0x0010
+#define CDP_SYSNAME_TLV			0x0014
+#define CDP_SYSOBJECT_TLV		0x0015
+#define CDP_MANAGEMENT_ADDRESS_TLV	0x0016
+
+#define CDP_TIMEOUT			(CFG_HZ/4)	/* one packet every 250ms */
+
+static int CDPSeq;
+static int CDPOK;
+
+ushort CDPNativeVLAN;
+ushort CDPApplianceVLAN;
+
+static const uchar CDP_SNAP_hdr[8] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
+
+static ushort CDP_compute_csum(const uchar *buff, ushort len)
+{
+	ushort csum;
+	int     odd;
+	ulong   result = 0;
+	ushort  leftover;
+
+	if (len > 0) {
+		odd = 1 & (ulong)buff;
+		if (odd) {
+			result = *buff << 8;
+			len--;
+			buff++;
+		}
+		while (len > 1) {
+			result += *((const ushort *)buff)++;
+			if (result & 0x80000000)
+				result = (result & 0xFFFF) + (result >> 16);
+			len -= 2;
+		}
+		if (len) {
+			leftover = (signed short)(*(const signed char *)buff);
+			/* * XXX CISCO SUCKS big time! (and blows too) */
+			result = (result & 0xffff0000) | ((result + leftover) & 0x0000ffff);
+		}
+		while (result >> 16)
+			result = (result & 0xFFFF) + (result >> 16);
+
+		if (odd)
+			result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
+	}
+
+	/* add up 16-bit and 17-bit words for 17+c bits */
+	result = (result & 0xffff) + (result >> 16);
+	/* add up 16-bit and 2-bit for 16+c bit */
+	result = (result & 0xffff) + (result >> 16);
+	/* add up carry.. */
+	result = (result & 0xffff) + (result >> 16);
+
+	/* negate */
+	csum = ~(ushort)result;
+
+	/* run time endian detection */
+	if (csum != htons(csum))	/* little endian */
+		csum = htons(csum);
+
+	return csum;
+}
+
+int CDPSendTrigger(void)
+{
+	volatile uchar *pkt;
+	volatile ushort *s;
+	volatile ushort *cp;
+	Ethernet_t *et;
+	char buf[32];
+	int len;
+	ushort chksum;
+
+	pkt = NetTxPacket;
+	et = (Ethernet_t *)pkt;
+
+	/* NOTE: trigger sent not on any VLAN */
+
+	/* form ethernet header */
+	memcpy(et->et_dest, NetCDPAddr, 6);
+	memcpy(et->et_src, NetOurEther, 6);
+
+	pkt += ETHER_HDR_SIZE;
+
+	/* SNAP header */
+	memcpy((uchar *)pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr));
+	pkt += sizeof(CDP_SNAP_hdr);
+
+	/* CDP header */
+	*pkt++ = 0x02;				/* CDP version 2 */
+	*pkt++ = 180;				/* TTL */
+	s = (volatile ushort *)pkt;
+	cp = s;
+	*s++ = htons(0);			/* checksum (0 for later calculation) */
+
+	/* CDP fields */
+#ifdef CONFIG_CDP_DEVICE_ID
+	*s++ = htons(CDP_DEVICE_ID_TLV);
+	*s++ = htons(CONFIG_CDP_DEVICE_ID);
+	memset(buf, 0, sizeof(buf));
+	sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%02X%02X%02X%02X%02X%02X",
+		NetOurEther[0] & 0xff, NetOurEther[1] & 0xff,
+		NetOurEther[2] & 0xff, NetOurEther[3] & 0xff,
+		NetOurEther[4] & 0xff, NetOurEther[5] & 0xff);
+	memcpy((uchar *)s, buf, 16);
+	s += 16 / 2;
+#endif
+
+#ifdef CONFIG_CDP_PORT_ID
+	*s++ = htons(CDP_PORT_ID_TLV);
+	memset(buf, 0, sizeof(buf));
+	sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
+	len = strlen(buf);
+	if (len & 1)	/* make it even */
+		len++;
+	*s++ = htons(len + 4);
+	memcpy((uchar *)s, buf, len);
+	s += len / 2;
+#endif
+
+#ifdef CONFIG_CDP_CAPABILITIES
+	*s++ = htons(CDP_CAPABILITIES_TLV);
+	*s++ = htons(8);
+	*(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
+	s += 2;
+#endif
+
+#ifdef CONFIG_CDP_VERSION
+	*s++ = htons(CDP_VERSION_TLV);
+	memset(buf, 0, sizeof(buf));
+	strcpy(buf, CONFIG_CDP_VERSION);
+	len = strlen(buf);
+	if (len & 1)	/* make it even */
+		len++;
+	*s++ = htons(len + 4);
+	memcpy((uchar *)s, buf, len);
+	s += len / 2;
+#endif
+
+#ifdef CONFIG_CDP_PLATFORM
+	*s++ = htons(CDP_PLATFORM_TLV);
+	memset(buf, 0, sizeof(buf));
+	strcpy(buf, CONFIG_CDP_PLATFORM);
+	len = strlen(buf);
+	if (len & 1)	/* make it even */
+		len++;
+	*s++ = htons(len + 4);
+	memcpy((uchar *)s, buf, len);
+	s += len / 2;
+#endif
+
+#ifdef CONFIG_CDP_TRIGGER
+	*s++ = htons(CDP_TRIGGER_TLV);
+	*s++ = htons(8);
+	*(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
+	s += 2;
+#endif
+
+#ifdef CONFIG_CDP_POWER_CONSUMPTION
+	*s++ = htons(CDP_POWER_CONSUMPTION_TLV);
+	*s++ = htons(6);
+	*s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
+#endif
+
+	/* length of ethernet packet */
+	len = (uchar *)s - ((uchar *)NetTxPacket + ETHER_HDR_SIZE);
+	et->et_protlen = htons(len);
+
+	len = ETHER_HDR_SIZE + sizeof(CDP_SNAP_hdr);
+	chksum = CDP_compute_csum((uchar *)NetTxPacket + len, (uchar *)s - (NetTxPacket + len));
+	if (chksum == 0)
+		chksum = 0xFFFF;
+	*cp = htons(chksum);
+
+	(void) eth_send(NetTxPacket, (uchar *)s - NetTxPacket);
+	return 0;
+}
+
+static void
+CDPTimeout (void)
+{
+	CDPSeq++;
+
+	if (CDPSeq < 3) {
+		NetSetTimeout (CDP_TIMEOUT, CDPTimeout);
+		CDPSendTrigger();
+		return;
+	}
+
+	/* if not OK try again */
+	if (!CDPOK)
+		NetStartAgain();
+	else
+		NetState = NETLOOP_SUCCESS;
+}
+
+static void
+CDPDummyHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
+{
+	/* nothing */
+}
+
+static void
+CDPHandler(const uchar * pkt, unsigned len)
+{
+	const uchar *t;
+	const ushort *ss;
+	ushort type, tlen;
+	uchar applid;
+	ushort vlan, nvlan;
+
+	/* minimum size? */
+	if (len < sizeof(CDP_SNAP_hdr) + 4)
+		goto pkt_short;
+
+	/* check for valid CDP SNAP header */
+	if (memcmp(pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)) != 0)
+		return;
+
+	pkt += sizeof(CDP_SNAP_hdr);
+	len -= sizeof(CDP_SNAP_hdr);
+
+	/* Version of CDP protocol must be >= 2 and TTL != 0 */
+	if (pkt[0] < 0x02 || pkt[1] == 0)
+		return;
+
+	/* if version is greater than 0x02 maybe we'll have a problem; output a warning */
+	if (pkt[0] != 0x02)
+		printf("** WARNING: CDP packet received with a protocol version %d > 2\n",
+				pkt[0] & 0xff);
+
+	if (CDP_compute_csum(pkt, len) != 0)
+		return;
+
+	pkt += 4;
+	len -= 4;
+
+	vlan = htons(-1);
+	nvlan = htons(-1);
+	while (len > 0) {
+		if (len < 4)
+			goto pkt_short;
+
+		ss = (const ushort *)pkt;
+		type = ntohs(ss[0]);
+		tlen = ntohs(ss[1]);
+		if (tlen > len) {
+			goto pkt_short;
+		}
+
+		pkt += tlen;
+		len -= tlen;
+
+		ss += 2;	/* point ss to the data of the TLV */
+		tlen -= 4;
+
+		switch (type) {
+			case CDP_DEVICE_ID_TLV:
+				break;
+			case CDP_ADDRESS_TLV:
+				break;
+			case CDP_PORT_ID_TLV:
+				break;
+			case CDP_CAPABILITIES_TLV:
+				break;
+			case CDP_VERSION_TLV:
+				break;
+			case CDP_PLATFORM_TLV:
+				break;
+			case CDP_NATIVE_VLAN_TLV:
+				nvlan = *ss;
+				break;
+			case CDP_APPLIANCE_VLAN_TLV:
+				t = (const uchar *)ss;
+				while (tlen > 0) {
+					if (tlen < 3)
+						goto pkt_short;
+
+					applid = t[0];
+					ss = (const ushort *)(t + 1);
+
+#ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
+					if (applid == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
+						vlan = *ss;
+#else
+					vlan = ntohs(*ss);	/* XXX will this work; dunno */
+#endif
+					t += 3; tlen -= 3;
+				}
+				break;
+			case CDP_TRIGGER_TLV:
+				break;
+			case CDP_POWER_CONSUMPTION_TLV:
+				break;
+			case CDP_SYSNAME_TLV:
+				break;
+			case CDP_SYSOBJECT_TLV:
+				break;
+			case CDP_MANAGEMENT_ADDRESS_TLV:
+				break;
+		}
+	}
+
+	CDPApplianceVLAN = vlan;
+	CDPNativeVLAN = nvlan;
+
+	CDPOK = 1;
+	return;
+
+ pkt_short:
+	printf("** CDP packet is too short\n");
+	return;
+}
+
+static void CDPStart(void)
+{
+#if defined(CONFIG_NET_MULTI)
+	printf ("Using %s device\n", eth_get_name());
+#endif
+	CDPSeq = 0;
+	CDPOK = 0;
+
+	CDPNativeVLAN = htons(-1);
+	CDPApplianceVLAN = htons(-1);
+
+	NetSetTimeout (CDP_TIMEOUT, CDPTimeout);
+	NetSetHandler (CDPDummyHandler);
+
+	CDPSendTrigger();
+}
+
+#endif
+
+
 void
-NetReceive(volatile uchar * pkt, int len)
+NetReceive(volatile uchar * inpkt, int len)
 {
 	Ethernet_t *et;
 	IP_t	*ip;
 	ARP_t	*arp;
 	IPaddr_t tmp;
 	int	x;
+	uchar *pkt;
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+	int iscdp;
+#endif
+	ushort cti = 0, vlanid = VLAN_NONE, myvlanid, mynvlanid;
+
+#ifdef ET_DEBUG
+	printf("packet received\n");
+#endif
 
-	NetRxPkt = pkt;
+	NetRxPkt = inpkt;
 	NetRxPktLen = len;
-	et = (Ethernet_t *)pkt;
+	et = (Ethernet_t *)inpkt;
+
+	/* too small packet? */
+	if (len < ETHER_HDR_SIZE)
+		return;
+
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+	/* keep track if packet is CDP */
+	iscdp = memcmp(et->et_dest, NetCDPAddr, 6) == 0;
+#endif
+
+	myvlanid = ntohs(NetOurVLAN);
+	if (myvlanid == (ushort)-1)
+		myvlanid = VLAN_NONE;
+	mynvlanid = ntohs(NetOurNativeVLAN);
+	if (mynvlanid == (ushort)-1)
+		mynvlanid = VLAN_NONE;
 
 	x = ntohs(et->et_protlen);
 
+#ifdef ET_DEBUG
+	printf("packet received\n");
+#endif
+
 	if (x < 1514) {
 		/*
 		 *	Got a 802 packet.  Check the other protocol field.
 		 */
 		x = ntohs(et->et_prot);
-		ip = (IP_t *)(pkt + E802_HDR_SIZE);
+
+		ip = (IP_t *)(inpkt + E802_HDR_SIZE);
 		len -= E802_HDR_SIZE;
-	} else {
-		ip = (IP_t *)(pkt + ETHER_HDR_SIZE);
+
+	} else if (x != PROT_VLAN) {	/* normal packet */
+		ip = (IP_t *)(inpkt + ETHER_HDR_SIZE);
 		len -= ETHER_HDR_SIZE;
+
+	} else {			/* VLAN packet */
+		VLAN_Ethernet_t *vet = (VLAN_Ethernet_t *)et;
+
+#ifdef ET_DEBUG
+		printf("VLAN packet received\n");
+#endif
+		/* too small packet? */
+		if (len < VLAN_ETHER_HDR_SIZE)
+			return;
+
+		/* if no VLAN active */
+		if ((ntohs(NetOurVLAN) & VLAN_IDMASK) == VLAN_NONE
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+				&& iscdp == 0
+#endif
+				)
+			return;
+
+		cti = ntohs(vet->vet_tag);
+		vlanid = cti & VLAN_IDMASK;
+		x = ntohs(vet->vet_type);
+
+		ip = (IP_t *)(inpkt + VLAN_ETHER_HDR_SIZE);
+		len -= VLAN_ETHER_HDR_SIZE;
 	}
 
 #ifdef ET_DEBUG
 	printf("Receive from protocol 0x%x\n", x);
 #endif
 
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+	if (iscdp) {
+		CDPHandler((uchar *)ip, len);
+		return;
+	}
+#endif
+
+	if ((myvlanid & VLAN_IDMASK) != VLAN_NONE) {
+		if (vlanid == VLAN_NONE)
+			vlanid = (mynvlanid & VLAN_IDMASK);
+		/* not matched? */
+		if (vlanid != (myvlanid & VLAN_IDMASK))
+			return;
+	}
+
 	switch (x) {
 
 	case PROT_ARP:
@@ -766,13 +1225,14 @@ NetReceive(volatile uchar * pkt, int len)
 #ifdef ET_DEBUG
 			puts ("Got ARP REQUEST, return our IP\n");
 #endif
-			NetSetEther((uchar *)et, et->et_src, PROT_ARP);
+			pkt = (uchar *)et;
+			pkt += NetSetEther(pkt, et->et_src, PROT_ARP);
 			arp->ar_op = htons(ARPOP_REPLY);
 			memcpy   (&arp->ar_data[10], &arp->ar_data[0], 6);
 			NetCopyIP(&arp->ar_data[16], &arp->ar_data[6]);
 			memcpy   (&arp->ar_data[ 0], NetOurEther, 6);
 			NetCopyIP(&arp->ar_data[ 6], &NetOurIP);
-			(void) eth_send((uchar *)et, ((uchar *)arp-pkt) + ARP_HDR_SIZE);
+			(void) eth_send((uchar *)et, (pkt - (uchar *)et) + ARP_HDR_SIZE);
 			return;
 
 		case ARPOP_REPLY:		/* arp reply */
@@ -963,6 +1423,7 @@ static int net_check_prereq (proto_t protocol)
 	case DHCP:
 	case RARP:
 	case BOOTP:
+	case CDP:
 			if (memcmp(NetOurEther, "\0\0\0\0\0\0", 6) == 0) {
 #ifdef CONFIG_NET_MULTI
 			    extern int eth_get_dev_index (void);
@@ -1016,17 +1477,42 @@ NetCksum(uchar * ptr, int len)
 	return (xsum & 0xffff);
 }
 
+int
+NetEthHdrSize(void)
+{
+	ushort myvlanid;
 
-void
+	myvlanid = ntohs(NetOurVLAN);
+	if (myvlanid == (ushort)-1)
+		myvlanid = VLAN_NONE;
+
+	return ((myvlanid & VLAN_IDMASK) == VLAN_NONE) ? ETHER_HDR_SIZE : VLAN_ETHER_HDR_SIZE;
+}
+
+int
 NetSetEther(volatile uchar * xet, uchar * addr, uint prot)
 {
 	Ethernet_t *et = (Ethernet_t *)xet;
+	ushort myvlanid;
+
+	myvlanid = ntohs(NetOurVLAN);
+	if (myvlanid == (ushort)-1)
+		myvlanid = VLAN_NONE;
 
 	memcpy (et->et_dest, addr, 6);
 	memcpy (et->et_src, NetOurEther, 6);
+	if ((myvlanid & VLAN_IDMASK) == VLAN_NONE) {
 	et->et_protlen = htons(prot);
-}
+		return ETHER_HDR_SIZE;
+	} else {
+		VLAN_Ethernet_t *vet = (VLAN_Ethernet_t *)xet;
 
+		vet->vet_vlan_type = htons(PROT_VLAN);
+		vet->vet_tag = htons((0 << 5) | (myvlanid & VLAN_IDMASK));
+		vet->vet_type = htons(prot);
+		return VLAN_ETHER_HDR_SIZE;
+	}
+}
 
 void
 NetSetIP(volatile uchar * xip, IPaddr_t dest, int dport, int sport, int len)
@@ -1079,13 +1565,12 @@ void copy_filename (uchar *dst, uchar *src, int size)
 
 void ip_to_string (IPaddr_t x, char *s)
 {
-    x = ntohl(x);
-    sprintf (s,"%d.%d.%d.%d",
-	(int)((x >> 24) & 0xff),
-	(int)((x >> 16) & 0xff),
-	(int)((x >>  8) & 0xff),
-	(int)((x >>  0) & 0xff)
-    );
+	x = ntohl (x);
+	sprintf (s, "%d.%d.%d.%d",
+		 (int) ((x >> 24) & 0xff),
+		 (int) ((x >> 16) & 0xff),
+		 (int) ((x >> 8) & 0xff), (int) ((x >> 0) & 0xff)
+		);
 }
 
 IPaddr_t string_to_ip(char *s)
@@ -1109,16 +1594,49 @@ IPaddr_t string_to_ip(char *s)
 	return (htonl(addr));
 }
 
+void VLAN_to_string(ushort x, char *s)
+{
+	x = ntohs(x);
+
+	if (x == (ushort)-1)
+		x = VLAN_NONE;
+
+	if (x == VLAN_NONE)
+		strcpy(s, "none");
+	else
+		sprintf(s, "%d", x & VLAN_IDMASK);
+}
+
+ushort string_to_VLAN(char *s)
+{
+	ushort id;
+
+	if (s == NULL)
+		return VLAN_NONE;
+
+	if (*s < '0' || *s > '9')
+		id = VLAN_NONE;
+	else
+		id = (ushort)simple_strtoul(s, NULL, 10);
+
+	return id;
+}
+
 void print_IPaddr (IPaddr_t x)
 {
-    char tmp[16];
+	char tmp[16];
 
-    ip_to_string(x, tmp);
+	ip_to_string (x, tmp);
 
-    puts(tmp);
+	puts (tmp);
 }
 
 IPaddr_t getenv_IPaddr (char *var)
 {
 	return (string_to_ip(getenv(var)));
 }
+
+ushort getenv_VLAN(char *var)
+{
+	return (string_to_VLAN(getenv(var)));
+}
diff --git a/net/nfs.c b/net/nfs.c
index 137b3bf8398..300c08fa967 100644
--- a/net/nfs.c
+++ b/net/nfs.c
@@ -192,7 +192,7 @@ rpc_req (int rpc_prog, int rpc_proc, uint32_t *data, int datalen)
 
 	pktlen = (char *)p + datalen*sizeof(uint32_t) - (char *)&pkt;
 
-	memcpy ((char *)NetTxPacket+ETHER_HDR_SIZE+IP_HDR_SIZE, (char *)&pkt, pktlen);
+	memcpy ((char *)NetTxPacket + NetEthHdrSize() + IP_HDR_SIZE, (char *)&pkt, pktlen);
 
 	if (rpc_prog == PROG_PORTMAP)
 		sport = SUNRPC_PORT;
diff --git a/net/rarp.c b/net/rarp.c
index f9df07658ea..1ba60e803cc 100644
--- a/net/rarp.c
+++ b/net/rarp.c
@@ -96,8 +96,7 @@ RarpRequest (void)
 	printf("RARP broadcast %d\n", ++RarpTry);
 	pkt = NetTxPacket;
 
-	NetSetEther(pkt, NetBcastAddr, PROT_RARP);
-	pkt += ETHER_HDR_SIZE;
+	pkt += NetSetEther(pkt, NetBcastAddr, PROT_RARP);
 
 	rarp = (ARP_t *)pkt;
 
@@ -114,7 +113,7 @@ RarpRequest (void)
 		rarp->ar_data[16 + i] = 0xff;
 	}
 
-	NetSendPacket(NetTxPacket, ETHER_HDR_SIZE + ARP_HDR_SIZE);
+	NetSendPacket(NetTxPacket, (pkt - NetTxPacket) + ARP_HDR_SIZE);
 
 	NetSetTimeout(TIMEOUT * CFG_HZ, RarpTimeout);
 	NetSetHandler(RarpHandler);
diff --git a/net/tftp.c b/net/tftp.c
index 2d3ee5f160e..5a5ae22ac85 100644
--- a/net/tftp.c
+++ b/net/tftp.c
@@ -111,7 +111,7 @@ TftpSend (void)
 	 *	We will always be sending some sort of packet, so
 	 *	cobble together the packet headers now.
 	 */
-	pkt = NetTxPacket + ETHER_HDR_SIZE + IP_HDR_SIZE;
+	pkt = NetTxPacket + NetEthHdrSize() + IP_HDR_SIZE;
 
 	switch (TftpState) {