[U-Boot-Users] [patch #9/11] VLAN/CDP networking support.

30 Mar 2004

Hello
The following patch adds 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.
Regards
Pantelis
README                 |   63 +++++
 common/cmd_net.c       |   55 ++++
 include/cmd_confdefs.h |    4 
 include/net.h          |   52 ++++
 net/bootp.c            |    6 
 net/eth.c              |   50 ++++
 net/net.c              |  577 ++++++++++++++++++++++++++++++++++++++++++++++---
 net/nfs.c              |    2 
 net/rarp.c             |    5 
 net/tftp.c             |    2 
 10 files changed, 775 insertions(+), 41 deletions(-)
diff -ruNb u-boot-original/README u-boot-net/README

--- u-boot-original/README	2004-03-30 14:37:28.395972064 +0300
+++ u-boot-net/README	2004-03-30 16:48:11.265674640 +0300
@@ -581,6 +581,7 @@
    	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
@@ -949,6 +950,48 @@
    	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
@@ -2182,6 +2225,26 @@
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 -ruNb u-boot-original/common/cmd_net.c u-boot-net/common/cmd_net.c
--- u-boot-original/common/cmd_net.c	2004-03-30 14:37:28.664931176 +0300
+++ u-boot-net/common/cmd_net.c	2004-03-30 15:41:00.406458384 +0300
@@ -96,7 +96,7 @@
static void netboot_update_env(void)
 {
-    char tmp[16] ;
+    char tmp[22] ;
if (NetOurGatewayIP) {
    ip_to_string (NetOurGatewayIP, tmp);
@@ -139,6 +139,16 @@
     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 @@
 );
 #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 -ruNb u-boot-original/include/cmd_confdefs.h u-boot-net/include/cmd_confdefs.h
--- u-boot-original/include/cmd_confdefs.h	2004-03-30 14:37:28.749918256 +0300
+++ u-boot-net/include/cmd_confdefs.h	2004-03-30 15:41:00.407458232 +0300
@@ -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 -ruNb u-boot-original/include/net.h u-boot-net/include/net.h
--- u-boot-original/include/net.h	2004-03-30 14:37:28.756917192 +0300
+++ u-boot-net/include/net.h	2004-03-30 15:41:00.408458080 +0300
@@ -110,7 +110,11 @@
 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 @@
#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 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		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 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 @@
 /* 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 @@
 /* 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 -ruNb u-boot-original/net/bootp.c u-boot-net/net/bootp.c
--- u-boot-original/net/bootp.c	2004-03-30 14:37:28.828906248 +0300
+++ u-boot-net/net/bootp.c	2004-03-30 15:41:00.409457928 +0300
@@ -644,8 +644,7 @@
    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 @@
    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 -ruNb u-boot-original/net/eth.c u-boot-net/net/eth.c
--- u-boot-original/net/eth.c	2004-03-30 14:37:28.828906248 +0300
+++ u-boot-net/net/eth.c	2004-03-30 15:41:00.410457776 +0300
@@ -87,6 +87,14 @@
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 @@
    		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 @@
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 -ruNb u-boot-original/net/net.c u-boot-net/net/net.c
--- u-boot-original/net/net.c	2004-03-30 14:37:28.829906096 +0300
+++ u-boot-net/net/net.c	2004-03-30 15:41:00.412457472 +0300
@@ -121,6 +121,10 @@
    		{ 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	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,14 @@
 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 +185,7 @@
 #endif
    pkt = NetTxPacket;
-	NetSetEther(pkt, NetBcastAddr, PROT_ARP);
-	pkt += ETHER_HDR_SIZE;
+	pkt += NetSetEther(pkt, NetBcastAddr, PROT_ARP);
arp = (ARP_t *)pkt;
@@ -196,7 +210,7 @@
        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 +283,7 @@
    }
eth_halt();
+	eth_set_current();
    if(eth_init(bd) < 0)
        return(-1);
@@ -298,6 +313,8 @@
    	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 +341,11 @@
    	 */
    	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 +400,11 @@
    		NfsStart();
    		break;
 #endif
+#if (CONFIG_COMMANDS & CFG_CMD_CDP)
+		case CDP:
+			CDPStart();
+			break;
+#endif
    	default:
    		break;
    	}
@@ -469,6 +496,9 @@
    				NetBootFileXferSize);
    			sprintf(buf, "%lx", NetBootFileXferSize);
    			setenv("filesize", buf);
+
+				sprintf(buf, "%lX", (unsigned long)load_addr);
+				setenv("fileaddr", buf);
    		}
    		eth_halt();
    		return NetBootFileXferSize;
@@ -496,12 +526,19 @@
 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 +596,8 @@
 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 +612,17 @@
 #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 +636,10 @@
    		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 +652,7 @@
    static uchar mac[6];
    volatile IP_t *ip;
    volatile ushort *s;
+	uchar *pkt;
/* XXX always send arp request */
@@ -623,9 +665,10 @@
    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 +693,7 @@
    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 +724,9 @@
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 +735,454 @@
#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;
-	NetRxPkt = pkt;
+#ifdef ET_DEBUG
+	printf("packet received\n");
+#endif
+
+	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 +1229,14 @@
 #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 +1427,7 @@
    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 +1481,42 @@
    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)
@@ -1109,6 +1599,34 @@
    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];
@@ -1122,3 +1640,8 @@
 {
    return (string_to_ip(getenv(var)));
 }
+
+ushort getenv_VLAN(char *var)
+{
+	return (string_to_VLAN(getenv(var)));
+}
diff -ruNb u-boot-original/net/nfs.c u-boot-net/net/nfs.c
--- u-boot-original/net/nfs.c	2004-03-30 14:37:28.829906096 +0300
+++ u-boot-net/net/nfs.c	2004-03-30 15:41:00.413457320 +0300
@@ -192,7 +192,7 @@
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 -ruNb u-boot-original/net/rarp.c u-boot-net/net/rarp.c
--- u-boot-original/net/rarp.c	2004-03-30 14:37:28.829906096 +0300
+++ u-boot-net/net/rarp.c	2004-03-30 15:41:00.414457168 +0300
@@ -96,8 +96,7 @@
    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 @@
    	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 -ruNb u-boot-original/net/tftp.c u-boot-net/net/tftp.c
--- u-boot-original/net/tftp.c	2004-03-30 14:37:28.829906096 +0300
+++ u-boot-net/net/tftp.c	2004-03-30 15:41:00.414457168 +0300
@@ -111,7 +111,7 @@
     *	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) {

    

[U-Boot-Users] [patch #9/11] VLAN/CDP networking support.

Pantelis Antoniou