Merge branch 'for-next/tftp'

7 files changed, 846 insertions, 102 deletions

diff --git a/Documentation/filesystems/tftp.rst b/Documentation/filesystems/tftp.rst
index a292765e25..e88ad3dd4c 100644
--- a/Documentation/filesystems/tftp.rst
+++ b/Documentation/filesystems/tftp.rst
@@ -20,3 +20,43 @@ Example:
 
 In addition to the TFTP filesystem implementation, barebox does also have a
 :ref:`tftp command <command_tftp>`.
+
+RFC 7440 "windowsize" support
+=============================
+
+barebox supports the tftp windowsize option for downloading files.  It
+is not implemented for uploads.
+
+Generally, this option greatly improves the download speed (factors
+4-30 are not uncommon).  But choosing a too large windowsize can have
+the opposite effect.  Performance depends on:
+
+ - the network infrastructure: when the tftp server sends files with
+   1Gb/s but there are components in the network (switches or the
+   target nic) which support only 100 Mb/s, packets will be dropped.
+
+   The lower the internal buffers of the bottleneck components, the
+   lower the optimal window size.
+
+   In practice (iMX8MP on a Netgear GS108Ev3 with a port configured to
+   100 Mb/s) it had to be reduced to
+
+   .. code-block:: console
+
+     global tftp.windowsize=26
+
+   for example.
+
+ - the target network driver: datagrams from server will arive faster
+   than they can be processed and must be buffered internally.  For
+   example, the `fec-imx` driver reserves place for
+
+   .. code-block:: c
+
+     #define FEC_RBD_NUM		64
+
+   packets before they are dropped
+
+ - partially the workload: copying downloaded files to ram will be
+   faster than burning them into flash.  Latter can consume internal
+   buffers quicker so that windowsize might be reduced
diff --git a/arch/arm/configs/imx_v7_defconfig b/arch/arm/configs/imx_v7_defconfig
index df2304eae9..d07abe28ae 100644
--- a/arch/arm/configs/imx_v7_defconfig
+++ b/arch/arm/configs/imx_v7_defconfig
@@ -210,6 +210,7 @@ CONFIG_GENERIC_PHY=y
 CONFIG_USB_NOP_XCEIV=y
 CONFIG_FS_EXT4=y
 CONFIG_FS_TFTP=y
+CONFIG_FS_TFTP_MAX_WINDOW_SIZE=8
 CONFIG_FS_NFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FAT_WRITE=y
diff --git a/commands/tftp.c b/commands/tftp.c
index 48ff00c621..6ac822c9e8 100644
--- a/commands/tftp.c
+++ b/commands/tftp.c
@@ -21,15 +21,24 @@ static int do_tftpb(int argc, char *argv[])
 	char *source, *dest, *freep;
 	int opt;
 	int tftp_push = 0;
+	int port = -1;
 	int ret;
 	IPaddr_t ip;
 	char ip4_str[sizeof("255.255.255.255")];
+	char mount_opts[sizeof("port=12345")];
 
