Browse Source

Patches by Pantelis Antoniou, 30 Mar 2004:
add networking support for VLANs (802.1q), and CDP (Cisco Discovery Protocol)

wdenk 20 years ago
parent
commit
a3d991bd0d
14 changed files with 817 additions and 52 deletions
  1. 7 0
      CHANGELOG
  2. 63 0
      README
  3. 13 1
      common/cmd_nand.c
  4. 54 1
      common/cmd_net.c
  5. 2 0
      common/miiphyutil.c
  6. 15 0
      doc/README.VLAN
  7. 3 1
      include/cmd_confdefs.h
  8. 49 3
      include/net.h
  9. 2 4
      net/bootp.c
  10. 50 0
      net/eth.c
  11. 555 37
      net/net.c
  12. 1 1
      net/nfs.c
  13. 2 3
      net/rarp.c
  14. 1 1
      net/tftp.c

+ 7 - 0
CHANGELOG

@@ -20,6 +20,13 @@ Changes for U-Boot 1.1.1:
        interface.
        interface.
     3. We now correctly match the MII/RMII interface
     3. We now correctly match the MII/RMII interface
        configuration to what the PHY reports.
        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:
 * Patch by Yuli Barcohen, 28 Mar 2004:
   - Add support for MPC8272 family including MPC8247/8248/8271/8272
   - Add support for MPC8272 family including MPC8247/8248/8271/8272

+ 63 - 0
README

@@ -582,6 +582,7 @@ The following options need to be configured:
 		CFG_CMD_USB	* USB support
 		CFG_CMD_USB	* USB support
 		CFG_CMD_VFD	* VFD support (TRAB)
 		CFG_CMD_VFD	* VFD support (TRAB)
 		CFG_CMD_BSP	* Board SPecific functions
 		CFG_CMD_BSP	* Board SPecific functions
+		CFG_CMD_CDP	* Cisco Discover Protocol support
 		-----------------------------------------------
 		-----------------------------------------------
 		CFG_CMD_ALL	all
 		CFG_CMD_ALL	all
 
 
@@ -950,6 +951,48 @@ The following options need to be configured:
 		environment variable is passed as option 12 to
 		environment variable is passed as option 12 to
 		the DHCP server.
 		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
 - Status LED:	CONFIG_STATUS_LED
 
 
 		Several configurations allow to display the current
 		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
   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
 The following environment variables may be used and automatically
 updated by the network boot commands ("bootp" and "rarpboot"),
 updated by the network boot commands ("bootp" and "rarpboot"),

+ 13 - 1
common/cmd_nand.c

@@ -9,6 +9,7 @@
 #include <command.h>
 #include <command.h>
 #include <malloc.h>
 #include <malloc.h>
 #include <asm/io.h>
 #include <asm/io.h>
+#include <watchdog.h>
 
 
 #ifdef CONFIG_SHOW_BOOT_PROGRESS
 #ifdef CONFIG_SHOW_BOOT_PROGRESS
 # include <status_led.h>
 # include <status_led.h>
