diff --git a/tftp.c b/tftp.c
index 999b5d7..530b3d1 100644
--- a/tftp.c
+++ b/tftp.c
@@ -3,7 +3,8 @@
 /*                                                                           */
 /* A simple tftp client for busybox.                                         */
 /* Tries to follow RFC1350.                                                  */
-/* Only "octet" mode and 512-byte data blocks are supported.                 */
+/* Only "octet" mode supported.                                              */
+/* Optional blocksize negotiation (RFC2347 + RFC2348)                        */
 /*                                                                           */
 /* Copyright (C) 2001 Magnus Damm <damm@opensource.se>                       */
 /*                                                                           */
@@ -47,6 +48,18 @@
 
 //#define BB_FEATURE_TFTP_DEBUG
 
+#define TFTP_BLOCKSIZE_DEFAULT 512 /* according to RFC 1350, don't change */
+#define TFTP_TIMEOUT 5             /* seconds */
+
+/* opcodes we support */
+
+#define TFTP_RRQ   1
+#define TFTP_WRQ   2
+#define TFTP_DATA  3
+#define TFTP_ACK   4
+#define TFTP_ERROR 5
+#define TFTP_OACK  6
+
 static const char *tftp_error_msg[] = {
 	"Undefined error",
 	"File not found",
@@ -61,8 +74,71 @@
 const int tftp_cmd_get = 1;
 const int tftp_cmd_put = 2;
 
+#ifdef BB_FEATURE_TFTP_BLOCKSIZE
+
+static int tftp_blocksize_check(int blocksize, int bufsize)  
+{
+        /* Check if the blocksize is valid: 
+	 * RFC2348 says between 8 and 65464,
+	 * but our implementation makes it impossible
+	 * to use blocksizes smaller than 22 octets.
+	 */
+
+        if ((bufsize && (blocksize > bufsize)) || 
+	    (blocksize < 8) || (blocksize > 65464)) {
+	        error_msg("bad blocksize");
+	        return 0;
+	}
+
+	return blocksize;
+}
+
+static char *tftp_option_get(char *buf, int len, char *option)  
+{
+        int opt_val = 0;
+	int opt_found = 0;
+	int k;
+  
+	while (len > 0) {
+
+	        /* Make sure the options are terminated correctly */
+
+	        for (k = 0; k < len; k++) {
+		        if (buf[k] == '\0') {
+			        break;
+			}
+		}
+
+		if (k >= len) {
+		        break;
+		}
+
+		if (opt_val == 0) {
+			if (strcasecmp(buf, option) == 0) {
+			        opt_found = 1;
+			}
+		}      
+		else {
+		        if (opt_found) {
+				return buf;
+			}
+		}
+    
+		k++;
+		
+		buf += k;
+		len -= k;
+		
+		opt_val ^= 1;
+	}
+	
+	return NULL;
+}
+
+#endif
+
 static inline int tftp(const int cmd, const struct hostent *host,
-	const char *serverfile, int localfd, const int port, int tftp_bufsize)
+	const char *remotefile, int localfd, const int port, int tftp_bufsize)
 {
 	const int cmd_get = cmd & tftp_cmd_get;
 	const int cmd_put = cmd & tftp_cmd_put;
@@ -81,7 +157,12 @@
 	int finished = 0;
 	int timeout = bb_tftp_num_retries;
 	int block_nr = 1;
-	RESERVE_BB_BUFFER(buf, tftp_bufsize + 4); // Why 4 ?
+
+#ifdef BB_FEATURE_TFTP_BLOCKSIZE
+	int want_option_ack = 0;
+#endif
+
+	RESERVE_BB_BUFFER(buf, tftp_bufsize + 4); /* Opcode + Block # + Data */
 
 	tftp_bufsize += 4;
 
@@ -103,50 +184,79 @@
 	/* build opcode */
 
 	if (cmd_get) {
-		opcode = 1;	// read request = 1
+		opcode = TFTP_RRQ;
 	}
 
 	if (cmd_put) {
-		opcode = 2;	// write request = 2
+		opcode = TFTP_WRQ;
 	}
 
 	while (1) {
 
-
-		/* build packet of type "opcode" */
-
-
 		cp = buf;
 
+		/* first create the opcode part */
+
 		*((unsigned short *) cp) = htons(opcode);
 
 		cp += 2;
 
 		/* add filename and mode */
 
-		if ((cmd_get && (opcode == 1)) || // read request = 1
-			(cmd_put && (opcode == 2))) { // write request = 2
+		if ((cmd_get && (opcode == TFTP_RRQ)) ||
+			(cmd_put && (opcode == TFTP_WRQ))) {
+                        int too_long = 0; 
 
-			/* what is this trying to do ? */
-			while (cp != &buf[tftp_bufsize - 1]) {
-				if ((*cp = *serverfile++) == '\0')
-					break;
-				cp++;
+			/* see if the filename fits into buf */
+			/* and fill in packet                */
+
+			len = strlen(remotefile) + 1;
+
+			if ((cp + len) >= &buf[tftp_bufsize - 1]) {
+			        too_long = 1;
 			}
-			/* and this ? */
-			if ((*cp != '\0') || (&buf[tftp_bufsize - 1] - cp) < 7) {
-				error_msg("too long server-filename");
+			else {
+			        safe_strncpy(cp, remotefile, len);
+				cp += len;
+			}
+
+			if (too_long || ((&buf[tftp_bufsize - 1] - cp) < 6)) {
+				error_msg("too long remote-filename");
 				break;
 			}
 
-			memcpy(cp + 1, "octet", 6);
-			cp += 7;
+			/* add "mode" part of the package */
+
+			memcpy(cp, "octet", 6);
+			cp += 6;
+
+#ifdef BB_FEATURE_TFTP_BLOCKSIZE
+
+			len = tftp_bufsize - 4; /* data block size */
+
+			if (len != TFTP_BLOCKSIZE_DEFAULT) {
+
+			        if ((&buf[tftp_bufsize - 1] - cp) < 15) {
+				        error_msg("too long remote-filename");
+					break;
+				}
+
+				/* add "blksize" + number of blocks  */
+
+				memcpy(cp, "blksize", 8);
+				cp += 8;
+
+				cp += snprintf(cp, 6, "%d", len) + 1;
+
+				want_option_ack = 1;
+			}
+#endif
 		}
 
 		/* add ack and data */
 
-		if ((cmd_get && (opcode == 4)) || // acknowledgement = 4
-			(cmd_put && (opcode == 3))) { // data packet == 3
+		if ((cmd_get && (opcode == TFTP_ACK)) ||
+			(cmd_put && (opcode == TFTP_DATA))) {
 
 			*((unsigned short *) cp) = htons(block_nr);
 
@@ -154,7 +264,7 @@
 
 			block_nr++;
 
-			if (cmd_put && (opcode == 3)) { // data packet == 3
+			if (cmd_put && (opcode == TFTP_DATA)) {
 				len = read(localfd, cp, tftp_bufsize - 4);
 
 				if (len < 0) {
@@ -200,7 +310,7 @@
 			memset(&from, 0, sizeof(from));
 			fromlen = sizeof(from);
 
-			tv.tv_sec = 5; // BB_TFPT_TIMEOUT = 5
+			tv.tv_sec = TFTP_TIMEOUT;
 			tv.tv_usec = 0;
 
 			FD_ZERO(&rfds);
@@ -261,9 +371,76 @@
 		printf("received %d bytes: %04x %04x\n", len, opcode, tmp);
 #endif
 
-		if (cmd_get && (opcode == 3)) { // data packet == 3
+		if (opcode == TFTP_ERROR) {
+			char *msg = NULL;
+
+			if (buf[4] != '\0') {
+				msg = &buf[4];
+				buf[tftp_bufsize - 1] = '\0';
+			} else if (tmp < (sizeof(tftp_error_msg) 
+					  / sizeof(char *))) {
+
+				msg = (char *) tftp_error_msg[tmp];
+			}
+
+			if (msg) {
+				error_msg("server says: %s", msg);
+			}
+
+			break;
+		}
+
+#ifdef BB_FEATURE_TFTP_BLOCKSIZE
+		if (want_option_ack) {
+
+			 want_option_ack = 0;
+
+		         if (opcode == TFTP_OACK) {
+
+			         /* server seems to support options */
+
+			         char *res;
+
+				 res = tftp_option_get(&buf[2], len-2, 
+						       "blksize");
+
+				 if (res) {
+				         int foo = atoi(res);
+			     
+					 if (tftp_blocksize_check(foo,
+							   tftp_bufsize - 4)) {
+
+					         if (cmd_put) {
+				                         opcode = TFTP_DATA;
+						 }
+						 else {
+				                         opcode = TFTP_ACK;
+						 }
+#ifdef BB_FEATURE_TFTP_DEBUG
+						 printf("using blksize %u\n");
+#endif
+					         tftp_bufsize = foo + 4;
+						 block_nr = 0;
+						 continue;
+					 }
+				 }
+				 /* FIXME:
+				  * we should send ERROR 8 */
+				 error_msg("bad server option");
+				 break;
+			 }
+
+			 error_msg("warning: blksize not supported by server"
+				   " - reverting to 512");
+
+			 tftp_bufsize = TFTP_BLOCKSIZE_DEFAULT + 4;
+		}
+#endif
+
+		if (cmd_get && (opcode == TFTP_DATA)) {
 
 			if (tmp == block_nr) {
+			    
 				len = write(localfd, &buf[4], len - 4);
 
 				if (len < 0) {
@@ -275,43 +452,30 @@
 					finished++;
 				}
 
-				opcode = 4; // acknowledgement = 4
+				opcode = TFTP_ACK;
 				continue;
 			}
 		}
 
-		if (cmd_put && (opcode == 4)) { // acknowledgement = 4
+		if (cmd_put && (opcode == TFTP_ACK)) {
 
 			if (tmp == (block_nr - 1)) {
 				if (finished) {
 					break;
 				}
 
-				opcode = 3; // data packet == 3
+				opcode = TFTP_DATA;
 				continue;
 			}
 		}
-
-		if (opcode == 5) { // error code == 5
-			char *msg = NULL;
-
-			if (buf[4] != '\0') {
-				msg = &buf[4];
-				buf[tftp_bufsize - 1] = '\0';
-			} else if (tmp < (sizeof(tftp_error_msg) / sizeof(char *))) {
-				msg = (char *) tftp_error_msg[tmp];
-			}
-
-			if (msg) {
-				error_msg("server says: %s", msg);
-			}
-
-			break;
-		}
 	}
 
+#ifdef BB_FEATURE_CLEAN_UP
 	close(socketfd);
 
+        RELEASE_BB_BUFFER(buf);
+#endif
+
 	return finished ? EXIT_SUCCESS : EXIT_FAILURE;
 }
 
@@ -326,13 +490,38 @@
 	int flags = 0;
 	int opt;
 	int result;
-	int blocksize = 512;
+	int blocksize = TFTP_BLOCKSIZE_DEFAULT;
 
-	while ((opt = getopt(argc, argv, "b:gpl:r:")) != -1) {
+	/* figure out what to pass to getopt */
+
+#ifdef BB_FEATURE_TFTP_BLOCKSIZE
+#define BS "b:"
+#else
+#define BS
+#endif
+
+#ifdef BB_FEATURE_TFTP_GET
+#define GET "g"
+#else
+#define GET 
+#endif
+
+#ifdef BB_FEATURE_TFTP_PUT
+#define PUT "p"
+#else
+#define PUT 
+#endif
+
+	while ((opt = getopt(argc, argv, BS GET PUT "l:r:")) != -1) {
 		switch (opt) {
+#ifdef BB_FEATURE_TFTP_BLOCKSIZE
 		case 'b':
 			blocksize = atoi(optarg);
+			if (!tftp_blocksize_check(blocksize, 0)) {
+                                return EXIT_FAILURE;
+			}
 			break;
+#endif
 #ifdef BB_FEATURE_TFTP_GET
 		case 'g':
 			cmd = tftp_cmd_get;
@@ -370,14 +559,16 @@
 	}
 
 #ifdef BB_FEATURE_TFTP_DEBUG
-	printf("using server \"%s\", serverfile \"%s\","
+	printf("using server \"%s\", remotefile \"%s\", "
 		"localfile \"%s\".\n",
 		inet_ntoa(*((struct in_addr *) host->h_addr)),
 		remotefile, localfile);
 #endif
 
 	result = tftp(cmd, host, remotefile, fd, port, blocksize);
-	close(fd);
 
+#ifdef BB_FEATURE_CLEAN_UP
+	close(fd);
+#endif
 	return(result);
-}
\ No newline at end of file
+}