IFC @ r222830

5 files changed, 303 insertions, 36 deletions

diff --git a/lib/libstand/bswap.c b/lib/libstand/bswap.c
index 212e2af..b8e6ffb 100644
--- a/lib/libstand/bswap.c
+++ b/lib/libstand/bswap.c
@@ -8,7 +8,7 @@ __FBSDID("$FreeBSD$");
 
 #if defined(LIBC_SCCS) && !defined(lint)
 static char *rcsid = "$NetBSD: bswap32.c,v 1.1 1997/10/09 15:42:33 bouyer Exp $";
-static char *rcsid = "$NetBSD: bswap64.c,v 1.1 1997/10/09 15:42:33 bouyer Exp $";
+static char *rcsid = "$NetBSD: bswap64.c,v 1.3 2009/03/16 05:59:21 cegger Exp $";
 #endif
 
 #include <sys/types.h>
@@ -30,12 +30,28 @@ bswap32(u_int32_t x)
 
 u_int64_t
 bswap64(u_int64_t x)
-{  
-	u_int32_t *p = (u_int32_t*)&x;
-	u_int32_t t;
-	t = bswap32(p[0]);
-	p[0] = bswap32(p[1]);
-	p[1] = t;
-	return x;
-}   
-
+{
+#ifdef _LP64
+	/*
+	 * Assume we have wide enough registers to do it without touching
+	 * memory.
+	 */
+	return  ( (x << 56) & 0xff00000000000000UL ) |
+		( (x << 40) & 0x00ff000000000000UL ) |
+		( (x << 24) & 0x0000ff0000000000UL ) |
+		( (x <<  8) & 0x000000ff00000000UL ) |
+		( (x >>  8) & 0x00000000ff000000UL ) |
+		( (x >> 24) & 0x0000000000ff0000UL ) |
+		( (x >> 40) & 0x000000000000ff00UL ) |
+		( (x >> 56) & 0x00000000000000ffUL );
+#else
+	/*
+	 * Split the operation in two 32bit steps.
+	 */
+	u_int32_t tl, th;
+
+	th = bswap32((u_int32_t)(x & 0x00000000ffffffffULL));
+	tl = bswap32((u_int32_t)((x >> 32) & 0x00000000ffffffffULL));
+	return ((u_int64_t)th << 32) | tl;
+#endif
+}
diff --git a/lib/libstand/net.c b/lib/libstand/net.c
index 81b4521..24593e6 100644
--- a/lib/libstand/net.c
+++ b/lib/libstand/net.c
@@ -100,7 +100,7 @@ sendrecv(struct iodesc *d,
 				    cc, ssize);
 
 			tleft = tmo;
-			tmo <<= 1;
+			tmo += MINTMO;
 			if (tmo > MAXTMO)
 				tmo = MAXTMO;
 
diff --git a/lib/libstand/tftp.c b/lib/libstand/tftp.c
index 29590fc..54e184c 100644
--- a/lib/libstand/tftp.c
+++ b/lib/libstand/tftp.c
@@ -60,12 +60,21 @@ __FBSDID("$FreeBSD$");
 
 #include "tftp.h"
 
+struct tftp_handle;
+
 static int	tftp_open(const char *path, struct open_file *f);
 static int	tftp_close(struct open_file *f);
+static void	tftp_parse_oack(struct tftp_handle *h, char *buf, size_t len);
 static int	tftp_read(struct open_file *f, void *buf, size_t size, size_t *resid);
 static int	tftp_write(struct open_file *f, void *buf, size_t size, size_t *resid);
 static off_t	tftp_seek(struct open_file *f, off_t offset, int where);
+static int	tftp_set_blksize(struct tftp_handle *h, const char *str);
 static int	tftp_stat(struct open_file *f, struct stat *sb);
