multicast tftp: RFC2090

Implemented IETF RFC2090, Multicast TFTP.  Initial implementation
on Realtek RTL8139 and Freescale TSEC.

Signed-off-by: David Updegraff <dave@cray.com>
Signed-off-by: Ben Warren <bwarren@qstreams.com>

7 files changed, 360 insertions, 7 deletions

diff --git a/README b/README
index 6af6aaa..291b304 100644
--- a/README
+++ b/README
@@ -1066,6 +1066,16 @@ The following options need to be configured:
 		Defines a default value for theIP address of a TFTP
 		server to contact when using the "tftboot" command.
 
+- Multicast TFTP Mode:
+		CONFIG_MCAST_TFTP
+
+		Defines whether you want to support multicast TFTP as per
+		rfc-2090; for example to work with atftp.  Lets lots of targets
+		tftp down the same boot image concurrently.  Note: the ethernet
+		driver in use must provide a function: mcast() to join/leave a
+		multicast group.
+
+		CONFIG_BOOTP_RANDOM_DELAY
 - BOOTP Recovery Mode:
 		CONFIG_BOOTP_RANDOM_DELAY
 
diff --git a/drivers/rtl8139.c b/drivers/rtl8139.c
index 9045523..3e259b6 100644
--- a/drivers/rtl8139.c
+++ b/drivers/rtl8139.c
@@ -193,6 +193,10 @@ static void rtl_reset(struct eth_device *dev);
 static int rtl_transmit(struct eth_device *dev, volatile void *packet, int length);
 static int rtl_poll(struct eth_device *dev);
 static void rtl_disable(struct eth_device *dev);