-	while ((opt = getopt(argc, argv, "p")) > 0) {
+	while ((opt = getopt(argc, argv, "pP:")) > 0) {
 		switch(opt) {
 		case 'p':
 			tftp_push = 1;
 			break;
+		case 'P':
+			port = simple_strtoul(optarg, NULL, 0);
+			if (port <= 0 || port > 0xffff) {
+				pr_err("invalid port '%s'\n", optarg);
+				return COMMAND_ERROR_USAGE;
+			}
+			break;
 		default:
 			return COMMAND_ERROR_USAGE;
 		}
@@ -59,7 +68,13 @@ static int do_tftpb(int argc, char *argv[])
 
 	ip = net_get_serverip();
 	sprintf(ip4_str, "%pI4", &ip);
-	ret = mount(ip4_str, "tftp", TFTP_MOUNT_PATH, NULL);
+
+	if (port >= 0)
+		sprintf(mount_opts, "port=%u", port);
+	else
+		mount_opts[0] = '\0';
+
+	ret = mount(ip4_str, "tftp", TFTP_MOUNT_PATH, mount_opts);
 	if (ret)
 		goto err_rmdir;
 
@@ -84,12 +99,13 @@ BAREBOX_CMD_HELP_TEXT("server address is taken from the environment (ethX.server
 BAREBOX_CMD_HELP_TEXT("")
 BAREBOX_CMD_HELP_TEXT("Options:")
 BAREBOX_CMD_HELP_OPT ("-p", "push to TFTP server")
+BAREBOX_CMD_HELP_OPT ("-P PORT", "tftp server port number")
 BAREBOX_CMD_HELP_END
 
 BAREBOX_CMD_START(tftp)
 	.cmd		= do_tftpb,
 	BAREBOX_CMD_DESC("load (or save) a file using TFTP")
-	BAREBOX_CMD_OPTS("[-p] SOURCE [DEST]")
+	BAREBOX_CMD_OPTS("[-p] [-P <port>] SOURCE [DEST]")
 	BAREBOX_CMD_GROUP(CMD_GRP_NET)
 	BAREBOX_CMD_HELP(cmd_tftp_help)
 BAREBOX_CMD_END
diff --git a/fs/Kconfig b/fs/Kconfig
index aeba00073e..cf884e0646 100644
--- a/fs/Kconfig
+++ b/fs/Kconfig
@@ -43,6 +43,42 @@ config FS_TFTP
 	prompt "tftp support"
 	depends on NET
 
+config FS_TFTP_MAX_WINDOW_SIZE
+	int
+	prompt "maximum tftp window size (RFC 7440)"
+	depends on FS_TFTP
+	default 128
+	range 1 128
+	help
+	  The maximum allowed tftp "windowsize" (RFC 7440).  Higher
+	  value increase speed of the tftp download with the cost of
+	  memory (1432 bytes per slot).
+
+	  Requires tftp "windowsize" (RFC 7440) support on server side
+	  to have an effect.
+
+config FS_TFTP_REORDER_CACHE_SIZE
+	int
+	prompt "number of out-of-order tftp packets to be cached"
+	depends on FS_TFTP
+	default 16 if FS_TFTP_MAX_WINDOW_SIZE > 16
+	default  0 if FS_TFTP_MAX_WINDOW_SIZE = 1
+        ## TODO: it should be 'FS_TFTP_MAX_WINDOW_SIZE - 1' but this
+        ## is not supported by Kconfig
+	default FS_TFTP_MAX_WINDOW_SIZE
+	range 0 FS_TFTP_MAX_WINDOW_SIZE
+	help
+	  UDP allows reordering of datagrams; with this option,
+	  unexpected tftp packets will be cached and later
+	  reassembled.  This increases stability of the tftp download
+	  with the cost of memory (around 1440 bytes per cache
+	  element) and barebox binary size (around 700 bytes).
+
+	  A value of 0 disables caching.
+
+	  Requires tftp "windowsize" (RFC 7440) support on server side
+	  to have an effect.
+
 config FS_OMAP4_USBBOOT
 	bool
 	prompt "Filesystem over usb boot"
diff --git a/fs/tftp-selftest.h b/fs/tftp-selftest.h
new file mode 100644
index 0000000000..2406ed329e
--- /dev/null
+++ b/fs/tftp-selftest.h
@@ -0,0 +1,56 @@
+// SPDX-License-Identifier: GPL-2.0+
+// SPDX-FileCopyrightText: 2022 Enrico Scholz <enrico.scholz@sigma-chemnitz.de>
+
+#ifndef H_BAREBOX_FS_TFTP_SELFTEST_H
+#define H_BAREBOX_FS_TFTP_SELFTEST_H
+
+#include <bselftest.h>
+
+#define _expect_fmt(_v) _Generic(_v,				\
+				 void const *: "%p",		\
+				 void *: "%p",			\
+				 bool: "%d",			\
+				 long int: "%lld",		\
+				 long unsigned int: "%llu",	\
+				 unsigned short: "%h",		\
+				 signed int: "%d",		\
+				 unsigned int: "%u")
+
+#define _expect_op(_a, _b, _op)						\
+	({								\
+		__typeof__(_a)	__a = (_a);				\
+		__typeof__(_b)	__b = (_b);				\
+									\
+		total_tests++;						\
+									\
+		if (!(__a _op __b)) {					\
+			char fmt[sizeof "%s:%d: failed: %XXX  %XXX\n" # _op]; \
+			strcpy(fmt, "%s:%d: failed: ");			\
+			strcat(fmt, _expect_fmt(__a));			\
+			strcat(fmt, " " # _op " ");			\
+			strcat(fmt, _expect_fmt(__b));			\
+			strcat(fmt, "\n");				\
+									\
+			failed_tests++;					\
+			printf(fmt, __func__, __LINE__, __a, __b);	\
+		}							\
+	})
+
+#define _as_void(_p) ({					\
+			void const *__res = (_p);	\
+			__res;				\
+		})
+
+#define expect_eq(_a, _b)	_expect_op(_a, _b, ==);
+#define expect_ne(_a, _b)	_expect_op(_a, _b, !=);
+#define expect_it(_a)		_expect_op(_a, true, ==);
+
+#define expect_err(_a)		_expect_op(_a, 0, <);
+#define expect_ok(_a)		_expect_op(_a, 0, ==);
+
+/* _Generic() does not match 'void *' for typed pointers; cast them to raw
+   'void *' first */
+#define expect_NULL(_a)		_expect_op(_as_void(_a), NULL, ==);
+#define expect_PTR(_a)		_expect_op(_as_void(_a), NULL, !=);
+
+#endif	/* H_BAREBOX_FS_TFTP_SELFTEST_H */
diff --git a/fs/tftp.c b/fs/tftp.c
index d186e7983a..e0886c49d2 100644
--- a/fs/tftp.c
+++ b/fs/tftp.c
@@ -26,12 +26,18 @@
 #include <libgen.h>
 #include <fcntl.h>
 #include <getopt.h>
+#include <globalvar.h>
 #include <init.h>
+#include <linux/bitmap.h>
 #include <linux/stat.h>
 #include <linux/err.h>
+#include <linux/kernel.h>
 #include <kfifo.h>
+#include <parseopt.h>
 #include <linux/sizes.h>
 
+#include "tftp-selftest.h"
+
 #define TFTP_PORT	69	/* Well known TFTP port number */
 
 /* Seconds to wait before remote server is allowed to resend a lost packet */
@@ -58,21 +64,69 @@
 #define STATE_WRQ	2
 #define STATE_RDATA	3
 #define STATE_WDATA	4
-#define STATE_OACK	5
+/* OACK from server has been received and we can begin to sent either the ACK
+   (for RRQ) or data (for WRQ) */
+#define STATE_START	5
 #define STATE_WAITACK	6
 #define STATE_LAST	7
 #define STATE_DONE	8
 
 #define TFTP_BLOCK_SIZE		512	/* default TFTP block size */
-#define TFTP_FIFO_SIZE		4096
+#define TFTP_MTU_SIZE		1432	/* MTU based block size */
+#define TFTP_MAX_WINDOW_SIZE	CONFIG_FS_TFTP_MAX_WINDOW_SIZE
+
+/* allocate this number of blocks more than needed in the fifo */
+#define TFTP_EXTRA_BLOCKS	2
+
+/* size of cache which deals with udp reordering */
+#define TFTP_WINDOW_CACHE_NUM	CONFIG_FS_TFTP_REORDER_CACHE_SIZE
+
+/* marker for an emtpy 'tftp_cache' */
+#define TFTP_CACHE_NO_ID	(-1)
 
 #define TFTP_ERR_RESEND	1
 
+#if defined(DEBUG) || IS_ENABLED(CONFIG_SELFTEST_TFTP)
+#  define debug_assert(_cond)	BUG_ON(!(_cond))
+#else
+#  define debug_assert(_cond) do {			\
+		if (!(_cond))				\
+			__builtin_unreachable();	\
+	} while (0)
+#endif
+
+static int g_tftp_window_size = DIV_ROUND_UP(TFTP_MAX_WINDOW_SIZE, 2);
+
+struct tftp_block {
+	uint16_t id;
+	uint16_t len;
+
+	struct list_head head;
+	uint8_t data[];
+};
+
+struct tftp_cache {
+	/* The id located at @pos or TFTP_CACHE_NO_ID when cache is empty.  It
+	   is possible that the corresponding bit in @used is NOT set. */
+	unsigned int		id;
+	unsigned int		pos;
+
+	/* bitmask */
+	unsigned long		used[BITS_TO_LONGS(TFTP_WINDOW_CACHE_NUM)];
+
+	/* size of the cache; is limited by TFTP_WINDOW_CACHE_NUM and the
+	   actual window size */
+	unsigned int		size;
+	unsigned int		block_len;
+	struct tftp_block	*blocks[TFTP_WINDOW_CACHE_NUM];
+};
+
 struct file_priv {
 	struct net_connection *tftp_con;
 	int push;
 	uint16_t block;
 	uint16_t last_block;
+	uint16_t ack_block;
 	int state;
 	int err;
 	char *filename;
@@ -82,28 +136,239 @@ struct file_priv {
 	struct kfifo *fifo;
 	void *buf;
 	int blocksize;
-	int block_requested;
+	unsigned int windowsize;
+	bool is_getattr;
+	struct tftp_cache cache;
 };
 
 struct tftp_priv {
 	IPaddr_t server;
 };
 
+static inline bool is_block_before(uint16_t a, uint16_t b)
+{
+	return (int16_t)(b - a) > 0;
+}
+
+static inline uint16_t get_block_delta(uint16_t a, uint16_t b)
+{
+	debug_assert(!is_block_before(b, a));
+
+	return b - a;
+}
+
+static bool in_window(uint16_t block, uint16_t start, uint16_t end)
+{
+	/* handle the three cases:
+	   - [ ......... | start | .. | BLOCK | .. | end | ......... ]
+	   - [ ..| BLOCK | .. | end | ................. | start | .. ]
+	   - [ ..| end | ................. | start | .. | BLOCK | .. ]
+	*/
+	return ((start <= block && block <= end) ||
+		(block <= end   && end   <= start) ||
+		(end   <= start && start <= block));
+}
+
+static inline size_t tftp_window_cache_size(struct tftp_cache const *cache)
+{
+	/* allows to optimize away the cache code when TFTP_WINDOW_CACHE_SIZE
+	   is 0 */
+	return TFTP_WINDOW_CACHE_NUM == 0 ? 0 : cache->size;
+}
+
+static void tftp_window_cache_init(struct tftp_cache *cache,
+				   uint16_t block_len, uint16_t window_size)
+{
+	debug_assert(window_size > 0);
+
+	*cache = (struct tftp_cache) {
+		.id		= TFTP_CACHE_NO_ID,
+		.block_len	= block_len,
+		.size		= min_t(uint16_t, window_size - 1,
+					ARRAY_SIZE(cache->blocks)),
+	};
+}
+
+static void tftp_window_cache_free(struct tftp_cache *cache)
+{
+	size_t	cache_size = tftp_window_cache_size(cache);
+	size_t	i;
+
+	for (i = 0; i < cache_size; ++i) {
+		free(cache->blocks[i]);
+		cache->blocks[i] = NULL;
+	}
+}
+
+static void tftp_window_cache_reset(struct tftp_cache *cache)
+{
+	cache->id = TFTP_CACHE_NO_ID;
+	memset(cache->used, 0, sizeof cache->used);
+}
+
+static int tftp_window_cache_get_pos(struct tftp_cache const *cache, uint16_t id)
+{
+	size_t		cache_size = tftp_window_cache_size(cache);
+	unsigned int	pos;
+
+	if (cache_size == 0)
+		return -1;
+
+	if (cache->id == TFTP_CACHE_NO_ID)
+		return -1;
+
+	if (!in_window(id, cache->id, cache->id + cache_size - 1))
+		return -1;
+
+	pos  = cache->pos + get_block_delta(cache->id, id);
+	pos %= cache_size;
+
+	return pos;
+}
+
+/* returns whether the first cached element has the given @id */
+static bool tftp_window_cache_starts_with(struct tftp_cache const *cache,
+					  uint16_t id)
+{
+	return (TFTP_WINDOW_CACHE_NUM > 0 &&
+		cache->id != TFTP_CACHE_NO_ID &&
+		cache->id == id &&
+		test_bit(cache->pos, cache->used));
+}
+
+static bool tftp_window_cache_is_empty(struct tftp_cache const *cache)
+{
+	/* use this indirection to avoid warnings about a '0 < 0' comparison
+	   in the loop condition when TFTP_WINDOW_CACHE_NUM is zero */
+	size_t	cache_size = ARRAY_SIZE(cache->used);
+	size_t	i;
+
+	for (i = 0; i < cache_size; ++i) {
+		if (cache->used[i] != 0)
+			return false;
+	}
+
+	return true;
+}
+
+static int tftp_window_cache_insert(struct tftp_cache *cache, uint16_t id,
+				    void const *data, size_t len)
+{
+	size_t const		cache_size = tftp_window_cache_size(cache);
+	int			pos;
+	struct tftp_block	*block;
+
+	if (cache_size == 0)
+		return -ENOSPC;
+
+	if (cache->id == TFTP_CACHE_NO_ID) {
+		/* initialize cache when it does not contain elements yet */
+		cache->id = id;
+		cache->pos = 0;
+
+		/* sanity check; cache is expected to be empty */
+		debug_assert(tftp_window_cache_is_empty(cache));
+	}
+
+	pos = tftp_window_cache_get_pos(cache, id);
+	if (pos < 0)
+		return -ENOSPC;
+
+	debug_assert(pos < cache_size);
+
+	if (test_bit(pos, cache->used))
+		/* block already cached */
+		return 0;
+
+	block = cache->blocks[pos];
+	if (!block) {
+		/* allocate space for the block; after being released, this
+		   memory can be reused for other blocks during the same tftp
+		   transfer */
+		block = malloc(sizeof *block + cache->block_len);
+		if (!block)
+			return -ENOMEM;
+
+		cache->blocks[pos] = block;
+	}
+
+	__set_bit(pos, cache->used);
+	memcpy(block->data, data, len);
+	block->id = id;
+	block->len = len;
+
+	return 0;
+}
+
+/* Marks the element at 'pos' as unused and updates internal cache information.
+   Returns true iff element at pos was valid. */
+static bool tftp_window_cache_remove(struct tftp_cache *cache, unsigned int pos)
+{
+	size_t const	cache_size = tftp_window_cache_size(cache);
+	bool		res;
+
+	if (cache_size == 0)
+		return 0;
+
+	res = __test_and_clear_bit(pos, cache->used);
+
+	if (tftp_window_cache_is_empty(cache)) {
+		/* cache has been cleared; reset pointers */
+		cache->id = TFTP_CACHE_NO_ID;
+	} else if (pos != cache->pos) {
+		/* noop; elements has been removed from the middle of cache */
+	} else {
+		/* first element of cache has been removed; proceed to the
+		   next one */
+		cache->id  += 1;
+		cache->pos += 1;
+		cache->pos %= cache_size;
+	}
+
+	return res;
+}
+
+/* Releases the first element from the cache and returns its content.
+ *
+ * Function can return NULL when the element is not cached
+ */
+static struct tftp_block *tftp_window_cache_pop(struct tftp_cache *cache)
+{
+	unsigned int	pos = cache->pos;
+
+	debug_assert(cache->id != TFTP_CACHE_NO_ID);
+
+	if (!tftp_window_cache_remove(cache, pos))
+		return NULL;
+
+	return cache->blocks[pos];
+}
+
+static bool tftp_window_cache_remove_id(struct tftp_cache *cache, uint16_t id)
+{
+	int		pos = tftp_window_cache_get_pos(cache, id);
+
+	if (pos < 0)
+		return false;
+
+	return tftp_window_cache_remove(cache, pos);
+}
+
 static int tftp_truncate(struct device_d *dev, FILE *f, loff_t size)
 {
 	return 0;
 }
 
-static char *tftp_states[] = {
+static char const * const tftp_states[] = {
 	[STATE_INVALID] = "INVALID",
 	[STATE_RRQ] = "RRQ",
 	[STATE_WRQ] = "WRQ",
 	[STATE_RDATA] = "RDATA",
 	[STATE_WDATA] = "WDATA",
-	[STATE_OACK] = "OACK",
 	[STATE_WAITACK] = "WAITACK",
 	[STATE_LAST] = "LAST",
 	[STATE_DONE] = "DONE",
+	[STATE_START] = "START",
 };
 
 static int tftp_send(struct file_priv *priv)
@@ -112,6 +377,7 @@ static int tftp_send(struct file_priv *priv)
 	int len = 0;
 	uint16_t *s;
 	unsigned char *pkt = net_udp_get_payload(priv->tftp_con);
+	unsigned int window_size;
 	int ret;
 
 	pr_vdebug("%s: state %s\n", __func__, tftp_states[priv->state]);
@@ -119,6 +385,15 @@ static int tftp_send(struct file_priv *priv)
 	switch (priv->state) {
 	case STATE_RRQ:
 	case STATE_WRQ:
+		if (priv->push || priv->is_getattr)
+			/* atm, windowsize is supported only for RRQ and there
+			   is no need to request a full window when we are
+			   just looking up file attributes */
+			window_size = 1;
+		else
+			window_size = min_t(unsigned int, g_tftp_window_size,
+					    TFTP_MAX_WINDOW_SIZE);
+
 		xp = pkt;
 		s = (uint16_t *)pkt;
 		if (priv->state == STATE_RRQ)
@@ -131,32 +406,45 @@ static int tftp_send(struct file_priv *priv)
 				"octet%c"
 				"timeout%c"
 				"%d%c"
-				"tsize%c"
-				"%lld%c"
 				"blksize%c"
-				"1432",
-				priv->filename + 1, 0,
-				0,
-				0,
-				TIMEOUT, 0,
-				0,
-				priv->filesize, 0,
-				0);
+				"%u",
+				priv->filename + 1, '\0',
+				'\0',	/* "octet" */
+				'\0',	/* "timeout" */
+				TIMEOUT, '\0',
+				'\0',	/* "blksize" */
+				/* use only a minimal blksize for getattr
+				   operations, */
+				priv->is_getattr ? TFTP_BLOCK_SIZE : TFTP_MTU_SIZE);
 		pkt++;
+
+		if (!priv->push)
+			/* we do not know the filesize in WRQ requests and
+			   'priv->filesize' will always be zero */
+			pkt += sprintf((unsigned char *)pkt,
+				       "tsize%c%lld%c",
+				       '\0', priv->filesize,
+				       '\0');
+
+		if (window_size > 1)
+			pkt += sprintf((unsigned char *)pkt,
+				       "windowsize%c%u%c",
+				       '\0', window_size,
+				       '\0');
+
 		len = pkt - xp;
 		break;
 
 	case STATE_RDATA:
-		if (priv->block == priv->block_requested)
-			return 0;
-	case STATE_OACK:
 		xp = pkt;
 		s = (uint16_t *)pkt;
 		*s++ = htons(TFTP_ACK);
-		*s++ = htons(priv->block);
-		priv->block_requested = priv->block;
+		*s++ = htons(priv->last_block);
+		priv->ack_block  = priv->last_block;
+		priv->ack_block += priv->windowsize;
 		pkt = (unsigned char *)s;
 		len = pkt - xp;
+		tftp_window_cache_reset(&priv->cache);
 		break;
 	}
 
@@ -197,7 +485,6 @@ static int tftp_poll(struct file_priv *priv)
 	if (is_timeout(priv->resend_timeout, TFTP_RESEND_TIMEOUT)) {
 		printf("T ");
 		priv->resend_timeout = get_time_ns();
-		priv->block_requested = -1;
 		return TFTP_ERR_RESEND;
 	}
 
@@ -212,7 +499,7 @@ static int tftp_poll(struct file_priv *priv)
 	return 0;
 }
 