+static ssize_t sendrecv_tftp(struct tftp_handle *h, 
+    ssize_t (*sproc)(struct iodesc *, void *, size_t),
+    void *sbuf, size_t ssize,
+    ssize_t (*rproc)(struct tftp_handle *h, void *, ssize_t, time_t, unsigned short *),
+    void *rbuf, size_t rsize, unsigned short *rtype);
 
 struct fs_ops tftp_fsops = {
 	"tftp",
@@ -81,8 +90,20 @@ struct fs_ops tftp_fsops = {
 extern struct in_addr servip;
 
 static int      tftpport = 2000;
+static int	is_open = 0;
+
+/*
+ * The legacy TFTP_BLKSIZE value was 512.
+ * TFTP_REQUESTED_BLKSIZE of 1428 is (Ethernet MTU, less the TFTP, UDP and
+ * IP header lengths).
+ */
+#define TFTP_REQUESTED_BLKSIZE 1428
 
-#define RSPACE 520		/* max data packet, rounded up */
+/*
+ * Choose a blksize big enough so we can test with Ethernet
+ * Jumbo frames in the future.
+ */ 
+#define TFTP_MAX_BLKSIZE 9008
 
 struct tftp_handle {
 	struct iodesc  *iodesc;
@@ -91,10 +112,12 @@ struct tftp_handle {
 	int             validsize;
 	int             off;
 	char           *path;	/* saved for re-requests */
+	unsigned int	tftp_blksize;
+	unsigned long	tftp_tsize; 
 	struct {
 		u_char header[HEADER_SIZE];
 		struct tftphdr t;
-		u_char space[RSPACE];
+		u_char space[TFTP_MAX_BLKSIZE];
 	} __packed __aligned(4) lastdata;
 };
 
@@ -110,8 +133,10 @@ static const int tftperrors[8] = {
 };
 
 static ssize_t 
-recvtftp(struct iodesc *d, void *pkt, ssize_t len, time_t tleft)
+recvtftp(struct tftp_handle *h, void *pkt, ssize_t len, time_t tleft,
+    unsigned short *rtype)
 {
+	struct iodesc *d = h->iodesc;
 	struct tftphdr *t;
 
 	errno = 0;
@@ -122,6 +147,7 @@ recvtftp(struct iodesc *d, void *pkt, ssize_t len, time_t tleft)
 		return (-1);
 
 	t = (struct tftphdr *) pkt;
+	*rtype = ntohs(t->th_opcode);
 	switch (ntohs(t->th_opcode)) {
 	case DATA: {
 		int got;
@@ -154,6 +180,18 @@ recvtftp(struct iodesc *d, void *pkt, ssize_t len, time_t tleft)
 			errno = tftperrors[ntohs(t->th_code)];
 		}
 		return (-1);
+	case OACK: {
+		struct udphdr *uh;
+		int tftp_oack_len = len - sizeof(t->th_opcode); 
+		tftp_parse_oack(h, t->th_u.tu_stuff, tftp_oack_len);
+		/*
+		 * Remember which port this OACK came from,
+		 * because we need to send the ACK back to it.
+		 */
+		uh = (struct udphdr *) pkt - 1;
+		d->destport = uh->uh_sport;
+		return (0);
+	}
 	default:
 #ifdef TFTP_DEBUG
 		printf("tftp type %d not handled\n", ntohs(t->th_opcode));
@@ -175,14 +213,35 @@ tftp_makereq(struct tftp_handle *h)
 	int             l;
 	ssize_t         res;
 	struct tftphdr *t;
+	char *tftp_blksize = NULL;
+	int blksize_l;
+	unsigned short rtype = 0;
+
+	/*
+	 * Allow overriding default TFTP block size by setting
+	 * a tftp.blksize environment variable.
+	 */
+	if ((tftp_blksize = getenv("tftp.blksize")) != NULL) {
+		tftp_set_blksize(h, tftp_blksize);
+	}
 
 	wbuf.t.th_opcode = htons((u_short) RRQ);
 	wtail = wbuf.t.th_stuff;
 	l = strlen(h->path);
+	if (l > FNAME_SIZE)
+		return (ENAMETOOLONG);
 	bcopy(h->path, wtail, l + 1);
 	wtail += l + 1;
 	bcopy("octet", wtail, 6);
 	wtail += 6;
+	bcopy("blksize", wtail, 8);
+	wtail += 8;
+	blksize_l = sprintf(wtail, "%d", h->tftp_blksize);
+	wtail += blksize_l + 1;
+	bcopy("tsize", wtail, 6);
+	wtail += 6;
+	bcopy("0", wtail, 2);
+	wtail += 2;
 
 	t = &h->lastdata.t;
 
@@ -191,18 +250,33 @@ tftp_makereq(struct tftp_handle *h)
 	h->iodesc->destport = htons(IPPORT_TFTP);
 	h->iodesc->xid = 1;	/* expected block */
 
-	res = sendrecv(h->iodesc, sendudp, &wbuf.t, wtail - (char *) &wbuf.t,
-		       recvtftp, t, sizeof(*t) + RSPACE);
+	res = sendrecv_tftp(h, &sendudp, &wbuf.t, wtail - (char *) &wbuf.t,
+		       &recvtftp, t, sizeof(*t) + h->tftp_blksize, &rtype);
+
+	if (rtype == OACK) {
+		wbuf.t.th_opcode = htons((u_short)ACK);
+		wtail = (char *) &wbuf.t.th_block;
+		wbuf.t.th_block = htons(0);
+		wtail += 2;
+		rtype = 0;
+		res = sendrecv_tftp(h, &sendudp, &wbuf.t, wtail - (char *) &wbuf.t,
+			       &recvtftp, t, sizeof(*t) + h->tftp_blksize, &rtype);
+	}
 
-	if (res == -1)
-		return (errno);
+	switch (rtype) {
+		case DATA: {
+			h->currblock = 1;
+			h->validsize = res;
+			h->islastblock = 0;
+			if (res < h->tftp_blksize)
+				h->islastblock = 1;	/* very short file */
+			return (0);
+		}
+		case ERROR:
+		default:
+			return (errno);
+	}
 
-	h->currblock = 1;
-	h->validsize = res;
-	h->islastblock = 0;
-	if (res < SEGSIZE)
-		h->islastblock = 1;	/* very short file */
-	return (0);
 }
 
 /* ack block, expect next */
@@ -216,7 +290,7 @@ tftp_getnextblock(struct tftp_handle *h)
 	char           *wtail;
 	int             res;
 	struct tftphdr *t;
-
+	unsigned short rtype = 0;
 	wbuf.t.th_opcode = htons((u_short) ACK);
 	wtail = (char *) &wbuf.t.th_block;
 	wbuf.t.th_block = htons((u_short) h->currblock);
@@ -226,16 +300,23 @@ tftp_getnextblock(struct tftp_handle *h)
 
 	h->iodesc->xid = h->currblock + 1;	/* expected block */
 
-	res = sendrecv(h->iodesc, sendudp, &wbuf.t, wtail - (char *) &wbuf.t,
-		       recvtftp, t, sizeof(*t) + RSPACE);
+	res = sendrecv_tftp(h, &sendudp, &wbuf.t, wtail - (char *) &wbuf.t,
+		       &recvtftp, t, sizeof(*t) + h->tftp_blksize, &rtype);
 
 	if (res == -1)		/* 0 is OK! */
 		return (errno);
 
 	h->currblock++;
 	h->validsize = res;
-	if (res < SEGSIZE)
+	if (res < h->tftp_blksize)
 		h->islastblock = 1;	/* EOF */
+
+	if (h->islastblock == 1) {
+		/* Send an ACK for the last block */ 
+		wbuf.t.th_block = htons((u_short) h->currblock);
+		sendudp(h->iodesc, &wbuf.t, wtail - (char *)&wbuf.t);
+	}
+
 	return (0);
 }
 
@@ -251,10 +332,15 @@ tftp_open(const char *path, struct open_file *f)
 		return (EINVAL);
 #endif
 
+	if (is_open)
+		return (EBUSY);
+
 	tftpfile = (struct tftp_handle *) malloc(sizeof(*tftpfile));
 	if (!tftpfile)
 		return (ENOMEM);
 
+	memset(tftpfile, 0, sizeof(*tftpfile));
+	tftpfile->tftp_blksize = TFTP_REQUESTED_BLKSIZE;
 	tftpfile->iodesc = io = socktodesc(*(int *) (f->f_devdata));
 	if (io == NULL)
 		return (EINVAL);
@@ -275,6 +361,7 @@ tftp_open(const char *path, struct open_file *f)
 		return (res);
 	}
 	f->f_fsdata = (void *) tftpfile;
+	is_open = 1;
 	return (0);
 }
 
@@ -292,7 +379,7 @@ tftp_read(struct open_file *f, void *addr, size_t size,
 		if (!(tc++ % 16))
 			twiddle();
 
-		needblock = tftpfile->off / SEGSIZE + 1;
+		needblock = tftpfile->off / tftpfile->tftp_blksize + 1;
 
 		if (tftpfile->currblock > needblock)	/* seek backwards */
 			tftp_makereq(tftpfile);	/* no error check, it worked
@@ -315,7 +402,7 @@ tftp_read(struct open_file *f, void *addr, size_t size,
 		if (tftpfile->currblock == needblock) {
 			int offinblock, inbuffer;
 
-			offinblock = tftpfile->off % SEGSIZE;
+			offinblock = tftpfile->off % tftpfile->tftp_blksize;
 
 			inbuffer = tftpfile->validsize - offinblock;
 			if (inbuffer < 0) {
@@ -361,18 +448,19 @@ tftp_close(struct open_file *f)
 		free(tftpfile->path);
 		free(tftpfile);
 	}
+	is_open = 0;
 	return (0);
 }
 
 static int 
 tftp_write(struct open_file *f __unused, void *start __unused, size_t size __unused,
-    size_t *resid /* out */ __unused)
+    size_t *resid __unused /* out */)
 {
 	return (EROFS);
 }
 
 static int 