+#ifdef CONFIG_MCAST_TFTP/*  This driver already accepts all b/mcast */
+static int rtl_bcast_addr (struct eth_device *dev, u8 bcast_mac, u8 set) 
+ { return (0); }
+#endif
 
 static struct pci_device_id supported[] = {
        {PCI_VENDOR_ID_REALTEK, PCI_DEVICE_ID_REALTEK_8139},
@@ -228,6 +232,9 @@ int rtl8139_initialize(bd_t *bis)
 		dev->halt = rtl_disable;
 		dev->send = rtl_transmit;
 		dev->recv = rtl_poll;
+#ifdef CONFIG_MCAST_TFTP
+		dev->mcast = rtl_bcast_addr;
+#endif
 
 		eth_register (dev);
 
diff --git a/drivers/tsec.c b/drivers/tsec.c
index c011123..fd21ed4 100644
--- a/drivers/tsec.c
+++ b/drivers/tsec.c
@@ -129,6 +129,9 @@ static int tsec_miiphy_write(char *devname, unsigned char addr,
 			     unsigned char reg, unsigned short value);
 static int tsec_miiphy_read(char *devname, unsigned char addr,
 			    unsigned char reg, unsigned short *value);
+#ifdef CONFIG_MCAST_TFTP
+static int tsec_mcast_addr (struct eth_device *dev, u8 mcast_mac, u8 set);
+#endif
 
 /* Initialize device structure. Returns success if PHY
  * initialization succeeded (i.e. if it recognizes the PHY)
@@ -167,6 +170,9 @@ int tsec_initialize(bd_t * bis, int index, char *devname)
 	dev->halt = tsec_halt;
 	dev->send = tsec_send;
 	dev->recv = tsec_recv;
+#ifdef CONFIG_MCAST_TFTP
+	dev->mcast = tsec_mcast_addr;
+#endif
 
 	/* Tell u-boot to get the addr from the env */
 	for (i = 0; i < 6; i++)
@@ -1539,4 +1545,46 @@ static int tsec_miiphy_write(char *devname, unsigned char addr,
 
 #endif
 
+#ifdef CONFIG_MCAST_TFTP
+
+/* CREDITS: linux gianfar driver, slightly adjusted... thanx. */
+
+/* Set the appropriate hash bit for the given addr */
+
+/* The algorithm works like so:
+ * 1) Take the Destination Address (ie the multicast address), and
+ * do a CRC on it (little endian), and reverse the bits of the
+ * result.
+ * 2) Use the 8 most significant bits as a hash into a 256-entry
+ * table.  The table is controlled through 8 32-bit registers:
+ * gaddr0-7.  gaddr0's MSB is entry 0, and gaddr7's LSB is
+ * gaddr7.  This means that the 3 most significant bits in the
+ * hash index which gaddr register to use, and the 5 other bits
+ * indicate which bit (assuming an IBM numbering scheme, which
+ * for PowerPC (tm) is usually the case) in the tregister holds
+ * the entry. */
+static int
+tsec_mcast_addr (struct eth_device *dev, u8 mcast_mac, u8 set)
+{
+ struct tsec_private *priv = privlist[1];
+ volatile tsec_t *regs = priv->regs;
+ volatile u32  *reg_array, value;
+ u8 result, whichbit, whichreg;
+
+	result = (u8)((ether_crc(MAC_ADDR_LEN,mcast_mac) >> 24) & 0xff);
+	whichbit = result & 0x1f;	/* the 5 LSB = which bit to set */
+	whichreg = result >> 5;		/* the 3 MSB = which reg to set it in */
+	value = (1 << (31-whichbit));
+
+	reg_array = &(regs->hash.gaddr0);
+
+	if (set) {
+		reg_array[whichreg] |= value;
+	} else {
+		reg_array[whichreg] &= ~value;
+	}
+	return 0;
+}
+#endif /* Multicast TFTP ? */
+
 #endif /* CONFIG_TSEC_ENET */
diff --git a/include/net.h b/include/net.h
index aa58e33..603452a 100644
--- a/include/net.h
+++ b/include/net.h
@@ -99,10 +99,12 @@ struct eth_device {
 	int state;
 
 	int  (*init) (struct eth_device*, bd_t*);
-	int  (*send) (struct eth_device*, volatile void* pachet, int length);
+	int  (*send) (struct eth_device*, volatile void* packet, int length);
 	int  (*recv) (struct eth_device*);
 	void (*halt) (struct eth_device*);
-
+#ifdef CONFIG_MCAST_TFTP
+	int (*mcast) (struct eth_device*, u32 ip, u8 set);
+#endif
 	struct eth_device *next;
 	void *priv;
 };
@@ -124,6 +126,11 @@ extern int eth_rx(void);			/* Check for received packets	*/
 extern void eth_halt(void);			/* stop SCC			*/
 extern char *eth_get_name(void);		/* get name of current device	*/
 
+#ifdef CONFIG_MCAST_TFTP
+int eth_mcast_join( IPaddr_t mcast_addr, u8 join);
+u32 ether_crc (size_t len, unsigned char const *p);
+#endif
+
 
 /**********************************************************************/
 /*
diff --git a/net/eth.c b/net/eth.c
index c8f92a5..9ccbcca 100644
--- a/net/eth.c
+++ b/net/eth.c
@@ -353,6 +353,51 @@ void eth_set_enetaddr(int num, char *addr) {
 
 	memcpy(dev->enetaddr, enetaddr, 6);
 }
+#ifdef CONFIG_MCAST_TFTP
+/* Multicast.
+ * mcast_addr: multicast ipaddr from which multicast Mac is made
+ * join: 1=join, 0=leave.  
+ */
+int eth_mcast_join( IPaddr_t mcast_ip, u8 join)
+{
+ u8 mcast_mac[6];
+	if (!eth_current || !eth_current->mcast) 
+		return -1;
+	mcast_mac[5] = htonl(mcast_ip) & 0xff;
+	mcast_mac[4] = (htonl(mcast_ip)>>8) & 0xff;
+	mcast_mac[3] = (htonl(mcast_ip)>>16) & 0x7f;
+	mcast_mac[2] = 0x5e;
+	mcast_mac[1] = 0x0;
+	mcast_mac[0] = 0x1;
+	return eth_current->mcast(eth_current, mcast_mac, join);
+}
+
+/* the 'way' for ethernet-CRC-32. Spliced in from Linux lib/crc32.c 
+ * and this is the ethernet-crc method needed for TSEC -- and perhaps 
+ * some other adapter -- hash tables
+ */
+#define CRCPOLY_LE 0xedb88320
+u32 ether_crc (size_t len, unsigned char const *p)
+{
+	int i;
+	u32 crc;
+	crc = ~0;
+	while (len--) {
+		crc ^= *p++;
+		for (i = 0; i < 8; i++)
+			crc = (crc >> 1) ^ ((crc & 1) ? CRCPOLY_LE : 0);
+	}
+	/* an reverse the bits, cuz of way they arrive -- last-first */
+	crc = (crc >> 16) | (crc << 16);
+	crc = (crc >> 8 & 0x00ff00ff) | (crc << 8 & 0xff00ff00);
+	crc = (crc >> 4 & 0x0f0f0f0f) | (crc << 4 & 0xf0f0f0f0);
+	crc = (crc >> 2 & 0x33333333) | (crc << 2 & 0xcccccccc);
+	crc = (crc >> 1 & 0x55555555) | (crc << 1 & 0xaaaaaaaa);
+	return crc;
+}
+
+#endif
+
 
 int eth_init(bd_t *bis)
 {
diff --git a/net/net.c b/net/net.c
index e9d7757..2b11a13 100644
--- a/net/net.c
+++ b/net/net.c
@@ -118,6 +118,10 @@ char		NetOurHostName[32]={0,};	/* Our hostname			*/
 char		NetOurRootPath[64]={0,};	/* Our bootpath			*/
 ushort		NetBootFileSize=0;		/* Our bootfile size in blocks	*/
 
+#ifdef CONFIG_MCAST_TFTP	/* Multicast TFTP */
+IPaddr_t Mcast_addr;
+#endif
+
 /** END OF BOOTP EXTENTIONS **/
 
 ulong		NetBootFileXferSize;	/* The actual transferred size of the bootfile (in bytes) */
@@ -1386,6 +1390,9 @@ NetReceive(volatile uchar * inpkt, int len)
 		}
 		tmp = NetReadIP(&ip->ip_dst);
 		if (NetOurIP && tmp != NetOurIP && tmp != 0xFFFFFFFF) {
+#ifdef CONFIG_MCAST_TFTP
+			if (Mcast_addr != tmp) 
+#endif
 			return;
 		}
 		/*
@@ -1492,6 +1499,7 @@ NetReceive(volatile uchar * inpkt, int len)
 		}
 #endif
 
+
 #ifdef CONFIG_NETCONSOLE
 		nc_input_packet((uchar *)ip +IP_HDR_SIZE,
 						ntohs(ip->udp_dst),
diff --git a/net/tftp.c b/net/tftp.c
index d56e30b..95fee77 100644
--- a/net/tftp.c
+++ b/net/tftp.c
@@ -61,10 +61,43 @@ static char *tftp_filename;
 extern flash_info_t flash_info[];
 #endif
 
+/* 512 is poor choice for ethernet, MTU is typically 1500. 
+ * Minus eth.hdrs thats 1468.  Can get 2x better throughput with 
+ * almost-MTU block sizes.  At least try... fall back to 512 if need be.
+ */
+#define TFTP_MTU_BLOCKSIZE 1468
+static unsigned short TftpBlkSize=TFTP_BLOCK_SIZE;
+static unsigned short TftpBlkSizeOption=TFTP_MTU_BLOCKSIZE;
+
+#ifdef CONFIG_MCAST_TFTP
+#include <malloc.h>
+#define MTFTP_BITMAPSIZE	0x1000
+static unsigned *Bitmap;
+static int PrevBitmapHole,Mapsize=MTFTP_BITMAPSIZE;
+static uchar ProhibitMcast=0, MasterClient=0;
+static uchar Multicast=0;
+extern IPaddr_t Mcast_addr;
+static int Mcast_port;
+static ulong TftpEndingBlock; /* can get 'last' block before done..*/
+
+static void parse_multicast_oack(char *pkt,int len);
+
+static void
+mcast_cleanup(void)
+{
+	if (Mcast_addr) eth_mcast_join(Mcast_addr, 0);
+	if (Bitmap) free(Bitmap);
+	Bitmap=NULL;
+	Mcast_addr = Multicast = Mcast_port = 0;
+	TftpEndingBlock = -1;
+}
+
+#endif	/* CONFIG_MCAST_TFTP */
+
 static __inline__ void
 store_block (unsigned block, uchar * src, unsigned len)
 {
-	ulong offset = block * TFTP_BLOCK_SIZE + TftpBlockWrapOffset;
+	ulong offset = block * TftpBlkSize + TftpBlockWrapOffset;
 	ulong newsize = offset + len;
 #ifdef CFG_DIRECT_FLASH_TFTP
 	int i, rc = 0;
@@ -90,6 +123,10 @@ store_block (unsigned block, uchar * src, unsigned len)
 	{
 		(void)memcpy((void *)(load_addr + offset), src, len);
 	}
+#ifdef CONFIG_MCAST_TFTP
+	if (Multicast)
+		ext2_set_bit(block, Bitmap);
+#endif
 
 	if (NetBootFileXferSize < newsize)
 		NetBootFileXferSize = newsize;
@@ -108,6 +145,13 @@ TftpSend (void)
 	int			len = 0;
 	volatile ushort *s;
 
+#ifdef CONFIG_MCAST_TFTP	
+	/* Multicast TFTP.. non-MasterClients do not ACK data. */
+	if (Multicast 
+	 && (TftpState == STATE_DATA) 
+	 && (MasterClient == 0)) 
+		return;
+#endif
 	/*
 	 *	We will always be sending some sort of packet, so
 	 *	cobble together the packet headers now.
@@ -132,11 +176,30 @@ TftpSend (void)
 		printf("send option \"timeout %s\"\n", (char *)pkt);
 #endif
 		pkt += strlen((char *)pkt) + 1;
+		/* try for more effic. blk size */
+		pkt += sprintf((char *)pkt,"blksize%c%d%c",
+				0,htons(TftpBlkSizeOption),0);
+#ifdef CONFIG_MCAST_TFTP	
+		/* Check all preconditions before even trying the option */
+		if (!ProhibitMcast 
+		 && (Bitmap=malloc(Mapsize)) 
+		 && eth_get_dev()->mcast) {
+			free(Bitmap);
+			Bitmap=NULL;
+			pkt += sprintf((char *)pkt,"multicast%c%c",0,0);
+		}
+#endif /* CONFIG_MCAST_TFTP */
 		len = pkt - xp;
 		break;
 
-	case STATE_DATA:
 	case STATE_OACK:
+#ifdef CONFIG_MCAST_TFTP
+		/* My turn!  Start at where I need blocks I missed.*/
+		if (Multicast)
+			TftpBlock=ext2_find_next_zero_bit(Bitmap,(Mapsize*8),0);
+		/*..falling..*/
+#endif
+	case STATE_DATA:
 		xp = pkt;
 		s = (ushort *)pkt;
 		*s++ = htons(TFTP_ACK);
@@ -177,8 +240,13 @@ TftpHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 {
 	ushort proto;
 	ushort *s;
+	int i;
 
 	if (dest != TftpOurPort) {
+#ifdef CONFIG_MCAST_TFTP
+		if (Multicast 
+		 && (!Mcast_port || (dest != Mcast_port)))
+#endif
 		return;
 	}
 	if (TftpState != STATE_RRQ && src != TftpServerPort) {
@@ -208,6 +276,24 @@ TftpHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 #endif
 		TftpState = STATE_OACK;
 		TftpServerPort = src;
+		/* Check for 'blksize' option */
+		for (i=0;i<len-8;i++) {
+			if (strcmp ((char*)pkt+i,"blksize") == 0) {
+				TftpBlkSize = (unsigned short)
+					simple_strtoul((char*)pkt+i+8,NULL,10);
+#ifdef ET_DEBUG
+				printf ("Blocksize ack: %s, %d\n",
+					(char*)pkt+i+8,TftpBlkSize);
+#endif
+				break;
+			}
+		}
+#ifdef CONFIG_MCAST_TFTP
+		parse_multicast_oack((char *)pkt,len-1);
+		if ((Multicast) && (!MasterClient)) 
+			TftpState = STATE_DATA;	/* passive.. */
+		else
+#endif
 		TftpSend (); /* Send ACK */
 		break;
 	case TFTP_DATA:
@@ -224,7 +310,7 @@ TftpHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 		 */
 		if (TftpBlock == 0) {
 			TftpBlockWrap++;
-			TftpBlockWrapOffset += TFTP_BLOCK_SIZE * TFTP_SEQUENCE_SIZE;
+			TftpBlockWrapOffset += TftpBlkSize * TFTP_SEQUENCE_SIZE;
 			printf ("\n\t %lu MB received\n\t ", TftpBlockWrapOffset>>20);
 		} else {
 			if (((TftpBlock - 1) % 10) == 0) {
@@ -248,6 +334,11 @@ TftpHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 			TftpBlockWrap = 0;
 			TftpBlockWrapOffset = 0;
 
+#ifdef CONFIG_MCAST_TFTP
+			if (Multicast) { /* start!=1 common if mcast */
+				TftpLastBlock = TftpBlock - 1;
+			} else
+#endif
 			if (TftpBlock != 1) {	/* Assertion */
 				printf ("\nTFTP error: "
 					"First block is not block 1 (%ld)\n"
@@ -274,9 +365,44 @@ TftpHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 		 *	Acknoledge the block just received, which will prompt
 		 *	the server for the next one.
 		 */
+#ifdef CONFIG_MCAST_TFTP
+		/* if I am the MasterClient, actively calculate what my next 
+		 * needed block is; else I'm passive; not ACKING 
+ 		 */
+		if (Multicast) {
+			if (len < TftpBlkSize)  {
+				TftpEndingBlock = TftpBlock;
+			} else if (MasterClient) {
+				TftpBlock = PrevBitmapHole = 
+					ext2_find_next_zero_bit(
+						Bitmap,
+						(Mapsize*8),
+						PrevBitmapHole);
+				if (TftpBlock > ((Mapsize*8) - 1)) {
+					printf ("tftpfile too big\n");
+					/* try to double it and retry */
+					Mapsize<<=1;
+					mcast_cleanup();
+					NetStartAgain ();
+					return;
+				}
+				TftpLastBlock = TftpBlock;
+			}
+		}
+#endif
 		TftpSend ();
 
-		if (len < TFTP_BLOCK_SIZE) {
+#ifdef CONFIG_MCAST_TFTP
+		if (Multicast) {
+			if (MasterClient && (TftpBlock >= TftpEndingBlock)) {
+				puts ("\nMulticast tftp done\n");
+				mcast_cleanup();
+				NetState = NETLOOP_SUCCESS;
+			} 
+		}
+		else
+#endif
+		if (len < TftpBlkSize) {
 			/*
 			 *	We received the whole thing.  Try to
 			 *	run it.
@@ -290,6 +416,9 @@ TftpHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 		printf ("\nTFTP error: '%s' (%d)\n",
 					pkt + 2, ntohs(*(ushort *)pkt));
 		puts ("Starting again\n\n");
+#ifdef CONFIG_MCAST_TFTP
+		mcast_cleanup();
+#endif
 		NetStartAgain ();
 		break;
 	}
@@ -301,6 +430,9 @@ TftpTimeout (void)
 {
 	if (++TftpTimeoutCount > TIMEOUT_COUNT) {
 		puts ("\nRetry count exceeded; starting again\n");
+#ifdef CONFIG_MCAST_TFTP
+		mcast_cleanup();
+#endif
 		NetStartAgain ();
 	} else {
 		puts ("T ");
@@ -370,6 +502,7 @@ TftpStart (void)
 	TftpState = STATE_RRQ;
 	/* Use a pseudo-random port unless a specific port is set */
 	TftpOurPort = 1024 + (get_timer(0) % 3072);
+
 #ifdef CONFIG_TFTP_PORT
 	if ((ep = getenv("tftpdstp")) != NULL) {
 		TftpServerPort = simple_strtol(ep, NULL, 10);
@@ -382,8 +515,103 @@ TftpStart (void)
 
 	/* zero out server ether in case the server ip has changed */
 	memset(NetServerEther, 0, 6);
+	/* Revert TftpBlkSize to dflt */
+	TftpBlkSize = TFTP_BLOCK_SIZE;
+#ifdef CONFIG_MCAST_TFTP
+    	mcast_cleanup();
+#endif
 
 	TftpSend ();
 }
 
-#endif
+#ifdef CONFIG_MCAST_TFTP
+/* Credits: atftp project.
+ */
+
+/* pick up BcastAddr, Port, and whether I am [now] the master-client. *
+ * Frame:
+ *    +-------+-----------+---+-------~~-------+---+
+ *    |  opc  | multicast | 0 | addr, port, mc | 0 |
+ *    +-------+-----------+---+-------~~-------+---+
+ * The multicast addr/port becomes what I listen to, and if 'mc' is '1' then
+ * I am the new master-client so must send ACKs to DataBlocks.  If I am not
+ * master-client, I'm a passive client, gathering what DataBlocks I may and
+ * making note of which ones I got in my bitmask.
+ * In theory, I never go from master->passive..
+ * .. this comes in with pkt already pointing just past opc
+ */
+static void parse_multicast_oack(char *pkt, int len)
+{
+ int i;
+ IPaddr_t addr;
+ char *mc_adr, *port,  *mc;
+
+	mc_adr=port=mc=NULL;
+	/* march along looking for 'multicast\0', which has to start at least
+	 * 14 bytes back from the end.
+	 */
+	for (i=0;i<len-14;i++)
+		if (strcmp (pkt+i,"multicast") == 0)
+			break;
+	if (i >= (len-14)) /* non-Multicast OACK, ign. */
+		return;
+	
+	i+=10; /* strlen multicast */
+	mc_adr = pkt+i;
+	for (;i<len;i++) {
+		if (*(pkt+i) == ',') {
+			*(pkt+i) = '\0';
+			if (port) {
+				mc = pkt+i+1;
+				break;
+			} else {
+				port = pkt+i+1;
+			}
+		}
+	}
+	if (!port || !mc_adr || !mc ) return;
+	if (Multicast && MasterClient) {
+		printf ("I got a OACK as master Client, WRONG!\n");
+		return;
+	}
+	/* ..I now accept packets destined for this MCAST addr, port */
+	if (!Multicast) {
+		if (Bitmap) {
+			printf ("Internal failure! no mcast.\n");
+			free(Bitmap);
+			Bitmap=NULL;
+			ProhibitMcast=1;
+			return ;
+		}
+		/* I malloc instead of pre-declare; so that if the file ends
+		 * up being too big for this bitmap I can retry 
+		 */ 
+		if (!(Bitmap = malloc (Mapsize))) {
+			printf ("No Bitmap, no multicast. Sorry.\n");
+			ProhibitMcast=1;
+			return;
+		}
+		memset (Bitmap,0,Mapsize);
+		PrevBitmapHole = 0;
+		Multicast = 1;
+	}
+	addr = string_to_ip(mc_adr);
+	if (Mcast_addr != addr) {
+		if (Mcast_addr)
+			eth_mcast_join(Mcast_addr, 0);
+		if (eth_mcast_join(Mcast_addr=addr, 1)) {
+			printf ("Fail to set mcast, revert to TFTP\n");
+			ProhibitMcast=1;
+			mcast_cleanup();
+			NetStartAgain();
+		}
+	}
+	MasterClient = (unsigned char)simple_strtoul((char *)mc,NULL,10);
+	Mcast_port = (unsigned short)simple_strtoul(port,NULL,10);
+	printf ("Multicast: %s:%d [%d]\n", mc_adr, Mcast_port, MasterClient);
+	return;
+}
+
+#endif /* Multicast TFTP */
+
+#endif /* CFG_CMD_NET */