diff --git a/tests/tcp_cmd.cpp b/tests/tcp_cmd.cpp
index a6ce4f4..1d3e108 100644
--- a/tests/tcp_cmd.cpp
+++ b/tests/tcp_cmd.cpp
@@ -9,7 +9,7 @@ extern "C" {
 #include "tcp_cmd.h"
 }
 
-#define MAX_TOKENS 10
+#define MAX_TOKENS  10
 #define MAX_CMD_LEN 50
 
 static uint32_t color_txt = 0;
@@ -117,7 +117,6 @@ static void tcp_cmd_parser(struct tcp_pcb* pcb, int argc, char** argv) {
         tcp_cmd_write(pcb, "Usage: bgcolor <background color>\n");
         tcp_cmd_write(pcb, "Usage: bgcolor r g b\n");
         return;
-
     } if (strcmp(argv[0], "color") == 0) {
         if (argc == 2) {
             color_txt = (uint32_t)strtol(argv[1], NULL, 16);
@@ -148,8 +147,7 @@ static void tcp_cmd_parser(struct tcp_pcb* pcb, int argc, char** argv) {
                 tcp_cmd_write(pcb, "File is not a bmp\n");
                 return;
             }
-        }
-        if (argc == 2) {
+        } if (argc == 2) {
             lcd_clear_images();
             lcd_draw_img_from_fs(argv[1], 0, 0);
             return;
@@ -161,15 +159,14 @@ static void tcp_cmd_parser(struct tcp_pcb* pcb, int argc, char** argv) {
         tcp_cmd_write(pcb, "Usage: setimage <filename>\n");
         tcp_cmd_write(pcb, "Usage: setimage <filename> <x> <y>\n");
         return;
-    } else if (strcmp(argv[0], "setgif") == 0) {
+    } if (strcmp(argv[0], "setgif") == 0) {
         if (argc >= 2) {
             char* ext = get_filename_ext(argv[1]);
             if (strcmp(ext, "gif") != 0) {
                 tcp_cmd_write(pcb, "File is not a gif\n");
                 return;
             }
-        }
-        if (argc == 2) {
+        } if (argc == 2) {
             lcd_clear_images();
             lcd_draw_gif_from_fs(argv[1], 0, 0);
             return;
@@ -200,41 +197,51 @@ static err_t tcp_cmd_recv_new(void* arg, struct tcp_pcb* pcb, struct pbuf* p, er
     char* argv[MAX_TOKENS];
 
     LWIP_UNUSED_ARG(arg);
-    if (err == ERR_OK && p != NULL) {
-        tcp_recved(pcb, p->tot_len);
-        char* next;
+    LOG_DEBUG(TAG, "TCP data received");
 
-        // Make sure the string is null terminated
-        if (p->len >= MAX_CMD_LEN) {
-            LOG_WARN(TAG, "Command too long");
-        }
-        uint16_t len = p->tot_len >= MAX_CMD_LEN ? MAX_CMD_LEN : p->tot_len;
+    // Connection closed?
+    if (p == NULL && err == ERR_OK) {
+        LOG_INFO(TAG, "Remote closed connection");
+        return tcp_close(pcb);
+    }
 
-        pbuf_copy_partial(p, cmd, len, 0);
-        cmd[len] = '\0';
+    if (err != ERR_OK) {
+        LOG_WARN(TAG, "TCP data received with error(%d): %s", err, lwip_strerr(err));
+        return ERR_OK;
+    }
+
+    char* next;
+
+    // Make sure the string is null terminated
+    if (p->len >= MAX_CMD_LEN) {
+        LOG_WARN(TAG, "Command too long");
+    }
+    uint16_t len = p->tot_len >= MAX_CMD_LEN ? MAX_CMD_LEN : p->tot_len;
+
+    pbuf_copy_partial(p, cmd, len, 0);
+    cmd[len] = '\0';
         
-        remove_newline(cmd);
-        // Split string into tokens by delimiter (space)
-        argv[0] = get_next_token(cmd, " ", &next);
-        argc = 1;
-        while (argv[argc - 1] != NULL && argc < MAX_TOKENS) {
-            argv[argc] = get_next_token(NULL, " ", &next);
-            if (argv[argc] == NULL) {
-                break;
-            }
-            argc++;
+    // Tell the tcp stack that we have taken the data
+    tcp_recved(pcb, p->tot_len);
+
+    remove_newline(cmd);
+    // Split string into tokens by delimiter (space)
+    argv[0] = get_next_token(cmd, " ", &next);
+    argc = 1;
+    while (argv[argc - 1] != NULL && argc < MAX_TOKENS) {
+        argv[argc] = get_next_token(NULL, " ", &next);
+        if (argv[argc] == NULL) {
+            break;
         }
-
-        tcp_cmd_parser(pcb, argc, argv);
-    } else {
-        pbuf_free(p);
+        argc++;
     }
 
-    if (err == ERR_OK && p == NULL) {
-        tcp_cmd_close(pcb);
-    }
+    tcp_cmd_parser(pcb, argc, argv);
 
     tcp_cmd_write(pcb, "$> ");
+
+defer:
+    pbuf_free(p);
     return ERR_OK;
 }