-tftp_stat(struct open_file *f, struct stat    *sb)
+tftp_stat(struct open_file *f, struct stat *sb)
 {
 	struct tftp_handle *tftpfile;
 	tftpfile = (struct tftp_handle *) f->f_fsdata;
@@ -404,3 +492,166 @@ tftp_seek(struct open_file *f, off_t offset, int where)
 	}
 	return (tftpfile->off);
 }
+
+static ssize_t
+sendrecv_tftp(struct tftp_handle *h, 
+    ssize_t (*sproc)(struct iodesc *, void *, size_t),
+    void *sbuf, size_t ssize,
+    ssize_t (*rproc)(struct tftp_handle *, void *, ssize_t, time_t, unsigned short *),
+    void *rbuf, size_t rsize, unsigned short *rtype)
+{
+	struct iodesc *d = h->iodesc;
+	ssize_t cc;
+	time_t t, t1, tleft;
+
+#ifdef TFTP_DEBUG
+	if (debug)
+		printf("sendrecv: called\n");
+#endif
+
+	tleft = MINTMO;
+	t = t1 = getsecs();
+	for (;;) {
+		if ((getsecs() - t) > MAXTMO) {
+			errno = ETIMEDOUT;
+			return -1;
+		}
+
+		cc = (*sproc)(d, sbuf, ssize);
+		if (cc != -1 && cc < ssize)
+			panic("sendrecv: short write! (%zd < %zu)",
+			    cc, ssize);
+
+		if (cc == -1) {
+			/* Error on transmit; wait before retrying */
+			while ((getsecs() - t1) < tleft);
+			continue;
+		}
+
+recvnext:
+		/* Try to get a packet and process it. */
+		cc = (*rproc)(h, rbuf, rsize, tleft, rtype);
+		/* Return on data, EOF or real error. */
+		if (cc != -1 || errno != 0)
+			return (cc);
+		if ((getsecs() - t1) < tleft) {
+		    goto recvnext;
+		}
+
+		/* Timed out or didn't get the packet we're waiting for */
+		tleft += MINTMO;
+		if (tleft > (2 * MINTMO)) {
+			tleft = (2 * MINTMO);
+		}
+		t1 = getsecs();
+	}
+}
+
+static int
+tftp_set_blksize(struct tftp_handle *h, const char *str)
+{
+        char *endptr;
+	int new_blksize;
+	int ret = 0;
+
+	if (h == NULL || str == NULL)
+		return (ret);
+
+	new_blksize =
+	    (unsigned int)strtol(str, &endptr, 0);
+
+	/*
+	 * Only accept blksize value if it is numeric.
+	 * RFC2348 specifies that acceptable valuesare 8-65464
+	 * 8-65464 .  Let's choose a limit less than MAXRSPACE
+	*/
+	if (*endptr == '\0' && new_blksize >= 8
+	    && new_blksize <= TFTP_MAX_BLKSIZE) {
+		h->tftp_blksize = new_blksize;
+		ret = 1;
+	}
+
+	return (ret);
+}
+
+/*
+ * In RFC2347, the TFTP Option Acknowledgement package (OACK)
+ * is used to acknowledge a client's option negotiation request.
+ * The format of an OACK packet is:
+ *    +-------+---~~---+---+---~~---+---+---~~---+---+---~~---+---+
+ *    |  opc  |  opt1  | 0 | value1 | 0 |  optN  | 0 | valueN | 0 |
+ *    +-------+---~~---+---+---~~---+---+---~~---+---+---~~---+---+
+ *
+ *    opc
+ *       The opcode field contains a 6, for Option Acknowledgment.
+ *
+ *    opt1
+ *       The first option acknowledgment, copied from the original
+ *       request.
+ *
+ *    value1
+ *       The acknowledged value associated with the first option.  If
+ *       and how this value may differ from the original request is
+ *       detailed in the specification for the option.
+ *
+ *    optN, valueN
+ *       The final option/value acknowledgment pair.
+ */
+static void 
+tftp_parse_oack(struct tftp_handle *h, char *buf, size_t len)
+{
+	/* 
+	 *  We parse the OACK strings into an array
+	 *  of name-value pairs.
+	 *  
+	 */
+	char *tftp_options[128] = { 0 };
+	char *val = buf;
+	int i = 0;
+	int option_idx = 0;
+	int blksize_is_set = 0;
+	int tsize = 0;
+	
+ 
+	while ( option_idx < 128 && i < len ) {
+		 if (buf[i] == '\0') {
+		    if (&buf[i] > val) {
+		       tftp_options[option_idx] = val;
+		       val = &buf[i] + 1;
+		       ++option_idx;
+		    }
+		 }
+		 ++i;
+	}
+
+	/* 
+	 * Parse individual TFTP options.
+	 *    * "blksize" is specified in RFC2348.
+	 *    * "tsize" is specified in RFC2349.
+	 */ 
+	for (i = 0; i < option_idx; i += 2) {
+	    if (strcasecmp(tftp_options[i], "blksize") == 0) {
+		if (i + 1 < option_idx) {
+			blksize_is_set =
+			    tftp_set_blksize(h, tftp_options[i + 1]);
+		}
+	    } else if (strcasecmp(tftp_options[i], "tsize") == 0) {
+		if (i + 1 < option_idx) {
+			tsize = strtol(tftp_options[i + 1], (char **)NULL, 10);
+		}
+	    }
+	}
+
+	if (!blksize_is_set) {
+		/*
+		 * If TFTP blksize was not set, try defaulting
+		 * to the legacy TFTP blksize of 512
+		 */
+		h->tftp_blksize = 512;
+	}
+
+#ifdef TFTP_DEBUG
+	printf("tftp_blksize: %u\n", h->tftp_blksize);
+	printf("tftp_tsize: %lu\n", h->tftp_tsize);
+#endif
+}
diff --git a/lib/libstand/zalloc.c b/lib/libstand/zalloc.c
index aa8ccd0..2cd71c4 100644
--- a/lib/libstand/zalloc.c
+++ b/lib/libstand/zalloc.c
@@ -154,7 +154,7 @@ zfree(MemPool *mp, void *ptr, iaddr_t bytes)
     if ((char *)ptr < (char *)mp->mp_Base || 
 	(char *)ptr + bytes > (char *)mp->mp_End ||
 	((iaddr_t)ptr & MEMNODE_SIZE_MASK) != 0)
