diff --git a/src/apps/tftp/tftp_server.c b/src/apps/tftp/tftp_server.c
index 07f6170e..57018bd5 100644
--- a/src/apps/tftp/tftp_server.c
+++ b/src/apps/tftp/tftp_server.c
@@ -294,18 +294,27 @@ recv(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16
       }
 
       blknum = lwip_ntohs(sbuf[1]);
-      pbuf_remove_header(p, TFTP_HEADER_LENGTH);
+      if (blknum == tftp_state.blknum) {
+        pbuf_remove_header(p, TFTP_HEADER_LENGTH);
 
-      ret = tftp_state.ctx->write(tftp_state.handle, p);
-      if (ret < 0) {
-        send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "error writing file");
-        close_handle();
-      } else {
+        ret = tftp_state.ctx->write(tftp_state.handle, p);
+        if (ret < 0) {
+          send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "error writing file");
+          close_handle();
+        } else {
+          send_ack(blknum);
+        }
+
+        if (p->tot_len < TFTP_MAX_PAYLOAD_SIZE) {
+          close_handle();
+        } else {
+          tftp_state.blknum++;
+        }
+      } else if ((u16_t)(blknum + 1) == tftp_state.blknum) {
+        /* retransmit of previous block, ack again (casting to u16_t to care for overflow) */
         send_ack(blknum);
-      }
-
-      if (p->tot_len < TFTP_MAX_PAYLOAD_SIZE) {
-        close_handle();
+      } else {
+        send_error(addr, port, TFTP_ERROR_UNKNOWN_TRFR_ID, "Wrong block number");
       }
       break;
     }