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>

3 files changed, 286 insertions, 5 deletions

diff --git a/net/eth.c b/net/eth.c
index c8f92a52634..9ccbccac0fb 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 e9d77576419..2b11a136574 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 d56e30b5b5c..95fee775ec9 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 */