-static void tftp_parse_oack(struct file_priv *priv, unsigned char *pkt, int len)
+static int tftp_parse_oack(struct file_priv *priv, unsigned char *pkt, int len)
 {
 	unsigned char *opt, *val, *s;
 
@@ -229,14 +516,25 @@ static void tftp_parse_oack(struct file_priv *priv, unsigned char *pkt, int len)
 		opt = s;
 		val = s + strlen(s) + 1;
 		if (val > s + len)
-			return;
+			break;
 		if (!strcmp(opt, "tsize"))
 			priv->filesize = simple_strtoull(val, NULL, 10);
 		if (!strcmp(opt, "blksize"))
 			priv->blocksize = simple_strtoul(val, NULL, 10);
+		if (!strcmp(opt, "windowsize"))
+			priv->windowsize = simple_strtoul(val, NULL, 10);
 		pr_debug("OACK opt: %s val: %s\n", opt, val);
 		s = val + strlen(val) + 1;
 	}
+
+	if (priv->blocksize > TFTP_MTU_SIZE ||
+	    priv->windowsize > TFTP_MAX_WINDOW_SIZE ||
+	    priv->windowsize == 0) {
+		pr_warn("tftp: invalid oack response\n");
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 static void tftp_timer_reset(struct file_priv *priv)
@@ -244,10 +542,126 @@ static void tftp_timer_reset(struct file_priv *priv)
 	priv->progress_timeout = priv->resend_timeout = get_time_ns();
 }
 