@@ -63,6 +64,7 @@ struct nand_oob_config {
 #define NANDRW_READ	0x01
 #define NANDRW_READ	0x01
 #define NANDRW_WRITE	0x00
 #define NANDRW_WRITE	0x00
 #define NANDRW_JFFS2	0x02
 #define NANDRW_JFFS2	0x02
+#define NANDRW_JFFS2_SKIP	0x04
 
 
 /*
 /*
  * Function Prototypes
  * 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))
 		else if (cmdtail && !strncmp(cmdtail, ".jffs2", 2))
 			cmd |= NANDRW_JFFS2;	/* skip bad blocks */
 			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
 #ifdef SXNI855T
 		/* need ".e" same as ".j" for compatibility with older units */
 		/* need ".e" same as ".j" for compatibility with older units */
 		else if (cmdtail && !strcmp(cmdtail, ".e"))
 		else if (cmdtail && !strcmp(cmdtail, ".e"))
@@ -258,7 +265,7 @@ U_BOOT_CMD(
 	"nand    - NAND sub-system\n",
 	"nand    - NAND sub-system\n",
 	"info  - show available NAND devices\n"
 	"info  - show available NAND devices\n"
 	"nand device [dev] - show or set current device\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"
 	"nand write[.jffs2] addr off size - read/write `size' bytes starting\n"
 	"    at offset `off' to/from memory address `addr'\n"
 	"    at offset `off' to/from memory address `addr'\n"
 	"nand erase [clean] [off size] - erase `size' bytes from\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
  *	1: NANDRW_READ			read, fail on bad block
  *	2: NANDRW_WRITE | NANDRW_JFFS2	write, skip bad blocks
  *	2: NANDRW_WRITE | NANDRW_JFFS2	write, skip bad blocks
  *	3: NANDRW_READ | NANDRW_JFFS2	read, data all 0xff for 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,
 static int nand_rw (struct nand_chip* nand, int cmd,
 	    size_t start, size_t len,
 	    size_t start, size_t len,
@@ -450,6 +458,10 @@ static int nand_rw (struct nand_chip* nand, int cmd,
 					}
 					}
 					continue;
 					continue;
 				}
 				}