-	panic("zfree(%p,%d): wild pointer", ptr, bytes);
+	panic("zfree(%p,%ju): wild pointer", ptr, bytes);
 
     /*
      * free the segment
@@ -178,7 +178,7 @@ zfree(MemPool *mp, void *ptr, iaddr_t bytes)
 		 * range check
 		 */
 		if ((char *)ptr + bytes > (char *)mn)
-		    panic("zfree(%p,%d): corrupt memlist1",ptr, bytes);
+		    panic("zfree(%p,%ju): corrupt memlist1",ptr, bytes);
 
 		/*
 		 * merge against next area or create independant area
@@ -209,7 +209,7 @@ zfree(MemPool *mp, void *ptr, iaddr_t bytes)
 		/* NOT REACHED */
 	    }
 	    if ((char *)ptr < (char *)mn + mn->mr_Bytes)
-		panic("zfree(%p,%d): corrupt memlist2", ptr, bytes);
+		panic("zfree(%p,%ju): corrupt memlist2", ptr, bytes);
 	}
 	/*
 	 * We are beyond the last MemNode, append new MemNode.  Merge against
diff --git a/lib/libstand/zalloc_malloc.c b/lib/libstand/zalloc_malloc.c
index cdacf13..5cd7bcb 100644
--- a/lib/libstand/zalloc_malloc.c
+++ b/lib/libstand/zalloc_malloc.c
@@ -110,7 +110,7 @@ Free(void *ptr, const char *file, int line)
 	    return;
 	}
 	if (*((signed char *)res + res->ga_Bytes - 1) != -2)
-	    panic("free: guard2 fail @ %p + %d from %s:%d", ptr, res->ga_Bytes - MALLOCALIGN, file, line);
+	    panic("free: guard2 fail @ %p + %zu from %s:%d", ptr, res->ga_Bytes - MALLOCALIGN, file, line);
 	*((signed char *)res + res->ga_Bytes - 1) = -1;
 #endif