Patches by Pantelis Antoniou, 30 Mar 2004:

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

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

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"),

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;

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 */

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);

doc/README.VLAN

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

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
  */

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);
 

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;

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");

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;
@@ -377,6 +395,11 @@ restart:
 		case NFS:
 			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)