Merge commit 'origin/master'

1 files changed, 236 insertions, 4 deletions

diff --git a/net/tftp.c b/net/tftp.c
index d56e30b..5ee7676 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,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,28 @@ TftpHandler (uchar * pkt, unsigned dest, unsigned src, unsigned len)
 #endif
 		TftpState = STATE_OACK;
 		TftpServerPort = src;
+		/*
+		 * Check for 'blksize' option.
+		 * Careful: "i" is signed, "len" is unsigned, thus
+		 * something like "len-8" may give a *huge* number
+		 */
+		for (i=0; i+8<len; 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 +314,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 +338,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 +369,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 +420,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 +434,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 +506,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 +519,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 ();
 }
 
+#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