+				else if (cmd == (NANDRW_READ | NANDRW_JFFS2 | NANDRW_JFFS2_SKIP)) {
+					start += erasesize;
+					continue;
+				}
 				else if (cmd == (NANDRW_WRITE | NANDRW_JFFS2)) {
 				else if (cmd == (NANDRW_WRITE | NANDRW_JFFS2)) {
 					/* skip bad block */
 					/* skip bad block */
 					start += erasesize;
 					start += erasesize;

+ 54 - 1
common/cmd_net.c

@@ -96,7 +96,7 @@ U_BOOT_CMD(
 
 
 static void netboot_update_env(void)
 static void netboot_update_env(void)
 {
 {
-    char tmp[16] ;
+    char tmp[22] ;
 
 
     if (NetOurGatewayIP) {
     if (NetOurGatewayIP) {
 	ip_to_string (NetOurGatewayIP, tmp);
 	ip_to_string (NetOurGatewayIP, tmp);
@@ -139,6 +139,16 @@ static void netboot_update_env(void)
     if (NetOurNISDomain[0])
     if (NetOurNISDomain[0])
 	setenv("domain", NetOurNISDomain);
 	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
 static int
 netboot_common (int proto, cmd_tbl_t *cmdtp, int argc, char *argv[])
 netboot_common (int proto, cmd_tbl_t *cmdtp, int argc, char *argv[])
@@ -238,4 +248,47 @@ U_BOOT_CMD(
 );
 );
 #endif	/* CFG_CMD_PING */
 #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 */
 #endif	/* CFG_CMD_NET */

+ 2 - 0
common/miiphyutil.c

@@ -203,6 +203,8 @@ int miiphy_link (unsigned char addr)
 {
 {
 	unsigned short reg;
 	unsigned short reg;
 
 
+	/* dummy read; needed to latch some phys */
+	(void)miiphy_read(addr, PHY_BMSR, &reg);
 	if (miiphy_read (addr, PHY_BMSR, &reg)) {
 	if (miiphy_read (addr, PHY_BMSR, &reg)) {
 		puts ("PHY_BMSR read failed, assuming no link\n");
 		puts ("PHY_BMSR read failed, assuming no link\n");
 		return (0);
 		return (0);

+ 15 - 0
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.

+ 3 - 1
include/cmd_confdefs.h

@@ -88,6 +88,7 @@
 #define CFG_CMD_ITEST	0x0040000000000000U	/* Integer (and string) test	*/
 #define CFG_CMD_ITEST	0x0040000000000000U	/* Integer (and string) test	*/
 #define CFG_CMD_NFS	0x0080000000000000U	/* NFS support			*/
 #define CFG_CMD_NFS	0x0080000000000000U	/* NFS support			*/
 #define CFG_CMD_REISER  0x0100000000000000U     /* Reiserfs support		*/
 #define CFG_CMD_REISER  0x0100000000000000U     /* Reiserfs support		*/
+#define CFG_CMD_CDP	0x0200000000000000U	/* Cisco Discovery Protocol 	*/
 
 
 #define CFG_CMD_ALL	0xFFFFFFFFFFFFFFFFU	/* ALL commands			*/
 #define CFG_CMD_ALL	0xFFFFFFFFFFFFFFFFU	/* ALL commands			*/
 
 
@@ -131,7 +132,8 @@
 			CFG_CMD_SDRAM	| \
 			CFG_CMD_SDRAM	| \
 			CFG_CMD_SPI	| \
 			CFG_CMD_SPI	| \
 			CFG_CMD_USB	| \
 			CFG_CMD_USB	| \
-			CFG_CMD_VFD	)
+			CFG_CMD_VFD	| \
+			CFG_CMD_CDP 	)
 
 
 /* Default configuration
 /* Default configuration
  */
  */

+ 49 - 3
include/net.h

@@ -110,7 +110,11 @@ struct eth_device {
 extern int eth_initialize(bd_t *bis);		/* Initialize network subsystem */
 extern int eth_initialize(bd_t *bis);		/* Initialize network subsystem */
 extern int eth_register(struct eth_device* dev);/* Register network device	*/
 extern int eth_register(struct eth_device* dev);/* Register network device	*/
 extern void eth_try_another(int first_restart);	/* Change the 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 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 void eth_set_enetaddr(int num, char* a);	/* Set new MAC address		*/
 
 
 extern int eth_init(bd_t *bis);			/* Initialize the device	*/
 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 ETHER_HDR_SIZE	14		/* Ethernet header size		*/
 #define E802_HDR_SIZE	22		/* 802 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_IP		0x0800		/* IP protocol			*/
 #define PROT_ARP	0x0806		/* IP ARP protocol		*/
 #define PROT_ARP	0x0806		/* IP ARP protocol		*/
 #define PROT_RARP	0x8035		/* 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_ICMP	 1	/* Internet Control Message Protocol	*/
 #define IPPROTO_UDP	17	/* User Datagram 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 unsigned		NetIPID;		/* IP ID (counting)		*/
 extern uchar		NetBcastAddr[6];	/* Ethernet boardcast address	*/
 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		*/
 extern int		NetState;		/* Network loop state		*/
 #define NETLOOP_CONTINUE	1
 #define NETLOOP_CONTINUE	1
 #define NETLOOP_RESTART		2
 #define NETLOOP_RESTART		2
@@ -306,7 +334,7 @@ extern int		NetState;		/* Network loop state		*/
 extern int		NetRestartWrap;		/* Tried all network devices	*/
 extern int		NetRestartWrap;		/* Tried all network devices	*/
 #endif
 #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 */
 /* from net/net.c */
 extern char	BootFile[128];			/* Boot File name		*/
 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 		*/
 extern IPaddr_t	NetPingIP;			/* the ip address to ping 		*/
 #endif
 #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 */
 /* Initialize the network adapter */
 extern int	NetLoop(proto_t);
 extern int	NetLoop(proto_t);
 
 
@@ -324,8 +358,11 @@ extern void	NetStop(void);
 /* Load failed.	 Start again. */
 /* Load failed.	 Start again. */
 extern void	NetStartAgain(void);
 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 */
 /* Set IP header */
 extern void	NetSetIP(volatile uchar *, IPaddr_t, int, int, int);
 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 */
 /* Convert a string to ip address */
 extern IPaddr_t string_to_ip(char *s);
 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 */
 /* read an IP address from a environment variable */
 extern IPaddr_t getenv_IPaddr (char *);
 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) */
 /* copy a filename (allow for "..." notation, limit length) */
 extern void	copy_filename (uchar *dst, uchar *src, int size);
 extern void	copy_filename (uchar *dst, uchar *src, int size);
 
 

+ 2 - 4
net/bootp.c

@@ -644,8 +644,7 @@ BootpRequest (void)
 	pkt = NetTxPacket;
 	pkt = NetTxPacket;
 	memset ((void*)pkt, 0, PKTSIZE);
 	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
 	 * Next line results in incorrect packet size being transmitted, resulting
@@ -791,8 +790,7 @@ static void DhcpSendRequestPkt(Bootp_t *bp_offer)
 	pkt = NetTxPacket;
 	pkt = NetTxPacket;
 	memset ((void*)pkt, 0, PKTSIZE);
 	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 */
 	iphdr = pkt;		/* We'll need this later to set proper pkt size */
 	pkt += IP_HDR_SIZE;
 	pkt += IP_HDR_SIZE;

+ 50 - 0
net/eth.c

@@ -87,6 +87,14 @@ int eth_register(struct eth_device* dev)
 
 
 	if (!eth_devices) {
 	if (!eth_devices) {
 		eth_current = eth_devices = dev;
 		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 {
 	} else {
 		for (d=eth_devices; d->next!=eth_devices; d=d->next);
 		for (d=eth_devices; d->next!=eth_devices; d=d->next);
 		d->next = dev;
 		d->next = dev;
@@ -221,6 +229,16 @@ int eth_initialize(bd_t *bis)
 			dev = dev->next;
 			dev = dev->next;
 		} while(dev != eth_devices);
 		} 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');
 		putc ('\n');
 	}
 	}
 
 
@@ -326,12 +344,44 @@ void eth_try_another(int first_restart)
 
 
 	eth_current = eth_current->next;
 	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)
 	if (first_failed == eth_current)
 	{
 	{
 		NetRestartWrap = 1;
 		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)
 char *eth_get_name (void)
 {
 {
 	return (eth_current ? eth_current->name : "unknown");
 	return (eth_current ? eth_current->name : "unknown");

+ 555 - 37
net/net.c

@@ -121,6 +121,10 @@ uchar		NetBcastAddr[6] =	/* Ethernet bcast address		*/
 			{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 			{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 uchar		NetEtherNullAddr[6] =
 uchar		NetEtherNullAddr[6] =
 			{ 0, 0, 0, 0, 0, 0 };
 			{ 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			*/
 int		NetState;		/* Network loop state			*/
 #ifdef CONFIG_NET_MULTI
 #ifdef CONFIG_NET_MULTI
 int		NetRestartWrap = 0;	/* Tried all network devices		*/
 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	*/
 static int	NetDevExists = 0;	/* At least one device configured	*/
 #endif
 #endif
 
 
+ushort		NetOurVLAN = ntohs(-1);	/* default is without VLAN		*/
+ushort		NetOurNativeVLAN = htons(-1);	/* dido				*/
+
 char		BootFile[128];		/* Boot File name			*/
 char		BootFile[128];		/* Boot File name			*/
 
 
 #if (CONFIG_COMMANDS & CFG_CMD_PING)
 #if (CONFIG_COMMANDS & CFG_CMD_PING)
@@ -136,6 +143,10 @@ IPaddr_t	NetPingIP;		/* the ip address to ping 		*/
 static void PingStart(void);
 static void PingStart(void);
 #endif
 #endif
 
 
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+static void CDPStart(void);
+#endif
+
 volatile uchar	PktBuf[(PKTBUFSRX+1) * PKTSIZE_ALIGN + PKTALIGN];
 volatile uchar	PktBuf[(PKTBUFSRX+1) * PKTSIZE_ALIGN + PKTALIGN];
 
 
 volatile uchar *NetRxPackets[PKTBUFSRX]; /* Receive packets			*/
 volatile uchar *NetRxPackets[PKTBUFSRX]; /* Receive packets			*/
@@ -170,8 +181,7 @@ void ArpRequest(void)
 #endif
 #endif
 	pkt = NetTxPacket;
 	pkt = NetTxPacket;
 
 
-	NetSetEther(pkt, NetBcastAddr, PROT_ARP);
-	pkt += ETHER_HDR_SIZE;
+	pkt += NetSetEther(pkt, NetBcastAddr, PROT_ARP);
 
 
 	arp = (ARP_t *)pkt;
 	arp = (ARP_t *)pkt;
 
 
@@ -196,7 +206,7 @@ void ArpRequest(void)
 	    NetArpWaitReplyIP = NetArpWaitPacketIP;
 	    NetArpWaitReplyIP = NetArpWaitPacketIP;
 
 
 	NetWriteIP((uchar*)&arp->ar_data[16], NetArpWaitReplyIP);
 	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)
 void ArpTimeoutCheck(void)
@@ -269,6 +279,7 @@ NetLoop(proto_t protocol)
 	}
 	}
 
 
 	eth_halt();
 	eth_halt();
+	eth_set_current();
 	if(eth_init(bd) < 0)
 	if(eth_init(bd) < 0)
 	    return(-1);
 	    return(-1);
 
 
@@ -298,6 +309,8 @@ restart:
 		NetCopyIP(&NetOurIP, &bd->bi_ip_addr);
 		NetCopyIP(&NetOurIP, &bd->bi_ip_addr);
 		NetOurGatewayIP = getenv_IPaddr ("gatewayip");
 		NetOurGatewayIP = getenv_IPaddr ("gatewayip");
 		NetOurSubnetMask= getenv_IPaddr ("netmask");
 		NetOurSubnetMask= getenv_IPaddr ("netmask");
+		NetOurVLAN = getenv_VLAN("vlan");
+		NetOurNativeVLAN = getenv_VLAN("nvlan");
 
 
 		switch (protocol) {
 		switch (protocol) {
 #if (CONFIG_COMMANDS & CFG_CMD_NFS)
 #if (CONFIG_COMMANDS & CFG_CMD_NFS)
@@ -324,6 +337,11 @@ restart:
 		 */
 		 */
 		NetOurIP = 0;
 		NetOurIP = 0;
 		NetServerIP = getenv_IPaddr ("serverip");
 		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;
 		break;
 	default:
 	default:
 		break;
 		break;
@@ -377,6 +395,11 @@ restart:
 		case NFS:
 		case NFS:
 			NfsStart();
 			NfsStart();
 			break;
 			break;
+#endif
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+		case CDP:
+			CDPStart();
+			break;
 #endif
 #endif
 		default:
 		default:
 			break;
 			break;
@@ -469,6 +492,9 @@ restart:
 					NetBootFileXferSize);
 					NetBootFileXferSize);
 				sprintf(buf, "%lx", NetBootFileXferSize);
 				sprintf(buf, "%lx", NetBootFileXferSize);
 				setenv("filesize", buf);
 				setenv("filesize", buf);
+
+				sprintf(buf, "%lX", (unsigned long)load_addr);
+				setenv("fileaddr", buf);
 			}
 			}
 			eth_halt();
 			eth_halt();
 			return NetBootFileXferSize;
 			return NetBootFileXferSize;
@@ -496,12 +522,19 @@ startAgainHandler(uchar * pkt, unsigned dest, unsigned src, unsigned len)
 void
 void
 NetStartAgain(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
 #ifndef CONFIG_NET_MULTI
 	NetSetTimeout(10 * CFG_HZ, startAgainTimeout);
 	NetSetTimeout(10 * CFG_HZ, startAgainTimeout);
 	NetSetHandler(startAgainHandler);
 	NetSetHandler(startAgainHandler);
 #else
 #else
-	DECLARE_GLOBAL_DATA_PTR;
-
 	eth_halt();
 	eth_halt();
 	eth_try_another(!NetRestarted);
 	eth_try_another(!NetRestarted);
 	eth_init(gd->bd);
 	eth_init(gd->bd);
@@ -559,6 +592,8 @@ NetSendPacket(volatile uchar * pkt, int len)
 int
 int
 NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
 NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
 {
 {
+	uchar *pkt;
+
 	/* convert to new style broadcast */
 	/* convert to new style broadcast */
 	if (dest == 0)
 	if (dest == 0)
 		dest = 0xFFFFFFFF;
 		dest = 0xFFFFFFFF;
@@ -573,16 +608,17 @@ NetSendUDPPacket(uchar *ether, IPaddr_t dest, int dport, int sport, int len)
 #ifdef ET_DEBUG
 #ifdef ET_DEBUG
 		printf("sending ARP for %08lx\n", dest);
 		printf("sending ARP for %08lx\n", dest);
 #endif
 #endif
-
 		NetArpWaitPacketIP = dest;
 		NetArpWaitPacketIP = dest;
 		NetArpWaitPacketMAC = ether;
 		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 */
 		/* 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 */
 		/* and do the ARP request */
 		NetArpWaitTry = 1;
 		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]);
 			dest, ether[0], ether[1], ether[2], ether[3], ether[4], ether[5]);
 #endif
 #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 */
 	return 0;	/* transmited */
 }
 }
@@ -611,6 +648,7 @@ int PingSend(void)
 	static uchar mac[6];
 	static uchar mac[6];
 	volatile IP_t *ip;
 	volatile IP_t *ip;
 	volatile ushort *s;
 	volatile ushort *s;
+	uchar *pkt;
 
 
 	/* XXX always send arp request */
 	/* XXX always send arp request */
 
 
@@ -623,9 +661,10 @@ int PingSend(void)
 	NetArpWaitPacketIP = NetPingIP;
 	NetArpWaitPacketIP = NetPingIP;
 	NetArpWaitPacketMAC = mac;
 	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)
 	 *	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);
 	s[1] = ~NetCksum((uchar *)s, 8/2);
 
 
 	/* size of the waiting packet */
 	/* 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 */
 	/* and do the ARP request */
 	NetArpWaitTry = 1;
 	NetArpWaitTry = 1;
@@ -681,6 +720,9 @@ PingHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 
 
 static void PingStart(void)
 static void PingStart(void)
 {
 {
+#if defined(CONFIG_NET_MULTI)
+	printf ("Using %s device\n", eth_get_name());
+#endif
 	NetSetTimeout (10 * CFG_HZ, PingTimeout);
 	NetSetTimeout (10 * CFG_HZ, PingTimeout);
 	NetSetHandler (PingHandler);
 	NetSetHandler (PingHandler);
 
 
@@ -689,37 +731,454 @@ static void PingStart(void)
 
 
 #endif
 #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
 void
-NetReceive(volatile uchar * pkt, int len)
+NetReceive(volatile uchar * inpkt, int len)
 {
 {
 	Ethernet_t *et;
 	Ethernet_t *et;
 	IP_t	*ip;
 	IP_t	*ip;
 	ARP_t	*arp;
 	ARP_t	*arp;
 	IPaddr_t tmp;
 	IPaddr_t tmp;
 	int	x;
 	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;
 	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);
 	x = ntohs(et->et_protlen);
 
 
+#ifdef ET_DEBUG
+	printf("packet received\n");
+#endif
+
 	if (x < 1514) {
 	if (x < 1514) {
 		/*
 		/*
 		 *	Got a 802 packet.  Check the other protocol field.
 		 *	Got a 802 packet.  Check the other protocol field.
 		 */
 		 */
 		x = ntohs(et->et_prot);
 		x = ntohs(et->et_prot);
-		ip = (IP_t *)(pkt + E802_HDR_SIZE);
+
+		ip = (IP_t *)(inpkt + E802_HDR_SIZE);
 		len -= 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;
 		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
 #ifdef ET_DEBUG
 	printf("Receive from protocol 0x%x\n", x);
 	printf("Receive from protocol 0x%x\n", x);
 #endif
 #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) {
 	switch (x) {
 
 
 	case PROT_ARP:
 	case PROT_ARP:
@@ -766,13 +1225,14 @@ NetReceive(volatile uchar * pkt, int len)
 #ifdef ET_DEBUG
 #ifdef ET_DEBUG
 			puts ("Got ARP REQUEST, return our IP\n");
 			puts ("Got ARP REQUEST, return our IP\n");
 #endif
 #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);
 			arp->ar_op = htons(ARPOP_REPLY);
 			memcpy   (&arp->ar_data[10], &arp->ar_data[0], 6);
 			memcpy   (&arp->ar_data[10], &arp->ar_data[0], 6);
 			NetCopyIP(&arp->ar_data[16], &arp->ar_data[6]);
 			NetCopyIP(&arp->ar_data[16], &arp->ar_data[6]);
 			memcpy   (&arp->ar_data[ 0], NetOurEther, 6);
 			memcpy   (&arp->ar_data[ 0], NetOurEther, 6);
 			NetCopyIP(&arp->ar_data[ 6], &NetOurIP);
 			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;
 			return;
 
 
 		case ARPOP_REPLY:		/* arp reply */
 		case ARPOP_REPLY:		/* arp reply */
@@ -963,6 +1423,7 @@ static int net_check_prereq (proto_t protocol)
 	case DHCP:
 	case DHCP:
 	case RARP:
 	case RARP:
 	case BOOTP:
 	case BOOTP:
+	case CDP:
 			if (memcmp(NetOurEther, "\0\0\0\0\0\0", 6) == 0) {
 			if (memcmp(NetOurEther, "\0\0\0\0\0\0", 6) == 0) {
 #ifdef CONFIG_NET_MULTI
 #ifdef CONFIG_NET_MULTI
 			    extern int eth_get_dev_index (void);
 			    extern int eth_get_dev_index (void);
@@ -1016,17 +1477,42 @@ NetCksum(uchar * ptr, int len)
 	return (xsum & 0xffff);
 	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)
 NetSetEther(volatile uchar * xet, uchar * addr, uint prot)
 {
 {
 	Ethernet_t *et = (Ethernet_t *)xet;
 	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_dest, addr, 6);
 	memcpy (et->et_src, NetOurEther, 6);
 	memcpy (et->et_src, NetOurEther, 6);
+	if ((myvlanid & VLAN_IDMASK) == VLAN_NONE) {
 	et->et_protlen = htons(prot);
 	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
 void
 NetSetIP(volatile uchar * xip, IPaddr_t dest, int dport, int sport, int len)
 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)
 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)
 IPaddr_t string_to_ip(char *s)
@@ -1109,16 +1594,49 @@ IPaddr_t string_to_ip(char *s)
 	return (htonl(addr));
 	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)
 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)
 IPaddr_t getenv_IPaddr (char *var)
 {
 {
 	return (string_to_ip(getenv(var)));
 	return (string_to_ip(getenv(var)));
 }
 }
+
+ushort getenv_VLAN(char *var)
+{
+	return (string_to_VLAN(getenv(var)));
+}

+ 1 - 1
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;
 	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)
 	if (rpc_prog == PROG_PORTMAP)
 		sport = SUNRPC_PORT;
 		sport = SUNRPC_PORT;

+ 2 - 3
net/rarp.c

@@ -96,8 +96,7 @@ RarpRequest (void)
 	printf("RARP broadcast %d\n", ++RarpTry);
 	printf("RARP broadcast %d\n", ++RarpTry);
 	pkt = NetTxPacket;
 	pkt = NetTxPacket;
 
 
-	NetSetEther(pkt, NetBcastAddr, PROT_RARP);
-	pkt += ETHER_HDR_SIZE;
+	pkt += NetSetEther(pkt, NetBcastAddr, PROT_RARP);
 
 
 	rarp = (ARP_t *)pkt;
 	rarp = (ARP_t *)pkt;
 
 
@@ -114,7 +113,7 @@ RarpRequest (void)
 		rarp->ar_data[16 + i] = 0xff;
 		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);
 	NetSetTimeout(TIMEOUT * CFG_HZ, RarpTimeout);
 	NetSetHandler(RarpHandler);
 	NetSetHandler(RarpHandler);

+ 1 - 1
net/tftp.c

@@ -111,7 +111,7 @@ TftpSend (void)
 	 *	We will always be sending some sort of packet, so
 	 *	We will always be sending some sort of packet, so
 	 *	cobble together the packet headers now.
 	 *	cobble together the packet headers now.
 	 */
 	 */
-	pkt = NetTxPacket + ETHER_HDR_SIZE + IP_HDR_SIZE;
+	pkt = NetTxPacket + NetEthHdrSize() + IP_HDR_SIZE;
 
 
 	switch (TftpState) {
 	switch (TftpState) {