+static int tftp_allocate_transfer(struct file_priv *priv)
+{
+	debug_assert(!priv->fifo);
+	debug_assert(!priv->buf);
+
+	/* multiplication is safe; both operands were checked in tftp_parse_oack()
+	   and are small integers */
+	priv->fifo = kfifo_alloc(priv->blocksize *
+				 (priv->windowsize + TFTP_EXTRA_BLOCKS));
+	if (!priv->fifo)
+		goto err;
+
+	if (priv->push) {
+		priv->buf = xmalloc(priv->blocksize);
+		if (!priv->buf) {
+			kfifo_free(priv->fifo);
+			priv->fifo = NULL;
+			goto err;
+		}
+	} else {
+		tftp_window_cache_init(&priv->cache,
+				       priv->blocksize, priv->windowsize);
+	}
+
+	return 0;
+
+err:
+	priv->err = -ENOMEM;
+	priv->state = STATE_DONE;
+
+	return priv->err;
+}
+
+static void tftp_put_data(struct file_priv *priv, uint16_t block,
+			  void const *pkt, size_t len)
+{
+	unsigned int sz;
+
+	if (len > priv->blocksize) {
+		pr_warn("tftp: oversized packet (%zu > %d) received\n",
+			len, priv->blocksize);
+		return;
+	}
+
+	priv->last_block = block;
+
+	sz = kfifo_put(priv->fifo, pkt, len);
+
+	if (sz != len) {
+		pr_err("tftp: not enough room in kfifo (only %u out of %zu written)\n",
+		       sz, len);
+		priv->err = -ENOMEM;
+		priv->state = STATE_DONE;
+	} else if (len < priv->blocksize) {
+		tftp_send(priv);
+		priv->err = 0;
+		priv->state = STATE_DONE;
+	}
+}
+
+static void tftp_apply_window_cache(struct file_priv *priv)
+{
+	struct tftp_cache *cache = &priv->cache;
+
+	while (tftp_window_cache_starts_with(cache, priv->last_block + 1)) {
+		struct tftp_block *block;
+
+		/* can be changed by tftp_put_data() below and must be
+		   checked in each loop */
+		if (priv->state != STATE_RDATA)
+			return;
+
+		block = tftp_window_cache_pop(cache);
+
+		debug_assert(block);
+		debug_assert(block->id == (uint16_t)(priv->last_block + 1));
+
+		tftp_put_data(priv, block->id, block->data, block->len);
+	}
+}
+
+static void tftp_handle_data(struct file_priv *priv, uint16_t block,
+			     void const *data, size_t len)
+{
+	uint16_t exp_block;
+	int rc;
+
+	exp_block = priv->last_block + 1;
+
+	if (exp_block == block) {
+		/* datagram over network is the expected one; put it in the
+		   fifo directly and try to apply cached items then */
+		tftp_timer_reset(priv);
+		tftp_put_data(priv, block, data, len);
+		tftp_window_cache_remove_id(&priv->cache, block);
+		tftp_apply_window_cache(priv);
+	} else if (!in_window(block, exp_block, priv->ack_block)) {
+		/* completely unexpected and unrelated to actual window;
+		   ignore the packet. */
+		printf("B");
+	} else {
+		/* The 'rc < 0' below happens e.g. when datagrams in the first
+		   part of the transfer window are dropped.
+
+		   TODO: this will usually result in a timeout
+		   (TFTP_RESEND_TIMEOUT).  It should be possible to bypass
+		   this timeout by acknowledging the last packet (e.g. by
+		   doing 'priv->ack_block = priv->last_block' here). */
+		rc = tftp_window_cache_insert(&priv->cache, block, data, len);
+		if (rc < 0)
+			printf("M");
+	}
+}
+
 static void tftp_recv(struct file_priv *priv,
 			uint8_t *pkt, unsigned len, uint16_t uh_sport)
 {
 	uint16_t opcode;
+	uint16_t block;
+	int rc;
 
 	/* according to RFC1350 minimal tftp packet length is 4 bytes */
 	if (len < 4)
@@ -270,75 +684,80 @@ static void tftp_recv(struct file_priv *priv,
 		if (!priv->push)
 			break;
 
-		priv->block = ntohs(*(uint16_t *)pkt);
-		if (priv->block != priv->last_block) {
-			pr_vdebug("ack %d != %d\n", priv->block, priv->last_block);
+		block = ntohs(*(uint16_t *)pkt);
+		if (block != priv->last_block) {
+			pr_vdebug("ack %d != %d\n", block, priv->last_block);
 			break;
 		}
 
-		priv->block++;
+		switch (priv->state) {
+		case STATE_WRQ:
+			priv->tftp_con->udp->uh_dport = uh_sport;
+			priv->state = STATE_START;
+			break;
 
-		tftp_timer_reset(priv);
+		case STATE_WAITACK:
+			priv->state = STATE_WDATA;
+			break;
 
-		if (priv->state == STATE_LAST) {
+		case STATE_LAST:
 			priv->state = STATE_DONE;
 			break;
+
+		default:
+			pr_warn("ACK packet in %s state\n",
+				tftp_states[priv->state]);
+			goto ack_out;
 		}
-		priv->tftp_con->udp->uh_dport = uh_sport;
-		priv->state = STATE_WDATA;
+
+		priv->block = block + 1;
+		tftp_timer_reset(priv);
+
+	ack_out:
 		break;
 
 	case TFTP_OACK:
-		tftp_parse_oack(priv, pkt, len);
+		if (priv->state != STATE_RRQ && priv->state != STATE_WRQ) {
+			pr_warn("OACK packet in %s state\n",
+				tftp_states[priv->state]);
+			break;
+		}
+
 		priv->tftp_con->udp->uh_dport = uh_sport;
 
-		if (priv->push) {
-			/* send first block */
-			priv->state = STATE_WDATA;
-			priv->block = 1;
-		} else {
-			/* send ACK */
-			priv->state = STATE_OACK;
-			priv->block = 0;
-			tftp_send(priv);
+		if (tftp_parse_oack(priv, pkt, len) < 0) {
+			priv->err = -EINVAL;
+			priv->state = STATE_DONE;
+			break;
 		}
 
+		priv->state = STATE_START;
 		break;
+
 	case TFTP_DATA:
 		len -= 2;
-		priv->block = ntohs(*(uint16_t *)pkt);
+		block = ntohs(*(uint16_t *)pkt);
 
-		if (priv->state == STATE_RRQ || priv->state == STATE_OACK) {
-			/* first block received */
-			priv->state = STATE_RDATA;
+		if (priv->state == STATE_RRQ) {
+			/* first block received; entered only with non rfc
+			   2347 (TFTP Option extension) compliant servers */
 			priv->tftp_con->udp->uh_dport = uh_sport;
+			priv->state = STATE_RDATA;
 			priv->last_block = 0;
+			priv->ack_block = priv->windowsize;
 
-			if (priv->block != 1) {	/* Assertion */
-				pr_err("error: First block is not block 1 (%d)\n",
-					priv->block);
-				priv->err = -EINVAL;
-				priv->state = STATE_DONE;
+			rc = tftp_allocate_transfer(priv);
+			if (rc < 0)
 				break;
-			}
 		}
 
-		if (priv->block == priv->last_block)
-			/* Same block again; ignore it. */
+		if (priv->state != STATE_RDATA) {
+			pr_warn("DATA packet in %s state\n",
+				tftp_states[priv->state]);
 			break;
-
-		priv->last_block = priv->block;
-
-		tftp_timer_reset(priv);
-
-		kfifo_put(priv->fifo, pkt + 2, len);
-
-		if (len < priv->blocksize) {
-			tftp_send(priv);
-			priv->err = 0;
-			priv->state = STATE_DONE;
 		}
 
+		tftp_handle_data(priv, block, pkt + 2, len);
 		break;
 
 	case TFTP_ERROR:
@@ -369,13 +788,38 @@ static void tftp_handler(void *ctx, char *packet, unsigned len)
 	tftp_recv(priv, pkt, net_eth_to_udplen(packet), udp->uh_sport);
 }
 
+static int tftp_start_transfer(struct file_priv *priv)
+{
+	int rc;
+
+	rc = tftp_allocate_transfer(priv);
+	if (rc < 0)
+		/* function sets 'priv->state = STATE_DONE' and 'priv->err' in
+		   error case */
+		return rc;
+
+	if (priv->push) {
+		/* send first block */
+		priv->state = STATE_WDATA;
+		priv->block = 1;
+	} else {
+		/* send ACK */
+		priv->state = STATE_RDATA;
+		priv->last_block = 0;
+		tftp_send(priv);
+	}
+
+	return 0;
+}
+
 static struct file_priv *tftp_do_open(struct device_d *dev,
-		int accmode, struct dentry *dentry)
+		int accmode, struct dentry *dentry, bool is_getattr)
 {
 	struct fs_device_d *fsdev = dev_to_fs_device(dev);
 	struct file_priv *priv;
 	struct tftp_priv *tpriv = dev->priv;
 	int ret;
+	unsigned short port = TFTP_PORT;
 
 	priv = xzalloc(sizeof(*priv));
 
@@ -397,49 +841,77 @@ static struct file_priv *tftp_do_open(struct device_d *dev,
 	priv->err = -EINVAL;
 	priv->filename = dpath(dentry, fsdev->vfsmount.mnt_root);
 	priv->blocksize = TFTP_BLOCK_SIZE;
-	priv->block_requested = -1;
+	priv->windowsize = 1;
+	priv->is_getattr = is_getattr;
 
-	priv->fifo = kfifo_alloc(TFTP_FIFO_SIZE);
-	if (!priv->fifo) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	parseopt_hu(fsdev->options, "port", &port);
 
-	priv->tftp_con = net_udp_new(tpriv->server, TFTP_PORT, tftp_handler,
-			priv);
+	priv->tftp_con = net_udp_new(tpriv->server, port, tftp_handler, priv);
 	if (IS_ERR(priv->tftp_con)) {
 		ret = PTR_ERR(priv->tftp_con);
-		goto out1;
+		goto out;
 	}
 
 	ret = tftp_send(priv);
 	if (ret)
-		goto out2;
+		goto out1;
 
 	tftp_timer_reset(priv);
-	while (priv->state != STATE_RDATA &&
-			priv->state != STATE_DONE &&
-			priv->state != STATE_WDATA) {
-		ret = tftp_poll(priv);
-		if (ret == TFTP_ERR_RESEND)
-			tftp_send(priv);
-		if (ret < 0)
-			goto out2;
-	}
 
-	if (priv->state == STATE_DONE && priv->err) {
-		ret = priv->err;
-		goto out2;
-	}
+	/* - 'ret < 0'  ... error
+	   - 'ret == 0' ... further tftp_poll() required
+	   - 'ret == 1' ... startup finished */
+	do {
+		switch (priv->state) {
+		case STATE_DONE:
+			/* branch is entered in two situations:
+			   - non rfc 2347 compliant servers finished the
+			     transfer by sending a small file
+			   - some error occurred */
+			if (priv->err < 0)
+				ret = priv->err;
+			else
+				ret = 1;
+			break;
+
+		case STATE_START:
+			ret = tftp_start_transfer(priv);
+			if (!ret)
+				ret = 1;
+			break;
+
+		case STATE_RDATA:
+			/* first data block of non rfc 2347 servers */
+			ret = 1;
+			break;
+
+		case STATE_RRQ:
+		case STATE_WRQ:
+			ret = tftp_poll(priv);
+			if (ret == TFTP_ERR_RESEND) {
+				tftp_send(priv);
+				ret = 0;
+			}
+			break;
+
+		default:
+			debug_assert(false);
+			break;
+		}
+	} while (ret == 0);
 
-	priv->buf = xmalloc(priv->blocksize);
+	if (ret < 0)
+		goto out1;
 
 	return priv;
-out2:
-	net_unregister(priv->tftp_con);
 out1:
-	kfifo_free(priv->fifo);
+	net_unregister(priv->tftp_con);
 out:
+	if (priv->fifo)
+		kfifo_free(priv->fifo);
+
+	free(priv->filename);
+	free(priv->buf);
 	free(priv);
 
 	return ERR_PTR(ret);
@@ -449,7 +921,7 @@ static int tftp_open(struct device_d *dev, FILE *file, const char *filename)
 {
 	struct file_priv *priv;
 
-	priv = tftp_do_open(dev, file->flags, file->dentry);
+	priv = tftp_do_open(dev, file->flags, file->dentry, false);
 	if (IS_ERR(priv))
 		return PTR_ERR(priv);
 
@@ -489,6 +961,7 @@ static int tftp_do_close(struct file_priv *priv)
 	}
 
 	net_unregister(priv->tftp_con);
+	tftp_window_cache_free(&priv->cache);
 	kfifo_free(priv->fifo);
 	free(priv->filename);
 	free(priv->buf);
@@ -544,7 +1017,7 @@ static int tftp_read(struct device_d *dev, FILE *f, void *buf, size_t insize)
 {
 	struct file_priv *priv = f->priv;
 	size_t outsize = 0, now;
-	int ret;
+	int ret = 0;
 
 	pr_vdebug("%s %zu\n", __func__, insize);
 
@@ -553,19 +1026,30 @@ static int tftp_read(struct device_d *dev, FILE *f, void *buf, size_t insize)
 		outsize += now;
 		buf += now;
 		insize -= now;
-		if (priv->state == STATE_DONE)
-			return outsize;
 
-		if (TFTP_FIFO_SIZE - kfifo_len(priv->fifo) >= priv->blocksize)
+		if (priv->state == STATE_DONE) {
+			ret = priv->err;
+			break;
+		}
+
+		/* send the ACK only when fifo has been nearly depleted; else,
+		   when tftp_read() is called with small 'insize' values, it
+		   is possible that there is read more data from the network
+		   than consumed by kfifo_get() and the fifo overflows */
+		if (priv->last_block == priv->ack_block &&
+		    kfifo_len(priv->fifo) <= TFTP_EXTRA_BLOCKS * priv->blocksize)
 			tftp_send(priv);
 
 		ret = tftp_poll(priv);
 		if (ret == TFTP_ERR_RESEND)
 			tftp_send(priv);
 		if (ret < 0)
-			return ret;
+			break;
 	}
 
+	if (ret < 0)
+		return ret;
+
 	return outsize;
 }
 
@@ -665,7 +1149,7 @@ static struct dentry *tftp_lookup(struct inode *dir, struct dentry *dentry,
 	struct file_priv *priv;
 	loff_t filesize;
 
-	priv = tftp_do_open(&fsdev->dev, O_RDONLY, dentry);
+	priv = tftp_do_open(&fsdev->dev, O_RDONLY, dentry, true);
 	if (IS_ERR(priv))
 		return NULL;
 
@@ -748,6 +1232,110 @@ static struct fs_driver_d tftp_driver = {
 
 static int tftp_init(void)
 {
+	globalvar_add_simple_int("tftp.windowsize", &g_tftp_window_size, "%u");
+
 	return register_fs_driver(&tftp_driver);
 }
 coredevice_initcall(tftp_init);
+
+
+BSELFTEST_GLOBALS();
+
+static int __maybe_unused tftp_window_cache_selftest(void)
+{
+	struct tftp_cache	*cache = malloc(sizeof *cache);
+
+	if (!cache)
+		return -ENOMEM;
+
+	(void)skipped_tests;
+
+	expect_it( is_block_before(0, 1));
+	expect_it(!is_block_before(1, 0));
+	expect_it( is_block_before(65535, 0));
+	expect_it(!is_block_before(0, 65535));
+
+	expect_eq(get_block_delta(0, 1),     1);
+	expect_eq(get_block_delta(65535, 0), 1);
+	expect_eq(get_block_delta(65535, 1), 2);
+
+	expect_it(!in_window(0, 1, 3));
+	expect_it( in_window(1, 1, 3));
+	expect_it( in_window(2, 1, 3));
+	expect_it( in_window(3, 1, 3));
+	expect_it(!in_window(4, 1, 3));
+
+	expect_it(!in_window(65534, 65535, 1));
+	expect_it( in_window(65535, 65535, 1));
+	expect_it( in_window(    0, 65535, 1));
+	expect_it( in_window(    1, 65535, 1));
+	expect_it(!in_window(    2, 65535, 1));
+
+
+	tftp_window_cache_init(cache, 512, 5);
+
+	if (tftp_window_cache_size(cache) < 4)
+		goto out;
+
+	expect_eq(tftp_window_cache_size(cache), 4);
+
+	/* sequence 1 */
+	expect_ok (tftp_window_cache_insert(cache, 20, "20", 2));
+	expect_ok (tftp_window_cache_insert(cache, 22, "22", 2));
+	expect_ok (tftp_window_cache_insert(cache, 21, "21", 2));
+	expect_ok (tftp_window_cache_insert(cache, 23, "23", 2));
+	expect_err(tftp_window_cache_insert(cache, 24, "24", 2));
+	expect_err(tftp_window_cache_insert(cache, 19, "19", 2));
+	expect_ok (tftp_window_cache_insert(cache, 22, "22", 2));
+	expect_ok (tftp_window_cache_insert(cache, 20, "20", 2));
+
+	expect_eq(tftp_window_cache_pop(cache)->id, 20);
+	expect_eq(tftp_window_cache_pop(cache)->id, 21);
+	expect_eq(tftp_window_cache_pop(cache)->id, 22);
+	expect_eq(tftp_window_cache_pop(cache)->id, 23);
+	expect_eq(cache->id, TFTP_CACHE_NO_ID);
+
+	/* sequence 2 */
+	expect_ok (tftp_window_cache_insert(cache, 30, "30", 2));
+	expect_ok (tftp_window_cache_insert(cache, 32, "32", 2));
+	expect_err(tftp_window_cache_insert(cache, 34, "34", 2));
+
+	expect_it(tftp_window_cache_starts_with(cache, 30));
+	expect_eq(tftp_window_cache_pop(cache)->id, 30);
+
+	expect_ok (tftp_window_cache_insert(cache, 34, "34", 2));
+	expect_err(tftp_window_cache_insert(cache, 35, "35", 2));
+
+	expect_it(!tftp_window_cache_starts_with(cache, 30));
+	expect_it(!tftp_window_cache_starts_with(cache, 31));
+	expect_it(!tftp_window_cache_starts_with(cache, 32));
+	expect_NULL(tftp_window_cache_pop(cache));
+
+	expect_it(tftp_window_cache_starts_with(cache, 32));
+	expect_eq(tftp_window_cache_pop(cache)->id, 32);
+
+	expect_NULL(tftp_window_cache_pop(cache));
+	expect_eq(tftp_window_cache_pop(cache)->id, 34);
+
+	expect_eq(cache->id, TFTP_CACHE_NO_ID);
+
+	/* sequence 3 */
+	expect_ok(tftp_window_cache_insert(cache, 40, "40", 2));
+	expect_ok(tftp_window_cache_insert(cache, 42, "42", 2));
+	expect_ok(tftp_window_cache_insert(cache, 43, "43", 2));
+
+	expect_it(!tftp_window_cache_remove_id(cache, 30));
+	expect_it(!tftp_window_cache_remove_id(cache, 41));
+	expect_it(!tftp_window_cache_remove_id(cache, 44));
+
+	expect_it( tftp_window_cache_remove_id(cache, 42));
+	expect_it(!tftp_window_cache_remove_id(cache, 42));
+
+out:
+	tftp_window_cache_free(cache);
+
+	return 0;
+}
+#ifdef CONFIG_SELFTEST_TFTP
+bselftest(core, tftp_window_cache_selftest);
+#endif
diff --git a/test/self/Kconfig b/test/self/Kconfig
index 680196a4fe..03cfa89987 100644
--- a/test/self/Kconfig
+++ b/test/self/Kconfig
@@ -32,6 +32,7 @@ config SELFTEST_ENABLE_ALL
 	select SELFTEST_PROGRESS_NOTIFIER
 	select SELFTEST_OF_MANIPULATION
 	select SELFTEST_ENVIRONMENT_VARIABLES if ENVIRONMENT_VARIABLES
+	imply SELFTEST_TFTP
 	help
 	  Selects all self-tests compatible with current configuration
 
@@ -57,4 +58,10 @@ config SELFTEST_PROGRESS_NOTIFIER
 config SELFTEST_ENVIRONMENT_VARIABLES
 	bool "environment variable selftest"
 
+config SELFTEST_TFTP
+	bool "tftp selftest"
+	depends on FS_TFTP
+	help
+	  Tests tftp functionality
+
 endif