Bring in new files from edwin's tftp

10 files changed, 2064 insertions, 0 deletions

diff --git a/libexec/tftpd/tftp-file.c b/libexec/tftpd/tftp-file.c
new file mode 100644
index 0000000..e0f8e78
--- /dev/null
+++ b/libexec/tftpd/tftp-file.c
@@ -0,0 +1,257 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <netinet/in.h>
+#include <arpa/tftp.h>
+
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+#include <unistd.h>
+
+#include "tftp-file.h"
+#include "tftp-utils.h"
+
+static FILE	*file;
+static int	convert;
+
+static char	convbuffer[66000];
+static int	gotcr = 0;
+
+static size_t
+convert_from_net(char *buffer, size_t count)
+{
+	size_t i, n;
+
+	/*
+	 * Convert all CR/LF to LF and all CR,NUL to CR
+	 */
+
+	n = 0;
+	for (i = 0; i < count; i++) {
+
+		if (gotcr == 0) {
+			convbuffer[n++] = buffer[i];
+			gotcr = (buffer[i] == '\r');
+			continue;
+		}
+
+		/* CR, NULL -> CR */
+		if (buffer[i] == '\0') {
+			gotcr = 0;
+			continue;
+		}
+
+		/* CR, LF -> LF */
+		if (buffer[i] == '\n') {
+			if (n == 0) {
+				if (ftell(file) != 0) {
+					fseek(file, -1, SEEK_END);
+					convbuffer[n++] = '\n';
+				} else {
+					/* This shouldn't happen */
+					tftp_log(LOG_ERR,
+					    "Received LF as first character");
+					abort();
+				}
+			} else
+				convbuffer[n-1] = '\n';
+			gotcr = 0;
+			continue;
+		}
+
+		/* Everything else just accept as is */
+		convbuffer[n++] = buffer[i];
+		gotcr = (buffer[i] == '\r');
+		continue;
+	}
+
+	return fwrite(convbuffer, 1, n, file);
+}
+
+static size_t
+convert_to_net(char *buffer, size_t count, int init)
+{
+	size_t i;
+	static size_t n = 0, read = 0;
+	static int newline = 0;
+
+	if (init) {
+		newline = 0;
+		n = 0;
+		read = 0;
+		return 0 ;
+	}
+
+	/*
+	 * Convert all LF to CR,LF and all CR to CR,NUL
+	 */
+	i = 0;
+
+	if (newline) {
+		buffer[i++] = newline;
+		newline = 0;
+	}
+
+	while (i < count) {
+		if (n == read) {
+			/* When done we're done */
+			if (feof(file)) break;
+
+			/* Otherwise read another bunch */
+			read = fread(convbuffer, 1, count, file);
+			if (read == 0) break;
+			n = 0;
+		}
+
+		/* CR -> CR,NULL */
+		if (convbuffer[n] == '\r') {
+			buffer[i++] = '\r';
+			buffer[i++] = '\0';
+			n++;
+			continue;
+		}
+
+		/* LF -> CR,LF */
+		if (convbuffer[n] == '\n') {
+			buffer[i++] = '\r';
+			buffer[i++] = '\n';
+			n++;
+			continue;
+		}
+
+		buffer[i++] = convbuffer[n++];
+	}
+
+	if (i > count) {
+		/*
+		 * Whoops... that isn't alllowed (but it will happen
+		 * when there is a CR or LF at the end of the buffer)
+		 */
+		newline = buffer[i-1];
+	}
+
+	if (i < count) {
+		/* We are done! */
+		return i;
+	} else
+		return count;
+
+}
+
+int
+write_init(int fd, FILE *f, const char *mode)
+{
+
+	if (f == NULL) {
+		file = fdopen(fd, "w");
+		if (file == NULL) {
+			int en = errno;
+			tftp_log(LOG_ERR, "fdopen() failed: %s",
+			    strerror(errno));
+			return en;
+		}
+	} else
+		file = f;
+	convert = !strcmp(mode, "netascii");
+	return 0;
+}
+
+size_t
+write_file(char *buffer, int count)
+{
+
+	if (convert == 0)
+		return fwrite(buffer, 1, count, file);
+
+	return convert_from_net(buffer, count);
+}
+
+int
+write_close(void)
+{
+
+	if (fclose(file) != 0) {
+		tftp_log(LOG_ERR, "fclose() failed: %s", strerror(errno));
+		return 1;
+	}
+	return 0;
+}
+
+int
+read_init(int fd, FILE *f, const char *mode)
+{
+
+	convert_to_net(NULL, 0, 1);
+	if (f == NULL) {
+		file = fdopen(fd, "r");
+		if (file == NULL) {
+			int en = errno;
+			tftp_log(LOG_ERR, "fdopen() failed: %s",
+			    strerror(errno));
+			return en;
+		}
+	} else
+		file = f;
+	convert = !strcmp(mode, "netascii");
+	return 0;
+}
+
+size_t
+read_file(char *buffer, int count)
+{
+
+	if (convert == 0)
+		return fread(buffer, 1, count, file);
+
+	return convert_to_net(buffer, count, 0);
+}
+
+int
+read_close(void)
+{
+
+	if (fclose(file) != 0) {
+		tftp_log(LOG_ERR, "fclose() failed: %s", strerror(errno));
+		return 1;
+	}
+	return 0;
+}
+
+
+int
+synchnet(int peer)
+{
+
+	return 0;
+}
diff --git a/libexec/tftpd/tftp-file.h b/libexec/tftpd/tftp-file.h
new file mode 100644
index 0000000..fcc4d0d
--- /dev/null
+++ b/libexec/tftpd/tftp-file.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+int	write_init(int fd, FILE *f, const char *mode);
+size_t	write_file(char *buffer, int count);
+int	write_close(void);
+
+int	read_init(int fd, FILE *f, const char *mode);
+size_t	read_file(char *buffer, int count);
+int	read_close(void);
+
+int	synchnet(int peer);
diff --git a/libexec/tftpd/tftp-io.c b/libexec/tftpd/tftp-io.c
new file mode 100644
index 0000000..28628ba
--- /dev/null
+++ b/libexec/tftpd/tftp-io.c
@@ -0,0 +1,478 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include <netinet/in.h>
+#include <arpa/tftp.h>
+#include <arpa/inet.h>
+
+#include <errno.h>
+#include <setjmp.h>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+#include <unistd.h>
+
+#include "tftp-file.h"
+#include "tftp-io.h"
+#include "tftp-utils.h"
+#include "tftp-options.h"
+
+struct sockaddr_storage peer_sock;
+struct sockaddr_storage me_sock;
+
+static int send_packet(int peer, uint16_t block, char *pkt, int size);
+
+struct errmsg {
+	int	e_code;
+	const char	*e_msg;
+} errmsgs[] = {
+	{ EUNDEF,	"Undefined error code" },
+	{ ENOTFOUND,	"File not found" },
+	{ EACCESS,	"Access violation" },
+	{ ENOSPACE,	"Disk full or allocation exceeded" },
+	{ EBADOP,	"Illegal TFTP operation" },
+	{ EBADID,	"Unknown transfer ID" },
+	{ EEXISTS,	"File already exists" },
+	{ ENOUSER,	"No such user" },
+	{ EOPTNEG,	"Option negotiation" },
+	{ -1,		NULL }
+};
+
+#define DROPPACKET(s)							\
+	if (packetdroppercentage != 0 &&				\
+	    random()%100 < packetdroppercentage) {			\
+		tftp_log(LOG_DEBUG, "Artifical packet drop in %s", s);	\
+		return;							\
+	}
+#define DROPPACKETn(s,n)						\
+	if (packetdroppercentage != 0 &&				\
+	    random()%100 < packetdroppercentage) {			\
+		tftp_log(LOG_DEBUG, "Artifical packet drop in %s", s);	\
+		return (n);						\
+	}
+
+const char *
+errtomsg(int error)
+{
+	static char ebuf[40];
+	struct errmsg *pe;
+	char buf[MAXPKTSIZE];
+
+	if (error == 0)
+		return ("success");
+	for (pe = errmsgs; pe->e_code >= 0; pe++)
+		if (pe->e_code == error)
+			return (pe->e_msg);
+	snprintf(ebuf, sizeof(buf), "error %d", error);
+	return (ebuf);
+}
+
+static int
+send_packet(int peer, uint16_t block, char *pkt, int size)
+{
+	int i;
+	int t = 1;
+
+	for (i = 0; i < 12 ; i++) {
+		DROPPACKETn("send_packet", 0);
+
+		if (sendto(peer, pkt, size, 0,
+			(struct sockaddr *)&peer_sock, peer_sock.ss_len)
+			== size) {
+			if (i)
+				tftp_log(LOG_ERR,
+				    "%s block %d, attempt %d successful",
+				    block, i);
+			return (0);
+		}
+		tftp_log(LOG_ERR,
+		    "%s block %d, attempt %d failed (Error %d: %s)", 
+		    packettype(ntohs(((struct tftphdr *)(pkt))->th_opcode)),
+		    block, i, errno, strerror(errno));
+		sleep(t);
+		if (t < 32)
+			t <<= 1;
+	}
+	tftp_log(LOG_ERR, "send_packet: %s", strerror(errno));
+	return (1);
+}
+
+/*
+ * Send an ERROR packet (error message).
+ * Error code passed in is one of the
+ * standard TFTP codes, or a UNIX errno
+ * offset by 100.
+ */
+void
+send_error(int peer, int error)
+{
+	struct tftphdr *tp;
+	int length;
+	struct errmsg *pe;
+	char buf[MAXPKTSIZE];
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Sending ERROR %d: %s", error);
+
+	DROPPACKET("send_error");
+
+	tp = (struct tftphdr *)buf;
+	tp->th_opcode = htons((u_short)ERROR);
+	tp->th_code = htons((u_short)error);
+	for (pe = errmsgs; pe->e_code >= 0; pe++)
+		if (pe->e_code == error)
+			break;
+	if (pe->e_code < 0) {
+		pe->e_msg = strerror(error - 100);
+		tp->th_code = EUNDEF;   /* set 'undef' errorcode */
+	}
+	strcpy(tp->th_msg, pe->e_msg);
+	length = strlen(pe->e_msg);
+	tp->th_msg[length] = '\0';
+	length += 5;
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Sending ERROR %d: %s", error, tp->th_msg);
+
+	if (sendto(peer, buf, length, 0,
+		(struct sockaddr *)&peer_sock, peer_sock.ss_len) != length)
+		tftp_log(LOG_ERR, "send_error: %s", strerror(errno));
+}
+
+/*
+ * Send an WRQ packet (write request).
+ */
+int
+send_wrq(int peer, char *filename, char *mode)
+{
+	int n;
+	struct tftphdr *tp;
+	char *bp;
+	char buf[MAXPKTSIZE];
+	int size;
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Sending WRQ: filename: '%s', mode '%s'",
+			filename, mode
+		);
+
+	DROPPACKETn("send_wrq", 1);
+
+	tp = (struct tftphdr *)buf;
+	tp->th_opcode = htons((u_short)WRQ);
+	size = 2;
+
+	bp = tp->th_stuff;
+	strcpy(bp, filename);
+	bp += strlen(filename);
+	*bp = 0;
+	bp++;
+	size += strlen(filename) + 1;
+
+	strcpy(bp, mode);
+	bp += strlen(mode);
+	*bp = 0;
+	bp++;
+	size += strlen(mode) + 1;
+
+	if (options_rfc_enabled)
+		size += make_options(peer, bp, sizeof(buf) - size);
+
+	n = sendto(peer, buf, size, 0,
+	    (struct sockaddr *)&peer_sock, peer_sock.ss_len);
+	if (n != size) {
+		tftp_log(LOG_ERR, "send_wrq: %s", strerror(errno));
+		return (1);
+	}
+	return (0);
+}
+
+/*
+ * Send an RRQ packet (write request).
+ */
+int
+send_rrq(int peer, char *filename, char *mode)
+{
+	int n;
+	struct tftphdr *tp;
+	char *bp;
+	char buf[MAXPKTSIZE];
+	int size;
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Sending RRQ: filename: '%s', mode '%s'",
+			filename, mode
+		);
+
+	DROPPACKETn("send_rrq", 1);
+
+	tp = (struct tftphdr *)buf;
+	tp->th_opcode = htons((u_short)RRQ);
+	size = 2;
+
+	bp = tp->th_stuff;
+	strcpy(bp, filename);
+	bp += strlen(filename);
+	*bp = 0;
+	bp++;
+	size += strlen(filename) + 1;
+
+	strcpy(bp, mode);
+	bp += strlen(mode);
+	*bp = 0;
+	bp++;
+	size += strlen(mode) + 1;
+
+	if (options_rfc_enabled) {
+		options[OPT_TSIZE].o_request = strdup("0");
+		size += make_options(peer, bp, sizeof(buf) - size);
+	}
+
+	n = sendto(peer, buf, size, 0,
+	    (struct sockaddr *)&peer_sock, peer_sock.ss_len);
+	if (n != size) {
+		tftp_log(LOG_ERR, "send_rrq: %s", n, strerror(errno));
+		return (1);
+	}
+	return (0);
+}
+
+/*
+ * Send an OACK packet (option acknowledgement).
+ */
+int
+send_oack(int peer)
+{
+	struct tftphdr *tp;
+	int size, i, n;
+	char *bp;
+	char buf[MAXPKTSIZE];
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Sending OACK");
+
+	DROPPACKETn("send_oack", 0);
+
+	/*
+	 * Send back an options acknowledgement (only the ones with
+	 * a reply for)
+	 */
+	tp = (struct tftphdr *)buf;
+	bp = buf + 2;
+	size = sizeof(buf) - 2;
+	tp->th_opcode = htons((u_short)OACK);
+	for (i = 0; options[i].o_type != NULL; i++) {
+		if (options[i].o_reply != NULL) {
+			n = snprintf(bp, size, "%s%c%s", options[i].o_type,
+				     0, options[i].o_reply);
+			bp += n+1;
+			size -= n+1;
+			if (size < 0) {
+				tftp_log(LOG_ERR, "oack: buffer overflow");
+				exit(1);
+			}
+		}
+	}
+	size = bp - buf;
+
+	if (sendto(peer, buf, size, 0,
+		(struct sockaddr *)&peer_sock, peer_sock.ss_len) != size) {
+		tftp_log(LOG_INFO, "send_oack: %s", strerror(errno));
+		return (1);
+	}
+
+	return (0);
+}
+
+/*
+ * Send an ACK packet (acknowledgement).
+ */
+int
+send_ack(int fp, uint16_t block)
+{
+	struct tftphdr *tp;
+	int size;
+	char *bp;
+	char buf[MAXPKTSIZE];
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Sending ACK for block %d", block);
+
+	DROPPACKETn("send_ack", 0);
+
+	tp = (struct tftphdr *)buf;
+	bp = buf + 2;
+	size = sizeof(buf) - 2;
+	tp->th_opcode = htons((u_short)ACK);
+	tp->th_block = htons((u_short)block);
+	size = 4;
+
+	if (sendto(fp, buf, size, 0,
+	    (struct sockaddr *)&peer_sock, peer_sock.ss_len) != size) {
+		tftp_log(LOG_INFO, "send_ack: %s", strerror(errno));
+		return (1);
+	}
+
+	return (0);
+}
+
+/*
+ * Send a DATA packet
+ */
+int
+send_data(int peer, uint16_t block, char *data, int size)
+{
+	char buf[MAXPKTSIZE];
+	struct tftphdr *pkt;
+	int n;
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Sending DATA packet %d of %d bytes",
+			block, size);
+
+	DROPPACKETn("send_data", 0);
+
+	pkt = (struct tftphdr *)buf;
+
+	pkt->th_opcode = htons((u_short)DATA);
+	pkt->th_block = htons((u_short)block);
+	memcpy(pkt->th_data, data, size);
+
+	n = send_packet(peer, block, (char *)pkt, size + 4);
+	return (n);
+}
+
+
+/*
+ * Receive a packet
+ */
+jmp_buf	timeoutbuf;
+
+static void
+timeout(int sig __unused)
+{
+
+	/* tftp_log(LOG_DEBUG, "Timeout\n");	Inside a signal handler... */
+	longjmp(timeoutbuf, 1);
+}
+
+int
+receive_packet(int peer, char *data, int size, struct sockaddr_storage *from,
+    int thistimeout)
+{
+	struct tftphdr *pkt;
+	struct sockaddr_storage from_local;
+	struct sockaddr_storage *pfrom;
+	socklen_t fromlen;
+	int n;
+	static int waiting;
+
+	pfrom = (from == NULL) ? &from_local : from;
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG,
+		    "Waiting %d seconds for packet", timeoutpacket);
+
+	pkt = (struct tftphdr *)data;
+
+	waiting = 0;
+	signal(SIGALRM, timeout);
+	setjmp(timeoutbuf);
+	alarm(thistimeout);
+
+	if (waiting > 0) {
+		alarm(0);
+		return (RP_TIMEOUT);
+	}
+
+	if (waiting > 0) {
+		tftp_log(LOG_ERR, "receive_packet: timeout");
+		alarm(0);
+		return (RP_TIMEOUT);
+	}
+
+	waiting++;
+	fromlen = sizeof(*pfrom);
+	n = recvfrom(peer, data, size, 0, (struct sockaddr *)pfrom, &fromlen);
+
+	alarm(0);
+
+	DROPPACKETn("receive_packet", RP_TIMEOUT);
+
+	if (n < 0) {
+		tftp_log(LOG_ERR, "receive_packet: timeout");
+		return (RP_TIMEOUT);
+	}
+
+	alarm(0);
+
+	if (n < 0) {
+		/* No idea what could have happened if it isn't a timeout */
+		tftp_log(LOG_ERR, "receive_packet: %s", strerror(errno));
+		return (RP_RECVFROM);
+	}
+	if (n < 4) {
+		tftp_log(LOG_ERR,
+		    "receive_packet: packet too small (%d bytes)", n);
+		return (RP_TOOSMALL);
+	}
+
+	pkt->th_opcode = ntohs((u_short)pkt->th_opcode);
+	if (pkt->th_opcode == DATA ||
+	    pkt->th_opcode == ACK)
+		pkt->th_block = ntohs((u_short)pkt->th_block);
+
+	if (pkt->th_opcode == DATA && n > pktsize) {
+		tftp_log(LOG_ERR, "receive_packet: packet too big");
+		return (RP_TOOBIG);
+	}
+
+	if (((struct sockaddr_in *)(pfrom))->sin_addr.s_addr !=
+	    ((struct sockaddr_in *)(&peer_sock))->sin_addr.s_addr) {
+		tftp_log(LOG_ERR,
+			"receive_packet: received packet from wrong source");
+		return (RP_WRONGSOURCE);
+	}
+
+	if (pkt->th_opcode == ERROR) {
+		tftp_log(LOG_ERR, "Got ERROR packet: %s", pkt->th_msg);
+		return (RP_ERROR);
+	}
+
+	if (debug&DEBUG_PACKETS)
+		tftp_log(LOG_DEBUG, "Received %d bytes in a %s packet",
+			n, packettype(pkt->th_opcode));
+
+	return n - 4;
+}
diff --git a/libexec/tftpd/tftp-io.h b/libexec/tftpd/tftp-io.h
new file mode 100644
index 0000000..70558bc
--- /dev/null
+++ b/libexec/tftpd/tftp-io.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#define RP_NONE		0
+#define	RP_RECVFROM	-1
+#define	RP_TOOSMALL	-2
+#define RP_ERROR	-3
+#define RP_WRONGSOURCE	-4
+#define	RP_TIMEOUT	-5
+#define	RP_TOOBIG	-6
+
+const char *errtomsg(int);
+void	send_error(int peer, int);
+int	send_wrq(int peer, char *, char *);
+int	send_rrq(int peer, char *, char *);
+int	send_oack(int peer);
+int	send_ack(int peer, unsigned short);
+int	send_data(int peer, uint16_t, char *, int);
+int	receive_packet(int peer, char *, int, struct sockaddr_storage *, int);
+
+extern struct sockaddr_storage peer_sock;
+extern struct sockaddr_storage me_sock;
diff --git a/libexec/tftpd/tftp-options.c b/libexec/tftpd/tftp-options.c
new file mode 100644
index 0000000..0b97aaf
--- /dev/null
+++ b/libexec/tftpd/tftp-options.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <sys/sysctl.h>
+#include <sys/stat.h>
+
+#include <netinet/in.h>
+#include <arpa/tftp.h>
+
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+
+#include "tftp-utils.h"
+#include "tftp-io.h"
+#include "tftp-options.h"
+
+/*
+ * Option handlers
+ */
+
+struct options options[] = {
+	{ "tsize",	NULL, NULL, NULL /* option_tsize */, 1 },
+	{ "timeout",	NULL, NULL, option_timeout, 1 },
+	{ "blksize",	NULL, NULL, option_blksize, 1 },
+	{ "blksize2",	NULL, NULL, option_blksize2, 0 },
+	{ "rollover",	NULL, NULL, option_rollover, 0 },
+	{ NULL,		NULL, NULL, NULL, 0 }
+};
+
+/* By default allow them */
+int options_rfc_enabled = 1;
+int options_extra_enabled = 1;
+
+/*
+ * Rules for the option handlers:
+ * - If there is no o_request, there will be no processing.
+ *
+ * For servers
+ * - Logging is done as warnings.
+ * - The handler exit()s if there is a serious problem with the
+ *   values submitted in the option.
+ *
+ * For clients
+ * - Logging is done as errors. After all, the server shouldn't
+ *   return rubbish.
+ * - The handler returns if there is a serious problem with the
+ *   values submitted in the option.
+ * - Sending the EBADOP packets is done by the handler.
+ */
+
+int
+option_tsize(int peer, struct tftphdr *tp, int mode, struct stat *stbuf)
+{
+
+	if (options[OPT_TSIZE].o_request == NULL)
+		return (0);
+
+	if (mode == RRQ) 
+		asprintf(&options[OPT_TSIZE].o_reply,
+			"%ju", stbuf->st_size);
+	else
+		/* XXX Allows writes of all sizes. */
+		options[OPT_TSIZE].o_reply =
+			strdup(options[OPT_TSIZE].o_request);
+	return (0);
+}
+
+int
+option_timeout(int peer)
+{
+
+	if (options[OPT_TIMEOUT].o_request == NULL)
+		return (0);
+
+	int to = atoi(options[OPT_TIMEOUT].o_request);
+	if (to < TIMEOUT_MIN || to > TIMEOUT_MAX) {
+		tftp_log(acting_as_client ? LOG_ERR : LOG_WARNING,
+		    "Received bad value for timeout. "
+		    "Should be between %d and %d, received %s",
+		    TIMEOUT_MIN, TIMEOUT_MAX);
+		send_error(peer, EBADOP);
+		if (acting_as_client)
+			return (1);
+		exit(1);
+	} else {
+		timeoutpacket = to;
+		options[OPT_TIMEOUT].o_reply =
+			strdup(options[OPT_TIMEOUT].o_request);
+	}
+	settimeouts(timeoutpacket, timeoutnetwork, maxtimeouts);
+
+	if (debug&DEBUG_OPTIONS)
+		tftp_log(LOG_DEBUG, "Setting timeout to '%s'",
+			options[OPT_TIMEOUT].o_reply);
+
+	return (0);
+}
+
+int
+option_rollover(int peer)
+{
+
+	if (options[OPT_ROLLOVER].o_request == NULL)
+		return (0);
+
+	if (strcmp(options[OPT_ROLLOVER].o_request, "0") != 0
+	 && strcmp(options[OPT_ROLLOVER].o_request, "1") != 0) {
+		tftp_log(acting_as_client ? LOG_ERR : LOG_WARNING,
+		    "Bad value for rollover, "
+		    "should be either 0 or 1, received '%s', "
+		    "ignoring request",
+		    options[OPT_ROLLOVER].o_request);
+		if (acting_as_client) {
+			send_error(peer, EBADOP);
+			return (1);
+		}
+		return (0);
+	}
+	options[OPT_ROLLOVER].o_reply =
+		strdup(options[OPT_ROLLOVER].o_request);
+
+	if (debug&DEBUG_OPTIONS)
+		tftp_log(LOG_DEBUG, "Setting rollover to '%s'",
+			options[OPT_ROLLOVER].o_reply);
+
+	return (0);
+}
+
+int
+option_blksize(int peer)
+{
+	int *maxdgram;
+	char maxbuffer[100];
+	size_t len;
+
+	if (options[OPT_BLKSIZE].o_request == NULL)
+		return (0);
+
+	/* maximum size of an UDP packet according to the system */
+	len = sizeof(maxbuffer);
+	if (sysctlbyname("net.inet.udp.maxdgram",
+	    maxbuffer, &len, NULL, 0) < 0) {
+		tftp_log(LOG_ERR, "sysctl: net.inet.udp.maxdgram");
+		return (acting_as_client ? 1 : 0);
+	}
+	maxdgram = (int *)maxbuffer;
+
+	int size = atoi(options[OPT_BLKSIZE].o_request);
+	if (size < BLKSIZE_MIN || size > BLKSIZE_MAX) {
+		if (acting_as_client) {
+			tftp_log(LOG_ERR,
+			    "Invalid blocksize (%d bytes), aborting",
+			    size);
+			send_error(peer, EBADOP);
+			return (1);
+		} else {
+			tftp_log(LOG_WARNING,
+			    "Invalid blocksize (%d bytes), ignoring request",
+			    size);
+			return (0);
+		}
+	}
+
+	if (size > *maxdgram) {
+		if (acting_as_client) {
+			tftp_log(LOG_ERR,
+			    "Invalid blocksize (%d bytes), "
+			    "net.inet.udp.maxdgram sysctl limits it to "
+			    "%d bytes.\n", size, *maxdgram);
+			send_error(peer, EBADOP);
+			return (1);
+		} else {
+			tftp_log(LOG_WARNING,
+			    "Invalid blocksize (%d bytes), "
+			    "net.inet.udp.maxdgram sysctl limits it to "
+			    "%d bytes.\n", size, *maxdgram);
+			size = *maxdgram;
+			/* No reason to return */
+		}
+	}
+
+	asprintf(&options[OPT_BLKSIZE].o_reply, "%d", size);
+	segsize = size;
+	pktsize = size + 4;
+	if (debug&DEBUG_OPTIONS)
+		tftp_log(LOG_DEBUG, "Setting blksize to '%s'",
+		    options[OPT_BLKSIZE].o_reply);
+
+	return (0);
+}
+
+int
+option_blksize2(int peer)
+{
+	int	*maxdgram;
+	char	maxbuffer[100];
+	int	size, i;
+	size_t	len;
+
+	int sizes[] = {
+		8, 16, 32, 64, 128, 256, 512, 1024,
+		2048, 4096, 8192, 16384, 32768, 0
+	};
+
+	if (options[OPT_BLKSIZE2].o_request == NULL)
+		return (0);
+
+	/* maximum size of an UDP packet according to the system */
+	len = sizeof(maxbuffer);
+	if (sysctlbyname("net.inet.udp.maxdgram",
+	    maxbuffer, &len, NULL, 0) < 0) {
+		tftp_log(LOG_ERR, "sysctl: net.inet.udp.maxdgram");
+		return (acting_as_client ? 1 : 0);
+	}
+	maxdgram = (int *)maxbuffer;
+
+	size = atoi(options[OPT_BLKSIZE2].o_request);
+	for (i = 0; sizes[i] != 0; i++) {
+		if (size == sizes[i]) break;
+	}
+	if (sizes[i] == 0) {
+		tftp_log(LOG_INFO,
+		    "Invalid blocksize2 (%d bytes), ignoring request", size);
+		return (acting_as_client ? 1 : 0);
+	}
+
+	if (size > *maxdgram) {
+		for (i = 0; sizes[i+1] != 0; i++) {
+			if (*maxdgram < sizes[i+1]) break;
+		}
+		tftp_log(LOG_INFO,
+		    "Invalid blocksize2 (%d bytes), net.inet.udp.maxdgram "
+		    "sysctl limits it to %d bytes.\n", size, *maxdgram);
+		size = sizes[i];
+		/* No need to return */
+	}
+
+	asprintf(&options[OPT_BLKSIZE2].o_reply, "%d", size);
+	segsize = size;
+	pktsize = size + 4;
+	if (debug&DEBUG_OPTIONS)
+		tftp_log(LOG_DEBUG, "Setting blksize2 to '%s'",
+		    options[OPT_BLKSIZE2].o_reply);
+
+	return (0);
+}
+
+/*
+ * Append the available options to the header
+ */
+uint16_t
+make_options(int peer, char *buffer, uint16_t size) {
+	int	i;
+	char	*value;
+	const char *option;
+	uint16_t length;
+	uint16_t returnsize = 0;
+
+	if (!options_rfc_enabled) return (0);
+
+	for (i = 0; options[i].o_type != NULL; i++) {
+		if (options[i].rfc == 0 && !options_extra_enabled)
+			continue;
+
+		option = options[i].o_type;
+		if (acting_as_client)
+			value = options[i].o_request;
+		else
+			value = options[i].o_reply;
+		if (value == NULL)
+			continue;
+
+		length = strlen(value) + strlen(option) + 2;
+		if (size <= length) {
+			tftp_log(LOG_ERR,
+			    "Running out of option space for "
+			    "option '%s' with value '%s': "
+			    "needed %d bytes, got %d bytes",
+			    option, value, size, length);
+			continue;
+		}
+
+		sprintf(buffer, "%s%c%s%c", option, '\000', value, '\000');
+		size -= length;
+		buffer += length;
+		returnsize += length;
+	}
+
+	return (returnsize);
+}
+
+/*
+ * Parse the received options in the header
+ */
+int
+parse_options(int peer, char *buffer, uint16_t size)
+{
+	int	i, options_failed;
+	char	*c, *cp, *option, *value;
+
+	if (!options_rfc_enabled) return (0);
+
+	/* Parse the options */
+	cp = buffer;
+	options_failed = 0;	
+	while (size > 0) {
+		option = cp;
+		i = get_field(peer, cp, size);
+		cp += i;
+
+		value = cp;
+		i = get_field(peer, cp, size);
+		cp += i;
+
+		/* We are at the end */
+		if (*option == '\0') break;
+
+		if (debug&DEBUG_OPTIONS)
+			tftp_log(LOG_DEBUG,
+			    "option: '%s' value: '%s'", option, value);
+
+		for (c = option; *c; c++)
+			if (isupper(*c))
+				*c = tolower(*c);
+		for (i = 0; options[i].o_type != NULL; i++) {
+			if (strcmp(option, options[i].o_type) == 0) {
+				if (!acting_as_client)
+					options[i].o_request = value;
+				if (!options_extra_enabled && !options[i].rfc) {
+					tftp_log(LOG_INFO,
+					    "Option '%s' with value '%s' found "
+					    "but it is not an RFC option",
+					    option, value);
+					continue;
+				}
+				if (options[i].o_handler)
+					options_failed +=
+					    (options[i].o_handler)(peer);
+				break;
+			}
+		}
+		if (options[i].o_type == NULL)
+			tftp_log(LOG_WARNING,
+			    "Unknown option: '%s'", option);
+
+		size -= strlen(option) + strlen(value) + 2;
+	}
+
+	return (options_failed);
+}
+
+/*
+ * Set some default values in the options
+ */
+void
+init_options(void)
+{
+
+	options[OPT_ROLLOVER].o_request = strdup("0");
+}
diff --git a/libexec/tftpd/tftp-options.h b/libexec/tftpd/tftp-options.h
new file mode 100644
index 0000000..d8bd2fc
--- /dev/null
+++ b/libexec/tftpd/tftp-options.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+/*
+ * Options
+ */
+
+void		init_options(void);
+uint16_t	make_options(int peer, char *buffer, uint16_t size);
+int		parse_options(int peer, char *buffer, uint16_t size);
+
+/* Call back functions */
+int	option_tsize(int peer, struct tftphdr *, int, struct stat *);
+int	option_timeout(int peer);
+int	option_blksize(int peer);
+int	option_blksize2(int peer);
+int	option_rollover(int peer);
+
+extern int options_extra_enabled;
+extern int options_rfc_enabled;
+
+struct options {
+	const char	*o_type;
+	char		*o_request;
+	char		*o_reply;
+	int		(*o_handler)(int peer);
+	int		rfc;
+};
+
+extern struct options	options[];
+enum opt_enum {
+	OPT_TSIZE = 0,
+	OPT_TIMEOUT,
+	OPT_BLKSIZE,
+	OPT_BLKSIZE2,
+	OPT_ROLLOVER,
+};
diff --git a/libexec/tftpd/tftp-transfer.c b/libexec/tftpd/tftp-transfer.c
new file mode 100644
index 0000000..fd0e00d
--- /dev/null
+++ b/libexec/tftpd/tftp-transfer.c
@@ -0,0 +1,318 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+#include <sys/param.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/socket.h>
+
+#include <netinet/in.h>
+#include <arpa/tftp.h>
+
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <syslog.h>
+
+#include "tftp-file.h"
+#include "tftp-io.h"
+#include "tftp-utils.h"
+#include "tftp-options.h"
+#include "tftp-transfer.h"
+
+/*
+ * Send a file via the TFTP data session.
+ */
+void
+tftp_send(int peer, uint16_t *block, struct tftp_stats *ts)
+{
+	struct tftphdr *rp;
+	int size, n_data, n_ack, try;
+	uint16_t oldblock;
+	char sendbuffer[MAXPKTSIZE];
+	char recvbuffer[MAXPKTSIZE];
+
+	rp = (struct tftphdr *)recvbuffer;
+	*block = 1;
+	ts->amount = 0;
+	do {
+		if (debug&DEBUG_SIMPLE)
+			tftp_log(LOG_DEBUG, "Sending block %d", *block);
+
+		size = read_file(sendbuffer, segsize);
+		if (size < 0) {
+			tftp_log(LOG_ERR, "read_file returned %d", size);
+			send_error(peer, errno + 100);
+			goto abort;
+		}
+
+		for (try = 0; ; try++) {
+			n_data = send_data(peer, *block, sendbuffer, size);
+			if (n_data > 0) {
+				if (try == maxtimeouts) {
+					tftp_log(LOG_ERR,
+					    "Cannot send DATA packet #%d, "
+					    "giving up", *block);
+					return;
+				}
+				tftp_log(LOG_ERR,
+				    "Cannot send DATA packet #%d, trying again",
+				    *block);
+				continue;
+			}
+
+			n_ack = receive_packet(peer, recvbuffer,
+			    MAXPKTSIZE, NULL, timeoutpacket);
+			if (n_ack < 0) {
+				if (n_ack == RP_TIMEOUT) {
+					if (try == maxtimeouts) {
+						tftp_log(LOG_ERR,
+						    "Timeout #%d send ACK %d "
+						    "giving up", try, *block);
+						return;
+					}
+					tftp_log(LOG_WARNING,
+					    "Timeout #%d on ACK %d",
+					    try, *block);
+					continue;
+				}
+
+				/* Either read failure or ERROR packet */
+				if (debug&DEBUG_SIMPLE)
+					tftp_log(LOG_ERR, "Aborting: %s",
+					    rp_strerror(n_ack));
+				goto abort;
+			}
+			if (rp->th_opcode == ACK) {
+				ts->blocks++;
+				if (rp->th_block == *block) {
+					ts->amount += size;
+					break;
+				}
+
+				/* Re-synchronize with the other side */
+				(void) synchnet(peer);
+				if (rp->th_block == (*block - 1)) {
+					ts->retries++;
+					continue;
+				}
+			}
+
+		}
+		oldblock = *block;
+		(*block)++;
+		if (oldblock > *block) {
+			if (options[OPT_ROLLOVER].o_request == NULL) {
+				tftp_log(LOG_ERR,
+				    "Block rollover but not allowed.");
+				send_error(peer, EBADOP);
+				gettimeofday(&(ts->tstop), NULL);
+				return;
+			}
+
+			*block = atoi(options[OPT_ROLLOVER].o_request);
+			ts->rollovers++;
+		}
+		gettimeofday(&(ts->tstop), NULL);
+	} while (size == segsize);
+abort:
+	return;
+}
+
+/*
+ * Receive a file via the TFTP data session.
+ *
+ * - It could be that the first block has already arrived while
+ *   trying to figure out if we were receiving options or not. In
+ *   that case it is passed to this function.
+ */
+void
+tftp_receive(int peer, uint16_t *block, struct tftp_stats *ts,
+    struct tftphdr *firstblock, size_t fb_size)
+{
+	struct tftphdr *rp;
+	uint16_t oldblock;
+	int n_data, n_ack, writesize, i, retry;
+	char recvbuffer[MAXPKTSIZE];
+
+	ts->amount = 0;
+
+	if (firstblock != NULL) {
+		writesize = write_file(firstblock->th_data, fb_size);
+		ts->amount += writesize;
+		for (i = 0; ; i++) {
+			n_ack = send_ack(peer, *block);
+			if (n_ack > 0) {
+				if (i == maxtimeouts) {
+					tftp_log(LOG_ERR,
+					    "Cannot send ACK packet #%d, "
+					    "giving up", *block);
+					return;
+				}
+				tftp_log(LOG_ERR,
+				    "Cannot send ACK packet #%d, trying again",
+				    *block);
+				continue;
+			}
+
+			break;
+		}
+
+		if (fb_size != segsize) {
+			gettimeofday(&(ts->tstop), NULL);
+			return;
+		}
+	}
+
+	rp = (struct tftphdr *)recvbuffer;
+	do {
+		oldblock = *block;
+		(*block)++;
+		if (oldblock > *block) {
+			if (options[OPT_ROLLOVER].o_request == NULL) {
+				tftp_log(LOG_ERR,
+				    "Block rollover but not allowed.");
+				send_error(peer, EBADOP);
+				gettimeofday(&(ts->tstop), NULL);
+				return;
+			}
+
+			*block = atoi(options[OPT_ROLLOVER].o_request);
+			ts->rollovers++;
+		}
+
+		for (retry = 0; ; retry++) {
+			if (debug&DEBUG_SIMPLE)
+				tftp_log(LOG_DEBUG,
+				    "Receiving DATA block %d", *block);
+
+			n_data = receive_packet(peer, recvbuffer,
+			    MAXPKTSIZE, NULL, timeoutpacket);
+			if (n_data < 0) {
+				if (retry == maxtimeouts) {
+					tftp_log(LOG_ERR,
+					    "Timeout #%d on DATA block %d, "
+					    "giving up", retry, *block);
+					return;
+				}
+				if (n_data == RP_TIMEOUT) {
+					tftp_log(LOG_WARNING,
+					    "Timeout #%d on DATA block %d",
+					    retry, *block);
+					send_ack(peer, oldblock);
+					continue;
+				}
+
+				/* Either read failure or ERROR packet */
+				if (debug&DEBUG_SIMPLE)
+					tftp_log(LOG_DEBUG, "Aborting: %s",
+					    rp_strerror(n_data));
+				goto abort;
+			}
+			if (rp->th_opcode == DATA) {
+				ts->blocks++;
+
+				if (rp->th_block == *block)
+					break;
+
+				tftp_log(LOG_WARNING,
+				    "Expected DATA block %d, got block %d",
+				    *block, rp->th_block);
+
+				/* Re-synchronize with the other side */
+				(void) synchnet(peer);
+				if (rp->th_block == (*block-1)) {
+					tftp_log(LOG_INFO, "Trying to sync");
+					*block = oldblock;
+					ts->retries++;
+					goto send_ack;	/* rexmit */
+				}
+
+			} else {
+				tftp_log(LOG_WARNING,
+				    "Expected DATA block, got %s block",
+				    packettype(rp->th_opcode));
+			}
+		}
+
+		if (n_data > 0) {
+			writesize = write_file(rp->th_data, n_data);
+			ts->amount += writesize;
+			if (writesize <= 0) {
+				tftp_log(LOG_ERR,
+				    "write_file returned %d", writesize);
+				if (writesize < 0)
+					send_error(peer, errno + 100);
+				else
+					send_error(peer, ENOSPACE);
+				goto abort;
+			}
+		}
+
+send_ack:
+		for (i = 0; ; i++) {
+			n_ack = send_ack(peer, *block);
+			if (n_ack > 0) {
+
+				if (i == maxtimeouts) {
+					tftp_log(LOG_ERR,
+					    "Cannot send ACK packet #%d, "
+					    "giving up", *block);
+					return;
+				}
+
+				tftp_log(LOG_ERR,
+				    "Cannot send ACK packet #%d, trying again",
+				    *block);
+				continue;
+			}
+
+			break;
+		}
+		gettimeofday(&(ts->tstop), NULL);
+	} while (n_data == segsize);
+
+	/* Don't do late packet management for the client implementation */
+	if (acting_as_client)
+		return;
+
+	for (i = 0; ; i++) {
+		n_data = receive_packet(peer, (char *)rp, pktsize,
+		    NULL, timeoutpacket);
+		if (n_data <= 0)
+			break;
+		if (n_data > 0 &&
+		    rp->th_opcode == DATA &&	/* and got a data block */
+		    *block == rp->th_block)	/* then my last ack was lost */
+			send_ack(peer, *block);	/* resend final ack */
+	}
+
+abort:
+	return;
+}
diff --git a/libexec/tftpd/tftp-transfer.h b/libexec/tftpd/tftp-transfer.h
new file mode 100644
index 0000000..2cfa2df
--- /dev/null
+++ b/libexec/tftpd/tftp-transfer.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+void	tftp_send(int peer, uint16_t *block, struct tftp_stats *tp);
+void	tftp_receive(int peer, uint16_t *block, struct tftp_stats *tp,
+	    struct tftphdr *firstblock, size_t fb_size);
diff --git a/libexec/tftpd/tftp-utils.c b/libexec/tftpd/tftp-utils.c
new file mode 100644
index 0000000..da58064
--- /dev/null
+++ b/libexec/tftpd/tftp-utils.c
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/socket.h>
+#include <sys/stat.h>
+
+#include <netinet/in.h>
+#include <arpa/tftp.h>
+
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+
+#include "tftp-utils.h"
+#include "tftp-io.h"
+
+/*
+ * Default values, can be changed later via the TFTP Options
+ */
+int		timeoutpacket = TIMEOUT;
+int		timeoutnetwork = MAX_TIMEOUTS * TIMEOUT;
+int		maxtimeouts = MAX_TIMEOUTS;
+uint16_t	segsize = SEGSIZE;
+uint16_t	pktsize = SEGSIZE + 4;
+
+int	acting_as_client;
+
+
+/*
+ * Set timeout values for packet reception. The idea is that you
+ * get 'maxtimeouts' of 5 seconds between 'timeoutpacket' (i.e. the
+ * first timeout) to 'timeoutnetwork' (i.e. the last timeout)
+ */
+int
+settimeouts(int _timeoutpacket, int _timeoutnetwork, int _maxtimeouts)
+{
+	int i;
+	
+	/* We cannot do impossible things */
+	if (_timeoutpacket >= _timeoutnetwork)
+		return (0);
+	
+	maxtimeouts = 0;
+	i = _timeoutpacket;
+	while (i < _timeoutnetwork || maxtimeouts < MIN_TIMEOUTS) {
+		maxtimeouts++;
+		i += 5;
+	}
+
+	timeoutpacket = _timeoutpacket;
+	timeoutnetwork = i;
+	return (1);
+}
+
+/* translate IPv4 mapped IPv6 address to IPv4 address */
+void
+unmappedaddr(struct sockaddr_in6 *sin6)
+{
+	struct sockaddr_in *sin4;
+	u_int32_t addr;
+	int port;
+
+	if (sin6->sin6_family != AF_INET6 ||
+	    !IN6_IS_ADDR_V4MAPPED(&sin6->sin6_addr))
+		return;
+	sin4 = (struct sockaddr_in *)sin6;
+	addr = *(u_int32_t *)&sin6->sin6_addr.s6_addr[12];
+	port = sin6->sin6_port;
+	memset(sin4, 0, sizeof(struct sockaddr_in));
+	sin4->sin_addr.s_addr = addr;
+	sin4->sin_port = port;
+	sin4->sin_family = AF_INET;
+	sin4->sin_len = sizeof(struct sockaddr_in);
+}
+
+/* Get a field from a \0 seperated string */
+ssize_t
+get_field(int peer, char *buffer, ssize_t size)
+{
+	char *cp = buffer;
+
+	while (cp < buffer + size) {
+		if (*cp == '\0') break;
+		cp++;
+	}
+	if (*cp != '\0') {
+		tftp_log(LOG_ERR, "Bad option - no trailing \\0 found");
+		send_error(peer, EBADOP);
+		exit(1);
+	}
+	return (cp - buffer + 1);
+}
+
+/*
+ * Logging functions
+ */
+int	_tftp_logtostdout = 1;
+
+void
+tftp_openlog(const char *ident, int logopt, int facility)
+{
+
+	_tftp_logtostdout = (ident == NULL);
+	if (_tftp_logtostdout == 0)
+		openlog(ident, logopt, facility);
+}
+
+void
+tftp_closelog(void)
+{
+
+	if (_tftp_logtostdout == 0)
+		closelog();
+}
+
+void
+tftp_log(int priority, const char *message, ...)
+{
+	va_list ap;
+	char *s;
+
+	va_start(ap, message);
+	if (_tftp_logtostdout == 0) {
+		vasprintf(&s, message, ap);
+		syslog(priority, "%s", s);
+	} else {
+		vprintf(message, ap);
+		printf("\n");
+	}
+	va_end(ap);
+}
+
+/*
+ * Packet types
+ */
+struct packettypes packettypes[] = {
+	{ RRQ,		"RRQ"	},
+	{ WRQ,		"WRQ"	},
+	{ DATA,		"DATA"	},
+	{ ACK,		"ACK"	},
+	{ ERROR,	"ERROR"	},
+	{ OACK,		"OACK"	},
+	{ 0,		NULL	},
+};
+
+char *
+packettype(int type)
+{
+	static char failed[100];
+	int i = 0;
+
+	while (packettypes[i].name != NULL) {
+		if (packettypes[i].value == type)
+			break;
+		i++;
+	}
+	if (packettypes[i].name != NULL)
+		return packettypes[i].name;
+	sprintf(failed, "unknown (type: %d)", type);
+	return (failed);
+}
+
+/*
+ * Debugs
+ */
+int	debug = DEBUG_NONE;
+struct debugs debugs[] = {
+	{ DEBUG_PACKETS,	"packet",	"Packet debugging"	},
+	{ DEBUG_SIMPLE,		"simple",	"Simple debugging"	},
+	{ DEBUG_OPTIONS,	"options",	"Options debugging"	},
+	{ DEBUG_ACCESS,		"access",	"TCPd access debugging"	},
+	{ DEBUG_NONE,		NULL,		"No debugging"		},
+};
+int	packetdroppercentage = 0;
+
+int
+debug_find(char *s)
+{
+	int i = 0;
+
+	while (debugs[i].name != NULL) {
+		if (strcasecmp(debugs[i].name, s) == 0)
+			break;
+		i++;
+	}
+	return (debugs[i].value);
+}
+
+int
+debug_finds(char *s)
+{
+	int i = 0;
+	char *ps = s;
+
+	while (s != NULL) {
+		ps = strchr(s, ' ');
+		if (ps != NULL)
+			*ps = '\0';
+		i += debug_find(s);
+		if (ps != NULL)
+			*ps = ' ';
+		s = ps;
+	}
+	return (i);
+}
+
+char *
+debug_show(int d)
+{
+	static char s[100];
+	int i = 0;
+
+	s[0] = '\0';
+	while (debugs[i].name != NULL) {
+		if (d&debugs[i].value) {
+			if (s[0] != '\0') 
+				strcat(s, " ");
+			strcat(s, debugs[i].name);
+		}
+		i++;
+	}
+	if (s[0] != '\0')
+		return (s);
+	return ("none");
+}
+
+/*
+ * RP_
+ */
+struct rp_errors rp_errors[] = {
+	{ RP_TIMEOUT,		"Network timeout" },
+	{ RP_TOOSMALL,		"Not enough data bytes" },
+	{ RP_WRONGSOURCE,	"Invalid IP address of UDP port" },
+	{ RP_ERROR,		"Error packet" },
+	{ RP_RECVFROM,		"recvfrom() complained" },
+	{ RP_TOOBIG,		"Too many data bytes" },
+	{ RP_NONE,		NULL }
+};
+
+char *
+rp_strerror(int error)
+{
+	static char s[100];
+	int i = 0;
+
+	while (rp_errors[i].desc != NULL) {
+		if (rp_errors[i].error == error) {
+			strcpy(s, rp_errors[i].desc);
+		}
+		i++;
+	}
+	if (s[0] == '\0')
+		sprintf(s, "unknown (error=%d)", error);
+	return (s);
+}
+
+/*
+ * Performance figures
+ */
+
+void
+stats_init(struct tftp_stats *ts)
+{
+
+	ts->amount = 0;
+	ts->rollovers = 0;
+	ts->retries = 0;
+	ts->blocks = 0;
+	ts->amount = 0;
+	gettimeofday(&(ts->tstart), NULL);
+}
+
+void
+printstats(const char *direction, int verbose, struct tftp_stats *ts)
+{
+	double delta;	/* compute delta in 1/10's second units */
+
+	delta = ((ts->tstop.tv_sec*10.)+(ts->tstop.tv_usec/100000)) -
+		((ts->tstart.tv_sec*10.)+(ts->tstart.tv_usec/100000));
+	delta = delta/10.;      /* back to seconds */
+
+	printf("%s %zu bytes during %.1f seconds in %u blocks",
+	    direction, ts->amount, delta, ts->blocks);
+
+	if (ts->rollovers != 0)
+		printf(" with %d rollover%s",
+		    ts->rollovers, ts->rollovers != 1 ? "s" : "");
+
+	if (verbose)
+		printf(" [%.0f bits/sec]", (ts->amount*8.)/delta);
+	putchar('\n');
+}
+
diff --git a/libexec/tftpd/tftp-utils.h b/libexec/tftpd/tftp-utils.h
new file mode 100644
index 0000000..d072479
--- /dev/null
+++ b/libexec/tftpd/tftp-utils.h
@@ -0,0 +1,124 @@
+/*
+ * Copyright (C) 2008 Edwin Groothuis. All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+/*
+ */
+#define	TIMEOUT		5
+#define	MAX_TIMEOUTS	5
+
+/* Generic values */
+#define MAXSEGSIZE	65464		/* Maximum size of the data segment */
+#define	MAXPKTSIZE	(MAXSEGSIZE + 4) /* Maximum size of the packet */
+
+/* For the blksize option */
+#define BLKSIZE_MIN	8		/* Minumum size of the data segment */
+#define BLKSIZE_MAX	MAXSEGSIZE	/* Maximum size of the data segment */
+
+/* For the timeout option */
+#define TIMEOUT_MIN	0		/* Minumum timeout value */
+#define TIMEOUT_MAX	255		/* Maximum timeout value */
+#define MIN_TIMEOUTS	3
+
+extern int	timeoutpacket;
+extern int	timeoutnetwork;
+extern int	maxtimeouts;
+int	settimeouts(int timeoutpacket, int timeoutnetwork, int maxtimeouts);
+
+extern uint16_t	segsize;
+extern uint16_t	pktsize;
+
+extern int	acting_as_client;
+
+/*
+ */
+void	unmappedaddr(struct sockaddr_in6 *sin6);
+ssize_t	get_field(int peer, char *buffer, ssize_t size);
+
+/*
+ * Packet types
+ */
+struct packettypes {
+	int	value;
+	char	*name;
+};
+extern struct packettypes packettypes[];
+char *packettype(int);
+
+/*
+ * RP_
+ */
+struct rp_errors {
+	int	error;
+	char	*desc;
+};
+extern struct rp_errors rp_errors[];
+char	*rp_strerror(int error);
+
+/*
+ * Debug features
+ */
+#define	DEBUG_NONE	0x0000
+#define DEBUG_PACKETS	0x0001
+#define DEBUG_SIMPLE	0x0002
+#define DEBUG_OPTIONS	0x0004
+#define DEBUG_ACCESS	0x0008
+struct debugs {
+	int	value;
+	char	*name;
+	char	*desc;
+};
+extern int	debug;
+extern struct debugs debugs[];
+extern int	packetdroppercentage;
+int	debug_find(char *s);
+int	debug_finds(char *s);
+char	*debug_show(int d);
+
+/*
+ * Log routines
+ */
+#define DEBUG(s) tftp_log(LOG_DEBUG, "%s", s)
+extern int tftp_logtostdout;
+void	tftp_openlog(const char *ident, int logopt, int facility);
+void	tftp_closelog(void);
+void	tftp_log(int priority, const char *message, ...);
+
+/*
+ * Performance figures
+ */
+struct tftp_stats {
+	size_t		amount;
+	int		rollovers;
+	uint32_t	blocks;
+	int		retries;
+	struct timeval	tstart;
+	struct timeval	tstop;
+};
+
+void	stats_init(struct tftp_stats *ts);
+void	printstats(const char *direction, int verbose, struct tftp_stats *ts);