tcp_cmd

fix the tests

docs/tcp_cmd_interface.md

@@ -0,0 +1,96 @@
+# TCP CMD Interface
+
+## Introduction
+The tcp cmd interface is a way to control the device over TCP. It is a simple protocol that allows the user to send commands to the device and receive responses. The protocol looks like bash commands. The commands are separated by a new line character (`\n`). The response is sent after the command is executed. The response is also terminated by a new line character (`\n`). The response can be a simple string or a JSON object.
+
+## Table of contents
+
+* [Introduction](#introduction)
+* [Table of contents](#table-of-contents)
+* [Usage of TCP CMD Interface](#usage-of-tcp-cmd-interface)
+  * [Connecting to the device](#connecting-to-the-device)
+  * [Sending commands](#sending-commands)
+  * [Available commands](#available-commands)
+    * [`help`](#help)
+    * [`text`](#text)
+    * [`bgcolor`](#bgcolor)
+    * [`color`](#color)
+    * [`listImages`](#listimages)
+    * [`setImage`](#setimage)
+    * [`setGif`](#setgif)
+
+## Usage of TCP CMD Interface
+### Connecting to the device
+The device can be connected to using a TCP client. The device will listen on port 23. The device will send a welcome message when a client connects.
+
+On Linux, the device can be connected to using the `nc` command:
+```bash
+nc <ip-address> 23
+```
+
+### Sending commands
+Commands can be sent to the device by typing them in the terminal and pressing enter. The device will execute the command and send a response.
+
+### Available commands
+#### `help`
+The `help` command will print a list of available commands.
+```bash 
+$> help
+help                    : shows a list of commands
+text "<text>"           : puts text on the lcd
+color <r> <g> <b>       : set the background color of the lcd
+textColor <r> <g> <b>   : set the color of the text
+listImages              : shows a list with images in the filesystem
+setImage  <image_name>  : put an image on the screen
+setGif    <image_name>  : put a gif on the screen
+exit                    : closes the connection
+```
+
+#### `text`
+The `text` command will print the given text on the screen. The text can be one or multiple words. one word does'n need quotes, multiple words do. The text will rapp around when it reaches the end of the screen.
+```bash
+$> text "Hello World!"
+```
+
+#### `bgcolor`
+The `bgcolor` command will set the background color of the screen. The color can be provided in 2 ways: as three numbers between 0 and 255 and one hex value. The first number is the red value, the second is the green value and the third is the blue value.
+```bash
+bgcolor 255 0 0
+```
+or
+```bash
+bgcolor 0xFF0000
+```
+
+#### `color`
+The `color` command will set the color of the text. The color can be provided in 2 ways: as three numbers between 0 and 255 and one hex value. The first number is the red value, the second is the green value and the third is the blue value.
+```bash
+color 255 0 0
+```
+or
+```bash
+color 0xFF0000
+```
+
+#### `listImages`
+The `listImages` command will print a list of images that are present in the filesystem.
+```bash
+$> listImages
+  image1.bmp
+  image2.bmp
+  image3.bmp
+  gif1.gif
+  ...
+```
+
+#### `setImage`
+The `setImage` command will put an image on the screen. The image must be present in the filesystem. If the image is not present in the filesystem, the device will send an error message. This function only works for BMP images, if you give any other file type, the device will send an error message.
+```bash
+$> setImage image1.bmp
+```
+
+#### `setGif`
+The `setGif` command will put a gif on the screen. The gif must be present in the filesystem. If the gif is not present in the filesystem, the device will send an error message. This function only works for GIF images, if you give any other file type, the device will send an error message.
+```bash
+$> setGif gif1.gif
+```

project/Core/Inc/modbus_tcp.h

@@ -1,20 +1,27 @@
 /**
  * @file modbus_tcp.h
- * @brief TCP Modbus server
- * @date Nov 29, 2023
+ *
+ * @brief TCP Modbus handler
+ * @date Nov 6, 2023
  * @author Obe
- * @author Lorenz C.
  */
 
 #ifndef INC_MODBUS_H_
 #define INC_MODBUS_H_
 
-#define MODBUS_TCP_PORT 502
+#define MODBUSPORT 502 // 502 is the default
 
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <tcp.h>
+#include "lcd_api.h"
+#include "llfs.h"
 
 /**
- * @brief Initializes the modbus tcp server
+ * @fn void modbus_init
+ * @brief Initializes the modbus tcp
  */
-void modbus_tcp_init(void);
+void modbus_init(void);
 
 #endif /* INC_MODBUS_H_ */

project/Core/Inc/tcp_cmd.h

@@ -1,19 +1,30 @@
 /**
- * @file tcp_cmd.h
- * @brief TCP CMD interface
- * @author Gert R.
- */
+* @file tcp_cmd.h
+* @brief This file contains the headers of the tcp command interface
+* @author Sander S.
+*/
 
 #ifndef INC_TCP_CMD_H_
 #define INC_TCP_CMD_H_
-
-#include <stdio.h>
-#include <string.h>
-#include <tcp.h>
+#ifndef TESTING
 #include "lcd_api.h"
+#else
+#include "mocs.h"
+#endif
 #include "llfs.h"
 #include "log.h"
+#include <tcp.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <ctype.h>
 
+void tcp_cmd_remove_newline(char* str, size_t len);
+char* tcp_cmd_remove_leading_space(char* str, size_t len);
+void tcp_cmd_str_tolower(char* str);
+char* tcp_cmd_get_next_token(char* input, const char* delimiters, char** next);
 void tcp_cmd_init(void);
+err_t tcp_cmd_recv(void* arg, struct tcp_pcb* pcb, struct pbuf* p, err_t err);
 
 #endif /* INC_TCP_CMD_H_ */

project/Core/Src/main.c

@@ -120,10 +120,7 @@ int main(void) {
     MX_QUADSPI_Init();
     /* USER CODE BEGIN 2 */
 
-    /* Initialize QSPI */
-    BSP_QSPI_Init();
-    BSP_QSPI_MemoryMappedMode();
-    WRITE_REG(QUADSPI->LPTR, 0xFFF);
+  /* USER CODE END 2 */
 
     /* Clear terminal */
     printf(CLEAR_SCREEN);
@@ -131,8 +128,10 @@ int main(void) {
     /* Initialize the LCD */
     lcd_init(true);
 
-    /* Initialize the filesystem */
-    llfs_init();
+  /* Initialize the filesystem */
+  llfs_init();
+  tcp_cmd_init();
+  /* USER CODE END 2 */
 
     /* Initialize the tftp server */
     tftp_server_init();

project/Core/Src/modbus_tcp.c

@@ -1,594 +1,163 @@
 /**
  * @file modbus_tcp.c
- * @brief TCP Modbus server
- * @date Nov 29, 2023
+ *
+ * @brief TCP Modbus handler
+ * @date Nov 6, 2023
  * @author Obe
- * @author Lorenz C.
  */
 
-#include <stdint.h>
-#include <stdio.h>
-#include <string.h>
-#include <tcp.h>
-#include "lcd_api.h"
-#include "llfs.h"
-#define LOGGER_LEVEL_ALL
-#include "log.h"
+// Includes
 #include "modbus_tcp.h"
+#include "log.h"
 
-// TCP server constants
-#define TCP_POLL_INTERVAL 10 // About 5 seconds
+// Defines
+#define MAX_REG          REG_LENGTH
+#define EXTENSION_LENGTH 4
+#define TEXT_LENGTH      200
+#define MULTIPLE_REG     0x10
+#define REG_LENGTH       428
+#define START_DATA       28
+#define MODBUS_MODE      7
 
-// Modbus constants (See Modbus_Application_Protocol_V1_1b3 and Modbus_Messaging_Implementation_Guide_V1_0b)
-#define PDU_MAX_LENGTH 253
-#define ADU_MAX_LENGTH 260
+#define REG_COLOR_B_RED   14
+#define REG_COLOR_B_GREEN 16
+#define REG_COLOR_B_BLUE  18
 
-#define MBAP_HEADER_LENGTH 7
-#define PROTOCOL_ID_MODBUS 0x0000
+#define REG_COLOR_F_RED   20
+#define REG_COLOR_F_GREEN 22
+#define REG_COLOR_F_BLUE  24
 
-#define WRITE_MULTIPLE_REG_REQ_MIN_LENGTH 5
-#define WRITE_MULTIPLE_REG_RSP_LENGTH     4
-#define WRITE_MULTIPLE_REG_QUANTITY_MIN   0x0001
-#define WRITE_MULTIPLE_REG_QUANTITY_MAX   0x007B // See m
+#define REG_IMAGE_NR 26
 
-#define EXCEPTION_OFFSET 0x80
+// Global variables
+static char* TAG = "Modbus_TCP"; // Tag used in logs
 
-// Application specific constants
-#define REGISTER_COUNT          208
-#define REG_ADDR_BG_COLOR_RED   0x0000 // 8-bit red background color
-#define REG_ADDR_BG_COLOR_GREEN 0x0001 // 8-bit green background color
-#define REG_ADDR_BG_COLOR_BLUE  0x0002 // 8-bit blue background colo
-#define REG_ADDR_FG_COLOR_RED   0x0003 // 8-bit red text color
-#define REG_ADDR_FG_COLOR_GREEN 0x0004 // 8-bit green text color
-#define REG_ADDR_FG_COLOR_BLUE  0x0005 // 8-bit blue text color
-#define REG_ADDR_IMAGE_NUM      0x0006 // 16-bit image number
-#define REG_ADDR_TEXT           0x0007 // Start of text registers (1 reg / ascii character, null terminated)
-#define REG_SIZE_TEXT           0x00C8 // 200 registers / characters
-#define TEXT_POS_X              10
-#define TEXT_POS_Y              10
-#define IMG_POS_X               0
-#define IMG_POS_Y               75
+static struct tcp_pcb* modbus_pcb;
+uint8_t registers[MAX_REG];
+
+// Functions
+static err_t modbus_incoming_data(void* arg, struct tcp_pcb* pcb, struct pbuf* p, err_t err);
+static err_t modbus_accept(void* arg, struct tcp_pcb* pcb, err_t err);
 
 /**
- * @brief Error codes for internal use in the modbus tcp server.
+ * @fn static err_t modbus_incoming_data(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err)
+ * @brief Function that's called when there is a new request on port 502.
+ * It handles the incoming data from QModMaster
  */
-typedef enum {
-    MB_TCP_ERR_OK,
-    MB_TCP_ERR_FAILED,
-    MB_TCP_ERR_INVALID_ADU,
-    MB_TCP_ERR_INVALID_PROTOCOL_ID,
-    MB_TCP_ERR_INVALID_LENGTH,
-    MB_TCP_ERR_MEM,
-} mb_tcp_err_t;
+static err_t modbus_incoming_data(void* arg, struct tcp_pcb* pcb, struct pbuf* p, err_t err) {
+    uint8_t counter;
+    char text[TEXT_LENGTH];
+    uint32_t result_background = 0xff000000;
+    uint32_t text_foreground_color = 0xff000000;
 
-/**
- * @brief Modbus function codes
- */
-enum {
-    WRITE_MULTIPLE_REGISTERS = 0x10,
-};
+    LWIP_UNUSED_ARG(arg); // This is used to prevent a warning
 
-/**
- * @brief Modbus exception codes
- */
-typedef enum {
-    ILLEGAL_FUNCTION = 0x01,
-    ILLEGAL_DATA_ADDRESS = 0x02,
-    ILLEGAL_DATA_VALUE = 0x03,
-    SERVER_DEVICE_FAILURE = 0x04,
-    ACKNOWLEDGE = 0x05,
-    SERVER_DEVICE_BUSY = 0x06,
-    MEMORY_PARITY_ERROR = 0x08,
-    GATEWAY_PATH_UNAVAILABLE = 0x0A,
-    GATEWAY_TARGET_DEVICE_FAILED_TO_RESPOND = 0x0B,
-} mb_exception_code_t;
+    // Putting underscores in the whole array
+    memset(text, '_', TEXT_LENGTH);
+    text[TEXT_LENGTH - 1] = '\0';
 
-/**
- * @brief Modbus TCP Application Data Unit (ADU)
- */
-typedef struct {
-    struct {
-        uint16_t transaction_id;
-        uint16_t protocol_id;
-        uint16_t length;
-        uint8_t unit_id;
-    } mbap_header;
-    uint8_t function_code;
-    uint8_t* data;
-} modbus_tcp_t;
+    if (p != NULL) {
+        LOG_INFO(TAG, "data is valid\n");
+        // Process the modbus data
+        tcp_recved(pcb, p->tot_len);
 
-/**
- * @brief The data fields of the write multiple registers request PDU.
- * @note The data field is not included in the struct.
- */
-typedef struct {
-    uint16_t start_address;
-    uint16_t quantity;
-    uint8_t byte_count;
-} write_multiple_reg_req_t;
-
-// Static global variables
-static char* TAG = "Modbus_TCP";           // Tag used in logs
-static uint16_t registers[REGISTER_COUNT]; // The modbus registers
-
-// Function prototypes
-static err_t tcp_accept_cb(void* arg, struct tcp_pcb* new_pcb, err_t err);
-static void tcp_err_cb(void* arg, err_t err);
-static err_t tcp_poll_cb(void* arg, struct tcp_pcb* pcb);
-static err_t tcp_sent_cb(void* arg, struct tcp_pcb* pcb, u16_t len);
-static err_t tcp_recv_cb(void* arg, struct tcp_pcb* pcb, struct pbuf* p, err_t err);
-static mb_tcp_err_t parse_data_to_adu(modbus_tcp_t* adu, uint8_t* data, size_t length);
-static mb_tcp_err_t handle_modbus_request(modbus_tcp_t* req_adu, modbus_tcp_t* rsp_adu);
-static mb_tcp_err_t send_modbus_response(struct tcp_pcb* pcb, modbus_tcp_t* rsp_adu);
-static void handle_mb_func_write_multiple_req(modbus_tcp_t* req_adu, modbus_tcp_t* rsp_adu);
-static void generate_modbus_exception_rsp(modbus_tcp_t* req_adu,
-                                          modbus_tcp_t* rsp_adu,
-                                          mb_exception_code_t exception_code);
-static void modbus_update_app(void);
-static const char* img_num_to_filename(uint16_t img_num);
-static void dump_adu(modbus_tcp_t* adu);
-
-void modbus_tcp_init(void) {
-    struct tcp_pcb* modbus_pcb;
-
-    // Initialize the modbus tcp pcb
-    modbus_pcb = tcp_new();
-    if (modbus_pcb == NULL) {
-        LOG_CRIT(TAG, "Failed to create modbus pcb");
-        return;
-    }
-
-    // Listen on all interfaces (port 502)
-    if (tcp_bind(modbus_pcb, IP_ADDR_ANY, MODBUS_TCP_PORT) != ERR_OK) {
-        LOG_CRIT(TAG, "Failed to bind modbus pcb");
-        return;
-    }
-
-    // Set the state of the pcb to LISTEN
-    modbus_pcb = tcp_listen(modbus_pcb);
-    if (modbus_pcb == NULL) {
-        LOG_CRIT(TAG, "Failed to listen on modbus pcb");
-        return;
-    }
-
-    // Set the callback function for incoming connections
-    tcp_accept(modbus_pcb, tcp_accept_cb);
-}
-
-/**
- * @brief Callback function for incoming connections.
- *
- * @param arg not used
- * @param new_pcb
- * @param err
- * @return
- */
-static err_t tcp_accept_cb(void* arg, struct tcp_pcb* new_pcb, err_t err) {
-    LOG_DEBUG(TAG, "TCP accept");
-
-    if (err != ERR_OK) {
-        LOG_WARN(TAG, "TCP accept failed with error(%d): %s", err, lwip_strerr(err));
-        return err;
-    }
-
-    // Set the callback functions for the new pcb
-    tcp_recv(new_pcb, tcp_recv_cb);
-    tcp_sent(new_pcb, tcp_sent_cb);
-    tcp_err(new_pcb, tcp_err_cb);
-    tcp_poll(new_pcb, tcp_poll_cb, TCP_POLL_INTERVAL);
-
-    return ERR_OK;
-}
-
-/**
- * @brief Callback function for tcp errors.
- *
- * @param arg
- * @param err
- */
-static void tcp_err_cb(void* arg, err_t err) {
-    LOG_WARN(TAG, "TCP error(%d): %s", err, lwip_strerr(err));
-}
-
-/**
- * @brief Callback function for tcp poll.
- *
- * This function is called periodically to check if the connection is still alive.
- * The interval is set by TCP_POLL_INTERVAL.
- *
- * @param arg
- * @param pcb
- * @return ERR_OK
- */
-static err_t tcp_poll_cb(void* arg, struct tcp_pcb* pcb) {
-    LOG_DEBUG(TAG, "TCP poll");
-    return ERR_OK;
-}
-
-/**
- * @brief Callback function for tcp sent.
- *
- * Called when sent data has been acknowledged by the remote side.
- *
- * @param arg
- * @param pcb
- * @param len
- * @return ERR_OK
- */
-static err_t tcp_sent_cb(void* arg, struct tcp_pcb* pcb, u16_t len) {
-    LOG_DEBUG(TAG, "TCP data acknowledged");
-    return ERR_OK;
-}
-
-/**
- * @brief Callback function for tcp receive.
- *
- * Called when data has been received.
- *
- * @param arg
- * @param pcb
- * @param p
- * @param err
- * @return
- */
-static err_t tcp_recv_cb(void* arg, struct tcp_pcb* pcb, struct pbuf* p, err_t err) {
-    modbus_tcp_t mb_req_adu; // Modbus request adu to store the received data in
-
-    LOG_DEBUG(TAG, "TCP data received");
-
-    // Connection closed?
-    if (p == NULL && err == ERR_OK) {
-        LOG_INFO(TAG, "Remote closed connection");
-        return tcp_close(pcb);
-    }
-
-    if (err != ERR_OK) {
-        LOG_WARN(TAG, "TCP data received with error(%d): %s", err, lwip_strerr(err));
-        return ERR_OK;
-    }
-
-    // Copy the data from the pbuf to the modbus request adu
-    mb_tcp_err_t mb_err = parse_data_to_adu(&mb_req_adu, p->payload, p->len);
-    if (mb_err != MB_TCP_ERR_OK) {
-        LOG_WARN(TAG, "Invalid modbus adu received");
-        goto err_adu_read;
-    }
-
-    // Handle the modbus request
-    modbus_tcp_t mb_rsp_adu;
-    handle_modbus_request(&mb_req_adu, &mb_rsp_adu);
-
-    // Tell the tcp stack that we have taken the data
-    tcp_recved(pcb, p->tot_len);
-
-    // Send the modbus response
-    if (send_modbus_response(pcb, &mb_rsp_adu) != MB_TCP_ERR_OK) {
-        LOG_WARN(TAG, "Failed to send modbus response");
-        goto err_rsp_fail;
-    }
-
-err_rsp_fail:
-    free(mb_rsp_adu.data);
-err_adu_read:
-    pbuf_free(p);
-    return ERR_OK;
-}
-
-/**
- * @brief Parses the given data into the tcp ADU struct.
- *
- * This function takes the raw data received and parses it into a modbus TCP Application Data Unit (ADU).
- *
- * @note The data field of the ADU still points to the raw data, so it must stay valid until the ADU is no longer
- * needed.
- * @todo Store the data in the ADU struct instead of just pointing to it?
- *
- * @param[out] adu Pointer to a modbus_tcp_t structure where the parsed ADU will be stored.
- * @param[in] data Pointer to the raw data received from the modbus TCP server.
- * @param[in] length Length of the raw data.
- */
-static mb_tcp_err_t parse_data_to_adu(modbus_tcp_t* adu, uint8_t* data, size_t length) {
-    if (length > ADU_MAX_LENGTH) {
-        LOG_DEBUG(TAG, "Invalid adu length: %d, expected max %d", length, ADU_MAX_LENGTH);
-        return MB_TCP_ERR_INVALID_ADU;
-    }
-    if (length < MBAP_HEADER_LENGTH) {
-        LOG_DEBUG(TAG, "Invalid adu length: %d, expected at least %d", length, MBAP_HEADER_LENGTH);
-        return MB_TCP_ERR_INVALID_ADU;
-    }
-
-    // The adu struct is a one-to-one map of the modbus adu, so we can just copy the data
-    // But modbus fields are big endian, so we need to convert them to little endian
-    adu->mbap_header.transaction_id = (data[0] << 8) | data[1];
-    adu->mbap_header.protocol_id = (data[2] << 8) | data[3];
-    adu->mbap_header.length = (data[4] << 8) | data[5];
-    adu->mbap_header.unit_id = data[6];
-    adu->function_code = data[7];
-    adu->data = &data[8]; // Don't change the data endianness yet, since it's structure is function dependent
-
-    // Correct protocol id?
-    if (adu->mbap_header.protocol_id != PROTOCOL_ID_MODBUS) {
-        LOG_DEBUG(TAG, "Invalid protocol id: %d, expected %d", adu->mbap_header.protocol_id, PROTOCOL_ID_MODBUS);
-        return MB_TCP_ERR_INVALID_PROTOCOL_ID;
-    }
-
-    // Length matches length field?
-    if (adu->mbap_header.length != length - MBAP_HEADER_LENGTH + 1) {
-        LOG_DEBUG(TAG, "Length mismatch: %d, expected %d", adu->mbap_header.length, length - MBAP_HEADER_LENGTH + 1);
-        return MB_TCP_ERR_INVALID_ADU;
-    }
-
-    return MB_TCP_ERR_OK;
-}
-
-/**
- * @brief Handles the given modbus request and generates a response.
- *
- * Handles the given modbus request and generates a response, either a normal response or an exception response.
- * The response data field is allocated and must be freed by the caller.
- *
- * @param[in] req_adu Pointer to the modbus request adu.
- * @param[out] rsp_adu Pointer to the modbus response adu.
- * @return MB_TCP_ERR_OK if the request was handled successfully, otherwise an error code.
- */
-static mb_tcp_err_t handle_modbus_request(modbus_tcp_t* req_adu, modbus_tcp_t* rsp_adu) {
-    // Check if the function code is supported
-    switch (req_adu->function_code) {
-        case WRITE_MULTIPLE_REGISTERS: {
-            LOG_INFO(TAG, "Write multiple registers request received");
-            handle_mb_func_write_multiple_req(req_adu, rsp_adu);
-            break;
+        // Putting the buffer in the register array
+        for (uint16_t i = 0; i < p->tot_len && i < MAX_REG; i++) {
+            registers[i] = ((uint8_t*)p->payload)[i];
         }
-        default: {
-            LOG_WARN(TAG, "Unsupported function code: %d", req_adu->function_code);
-            generate_modbus_exception_rsp(req_adu, rsp_adu, ILLEGAL_FUNCTION);
-        }
-    }
 
-    return MB_TCP_ERR_OK;
-}
+        if (registers[MODBUS_MODE] == MULTIPLE_REG) {
+            // Check if it's a Modbus Write Multiple Registers request (0x10)
+            LOG_INFO(TAG, "in writing multiple register mode\n");
 
-/**
- * @brief Generates a modbus exception response.
- *
- * Generates a modbus exception response based on the given request adu and exception code.
- * The response data field is allocated and must be freed by the caller.
- *
- * @param[in] req_adu The request adu to base the response adu on.
- * @param[out] rsp_adu The response adu to fill.
- * @param[in] exception_code The exception code to use.
- */
-static void generate_modbus_exception_rsp(modbus_tcp_t* req_adu,
-                                          modbus_tcp_t* rsp_adu,
-                                          mb_exception_code_t exception_code) {
-    uint16_t pdu_length = 2; // Function code + exception code
+            LOG_INFO(TAG, "Background R:%d G:%d B:%d\nForeground: R:%d G:%d B:%d\nImage Nr: %d",
+                     registers[REG_COLOR_B_RED], registers[REG_COLOR_B_GREEN], registers[REG_COLOR_B_BLUE],
+                     registers[REG_COLOR_F_RED], registers[REG_COLOR_F_GREEN], registers[REG_COLOR_F_BLUE],
+                     registers[REG_IMAGE_NR]);
 
-    // Fill the response adu based on the request adu
-    rsp_adu->mbap_header.transaction_id = req_adu->mbap_header.transaction_id;
-    rsp_adu->mbap_header.protocol_id = PROTOCOL_ID_MODBUS;
-    rsp_adu->mbap_header.length = 1 + pdu_length; // 1 for the unit id
-    rsp_adu->mbap_header.unit_id = req_adu->mbap_header.unit_id;
-    rsp_adu->function_code = req_adu->function_code + EXCEPTION_OFFSET;
+            counter = 0;
+            for (int i = START_DATA; i < REG_LENGTH; i++) {
+                if (i % 2 == 0) {
+                    text[counter] = registers[i];
+                    counter++;
+                }
+            }
 
-    // Allocate memory for the exception code
-    rsp_adu->data = malloc(1);
-    if (rsp_adu->data == NULL) {
-        LOG_CRIT(TAG, "Failed to allocate memory for exception code");
-        return;
-    }
+            result_background |= ((uint32_t)registers[REG_COLOR_B_RED]) << 16;
+            result_background |= ((uint32_t)registers[REG_COLOR_B_GREEN]) << 8;
+            result_background |= (uint32_t)registers[REG_COLOR_B_BLUE];
 
-    rsp_adu->data[0] = exception_code;
-}
+            text_foreground_color |= ((uint32_t)registers[REG_COLOR_F_RED]) << 16;
+            text_foreground_color |= ((uint32_t)registers[REG_COLOR_F_GREEN]) << 8;
+            text_foreground_color |= (uint32_t)registers[REG_COLOR_F_BLUE];
 
-/**
- * @brief Sends the given modbus response.
- *
- * Sends the given modbus response to the given tcp pcb.
- * A copy of the response data is made, so the response adu data can be freed after this function returns.
- *
- * @param[in,out] pcb The tcp pcb to send the response to (same as the pcb used to receive the request).
- * @param[in] rsp_adu The response adu to send.
- * @return MB_TCP_ERR_OK if the response was sent successfully, otherwise an error code.
- */
-static mb_tcp_err_t send_modbus_response(struct tcp_pcb* pcb, modbus_tcp_t* rsp_adu) {
-    uint16_t pdu_length = rsp_adu->mbap_header.length - 1; // Length of the data + 1 (for the unit id)
-    uint16_t adu_length = MBAP_HEADER_LENGTH + pdu_length;
-    uint8_t data[adu_length];
-    err_t err;
+            // Processing the image index
+            size_t number_of_files = llfs_file_count(); // How many files that there are
 
-    if (pdu_length > PDU_MAX_LENGTH) {
-        LOG_WARN(TAG, "Invalid pdu length: %d, expected less than %d", pdu_length, PDU_MAX_LENGTH);
-        return MB_TCP_ERR_INVALID_ADU;
-    }
+            if (number_of_files > 0) {
+                llfs_file_t file_list[number_of_files];
+                number_of_files = llfs_file_list(file_list, number_of_files, NULL);
 
-    LOG_DEBUG(TAG, "Sending modbus response with length: %d", adu_length);
+                lcd_clear_text();
+                lcd_clear_images();
+                lcd_stop_all_gifs();
 
-    // Serialize the adu (little endian -> big endian)
-    data[0] = rsp_adu->mbap_header.transaction_id >> 8;
-    data[1] = rsp_adu->mbap_header.transaction_id & 0xFF;
-    data[2] = rsp_adu->mbap_header.protocol_id >> 8;
-    data[3] = rsp_adu->mbap_header.protocol_id & 0xFF;
-    data[4] = rsp_adu->mbap_header.length >> 8;
-    data[5] = rsp_adu->mbap_header.length & 0xFF;
-    data[6] = rsp_adu->mbap_header.unit_id;
-    data[7] = rsp_adu->function_code;
+                lcd_display_text(text, 10, 10, text_foreground_color, result_background, LCD_FONT24);
 
-    // The data should already be in big endian, so we can just copy it
-    memcpy(&data[8], rsp_adu->data, pdu_length - 1); // -1 function code is also in the pdu
+                if (number_of_files < registers[REG_IMAGE_NR]) {
+                    lcd_display_text("FILE NOT IN FILESYSTEM", 10, 75, LCD_RED, LCD_BLACK, LCD_FONT24);
+                } else {
+                    const char* ext = strrchr(file_list[registers[REG_IMAGE_NR] - 1].name, '.');
+                    if (ext == NULL) {
+                        LOG_CRIT(TAG, "No valid extension found");
+                    } else if (strcmp(ext, ".gif") == 0) {
+                        lcd_draw_gif_from_llfs_file(&file_list[registers[REG_IMAGE_NR] - 1], 0, 75);
+                    } else if (strcmp(ext, ".bmp") == 0) {
+                        lcd_draw_img_from_llfs_file(&file_list[registers[REG_IMAGE_NR] - 1], 0, 75);
+                    }
+                }
+            }
 
-    if (adu_length > tcp_sndbuf(pcb)) {
-        LOG_WARN(TAG, "Not enough space in tcp buffer to send modbus response");
-        return MB_TCP_ERR_MEM;
-    }
-
-    // Send the data
-    err = tcp_write(pcb, data, adu_length, TCP_WRITE_FLAG_COPY);
-    if (err != ERR_OK) {
-        LOG_WARN(TAG, "Failed to send modbus response with error(%d): %s", err, lwip_strerr(err));
-        return MB_TCP_ERR_FAILED;
-    }
-
-    return MB_TCP_ERR_OK;
-}
-
-/**
- * @brief Handles a write multiple registers request.
- *
- * Handles a write multiple registers request and generates a response adu accordingly.
- *
- * @param[in] req_adu The request adu to handle.
- * @param[out] rsp_adu The response adu to fill.
- */
-static void handle_mb_func_write_multiple_req(modbus_tcp_t* req_adu, modbus_tcp_t* rsp_adu) {
-    write_multiple_reg_req_t req_pdu;
-    uint16_t req_data_length = req_adu->mbap_header.length - 2; // -2 for the unit id and function code
-
-    // Request at least enough data for the minimum length?
-    if (req_data_length < WRITE_MULTIPLE_REG_REQ_MIN_LENGTH) {
-        LOG_WARN(TAG, "Invalid write multiple registers request length, not enough data for minimum length");
-        generate_modbus_exception_rsp(req_adu, rsp_adu, ILLEGAL_DATA_VALUE);
-        return;
-    }
-
-    // Map the data to the write multiple registers request struct and convert it to little endian
-    req_pdu.start_address = (req_adu->data[0] << 8) | req_adu->data[1];
-    req_pdu.quantity = (req_adu->data[2] << 8) | req_adu->data[3];
-    req_pdu.byte_count = req_adu->data[4];
-
-    // Request the correct length? Do the byte and register count match?
-    if (req_data_length != WRITE_MULTIPLE_REG_REQ_MIN_LENGTH + req_pdu.byte_count
-        || req_pdu.quantity < WRITE_MULTIPLE_REG_QUANTITY_MIN || req_pdu.quantity > WRITE_MULTIPLE_REG_QUANTITY_MAX
-        || req_pdu.quantity * 2 != req_pdu.byte_count) {
-        LOG_WARN(TAG, "Invalid write multiple registers request length");
-        generate_modbus_exception_rsp(req_adu, rsp_adu, ILLEGAL_DATA_VALUE);
-        return;
-    }
-
-    // Invalid start address or quantity?
-    if (req_pdu.start_address + req_pdu.quantity >= REGISTER_COUNT) {
-        LOG_DEBUG(TAG, "Invalid start address or quantity");
-        generate_modbus_exception_rsp(req_adu, rsp_adu, ILLEGAL_DATA_ADDRESS);
-        return;
-    }
-
-    // Convert the data to register values (big endian -> little endian)
-    for (uint16_t i = 0; i < req_pdu.quantity; i++) {
-        registers[req_pdu.start_address + i] = (req_adu->data[WRITE_MULTIPLE_REG_REQ_MIN_LENGTH + i * 2] << 8)
-                                               | req_adu->data[WRITE_MULTIPLE_REG_REQ_MIN_LENGTH + 1 + i * 2];
-    }
-
-    // Update the application with the new register values
-    modbus_update_app(); // TODO: do this when the request is handled successfully, to avoid timeouts.
-
-    // Fill the response adu based on the request adu
-    rsp_adu->mbap_header.transaction_id = req_adu->mbap_header.transaction_id;
-    rsp_adu->mbap_header.protocol_id = PROTOCOL_ID_MODBUS;
-    rsp_adu->mbap_header.length = 2 + WRITE_MULTIPLE_REG_RSP_LENGTH; // 2 for the unit id and function code
-    rsp_adu->mbap_header.unit_id = req_adu->mbap_header.unit_id;
-    rsp_adu->function_code = req_adu->function_code;
-
-    // Allocate memory for the response data
-    rsp_adu->data = malloc(WRITE_MULTIPLE_REG_RSP_LENGTH);
-    if (rsp_adu->data == NULL) {
-        LOG_CRIT(TAG, "Failed to allocate memory for response data");
-        return;
-    }
-
-    // The response data are the same 4 bytes as the request data
-    memcpy(rsp_adu->data, req_adu->data, WRITE_MULTIPLE_REG_RSP_LENGTH);
-}
-
-/**
- * @brief Dumps the given ADU to the log.
- *
- * @param[in] adu Pointer to the ADU to dump.
- */
-static void dump_adu(modbus_tcp_t* adu) {
-    LOG_DEBUG(TAG, "Modbus adu:");
-    LOG_DEBUG(TAG, "    Transaction id: %d", adu->mbap_header.transaction_id);
-    LOG_DEBUG(TAG, "    Protocol id: %d", adu->mbap_header.protocol_id);
-    LOG_DEBUG(TAG, "    Length: %d", adu->mbap_header.length);
-    LOG_DEBUG(TAG, "    Unit id: %d", adu->mbap_header.unit_id);
-    LOG_DEBUG(TAG, "    Function code: %d", adu->function_code);
-    LOG_DEBUG(TAG, "    Data: ");
-    for (size_t i = 0; i < adu->mbap_header.length - 2; i++) {
-        LOG_DEBUG(TAG, "     [%d]:%d", i, adu->data[i]);
-    }
-}
-
-/**
- * @brief Update the application with the new register values
- */
-static void modbus_update_app(void) {
-    uint32_t text_color; // Text color in ARGB888
-    uint32_t bg_color;   // Background color in ARGB888
-    char text[REG_SIZE_TEXT];
-    const char* filename;
-
-    LOG_INFO(TAG, "Updating application with new register values");
-
-    // Get the colors from the registers
-    text_color = 0xFF000000 | (registers[REG_ADDR_FG_COLOR_RED] << 16) | (registers[REG_ADDR_FG_COLOR_GREEN] << 8)
-                 | registers[REG_ADDR_FG_COLOR_BLUE];
-    bg_color = 0xFF000000 | (registers[REG_ADDR_BG_COLOR_RED] << 16) | (registers[REG_ADDR_BG_COLOR_GREEN] << 8)
-               | registers[REG_ADDR_BG_COLOR_BLUE];
-
-    // Get the text from the registers
-    for (int i = 0; i < REG_SIZE_TEXT; i++) {
-        text[i] = registers[REG_ADDR_TEXT + i];
-    }
-
-    // Get the filename based on the image number register
-    filename = img_num_to_filename(registers[REG_ADDR_IMAGE_NUM]);
-
-    // Clear the screen
-    lcd_clear_images();
-    lcd_clear_text();
-
-    // Display the text
-    lcd_display_text(text, TEXT_POS_X, TEXT_POS_Y, text_color, bg_color, LCD_FONT24);
-
-    // Try to display the image
-    if (filename != NULL) {
-        LOG_DEBUG(TAG, "Displaying image: %s", filename);
-
-        char* ext = strrchr(filename, '.');
-        if (ext == NULL) {
-            LOG_WARN(TAG, "File %s has no valid extension", filename);
-        } else if (strcmp(ext, ".gif") == 0) {
-            lcd_draw_gif_from_fs(filename, IMG_POS_X, IMG_POS_Y);
-        } else if (strcmp(ext, ".bmp") == 0) {
-            lcd_draw_img_from_fs(filename, IMG_POS_X, IMG_POS_Y);
         } else {
-            LOG_WARN(TAG, "File %s is not a valid img", filename);
+            LOG_INFO(TAG, "not in writing multiple register mode!!!\n");
         }
-    } else {
-        LOG_WARN(TAG, "No image found");
+    } else if (err == ERR_OK) {
+        tcp_close(pcb); // When everything was ok close the TCP connection
     }
+    return ERR_OK;
 }
 
 /**
- * @brief Convert the image number register to a filename
- *
- * Converts the image number register to a filename by looking up the file in the filesystem.
- *
- * @note This function doesn't check if the file is a valid image file.
- * So the image number is more a file number.
- *
- * @param[in] img_num The image number register
- * @return The filename of the image or NULL if no file is found
+ * @fn static err_t modbus_accept(void *arg, struct tcp_pcb *pcb, err_t err)
+ * @brief Sets the function that's being called when theirs incoming data
  */
-static const char* img_num_to_filename(uint16_t img_num) {
-    size_t number_of_files = llfs_file_count();
+static err_t modbus_accept(void* arg, struct tcp_pcb* pcb, err_t err) {
+    LWIP_UNUSED_ARG(arg);
+    LWIP_UNUSED_ARG(err);
 
-    LOG_DEBUG(TAG, "Converting image number %d to filename, %d files found", img_num, number_of_files);
+    // Sets the priority of a connection.
+    tcp_setprio(pcb, TCP_PRIO_MIN);
 
-    if (number_of_files == 0 || img_num > number_of_files) {
-        LOG_DEBUG(TAG, "No files found or invalid image number: %d", img_num);
-        return NULL;
-    }
+    // Sets which function is being called when new data arrives
+    tcp_recv(pcb, modbus_incoming_data);
 
-    llfs_file_t files[number_of_files];
-    llfs_file_list(files, number_of_files, NULL);
-    return files[img_num].name;
-}
+    return ERR_OK;
+}
+
+/**
+ * @fn void modbus_init
+ * @brief Initializes the modbus tcp
+ */
+void modbus_init(void) {
+    LOG_INFO(TAG, "Initializing");
+    // Creating a new tcp pcb
+    modbus_pcb = tcp_new();
+
+    // Bind the modbus_pcb to port 502
+    tcp_bind(modbus_pcb, IP_ADDR_ANY, MODBUSPORT);
+
+    modbus_pcb = tcp_listen(modbus_pcb);
+    // Set callback function for incoming connections
+    tcp_accept(modbus_pcb, modbus_accept);
+    LOG_INFO(TAG, "initialized");
+}

project/Core/Src/tcp_cmd.c

@@ -1,184 +1,592 @@
 /**
  * @file tcp_cmd.c
- * @brief TCP CMD interface
- * @author Gert R.
+ * @brief This file contains the implementation of the tcp command interface
+ * @author Sander S.
  */
 #include "tcp_cmd.h"
 
-static uint32_t result_txt = 0xff000000; // Store text color
-static uint32_t result_bg = 0xff000000;  // Store background color
+#define MAX_TOKENS  10
+#define MAX_CMD_LEN 50
 
-static void tcp_cmd_close(struct tcp_pcb* pcb) {
-    tcp_arg(pcb, NULL);
-    tcp_sent(pcb, NULL);
-    tcp_recv(pcb, NULL);
-    tcp_close(pcb);
+#define NCAT
+
+static const char* TAG = "tcp_cmd";
+static uint32_t color_txt = 0xffffffff; // Store text color
+static uint32_t color_bg =  0xff000000;  // Store background color
+
+static void tcp_cmd_write(struct tcp_pcb* pcb, const char* str);
+static void tcp_cmd_print_header(struct tcp_pcb* pcb);
+static void tcp_cmd_print_help(struct tcp_pcb* pcb);
+static void tcp_cmd_clear(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_text(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_bg_color(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_color(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_list_files(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_set_image(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_set_gif(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_exit(struct tcp_pcb* pcb, int argc, char** argv);
+static void tcp_cmd_unknown_cmd(struct tcp_pcb* pcb, char* cmd);
+static bool tcp_cmd_parser(struct tcp_pcb* pcb, int argc, char** argv);
+static err_t tcp_cmd_accept(void* arg, struct tcp_pcb* pcb, err_t err);
+static void tcp_cmd_close(struct tcp_pcb* pcb);
+
+/**
+ * @brief This function shifts the arguments in argv and argc
+ * @param[in,out] argc The argc pointer
+ * @param[in,out] argv The argv pointer
+ * @return The shifted argument
+ */
+static char* shift_args(int* argc, char*** argv) {
+    if (*argc == 0) {
+        return NULL;
+    }
+    char* arg = (*argv)[0];
+    (*argv)++;
+    (*argc)--;
+    return arg;
 }
 
-static err_t tcp_cmd_recv(void* arg, struct tcp_pcb* pcb, struct pbuf* p, err_t err) {
-    size_t len;
-    size_t number_of_files;
-    uint8_t file_in_fs;
-    uint8_t check = 0;
+/**
+ * @brief This function converts a string to a color
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ * @param[out] color The color to write to
+ * @return true The conversion failed
+ * @return false The conversion succeeded
+ */
+static bool str_to_color(int argc, char** argv, uint32_t* color) {
+    if (argc == 1) {
+        *color = (uint32_t)strtoul(argv[0], NULL, 16);
+        return false;
+    }
+    if (argc == 3) {
+        *color = 0xff000000;
+        *color |= (uint32_t)strtoul(argv[0], NULL, 10) << 16;
+        *color |= (uint32_t)strtoul(argv[1], NULL, 10) << 8;
+        *color |= (uint32_t)strtoul(argv[2], NULL, 10);
+        return false;
+    }
+    if (argc == 4) {
+        *color = (uint32_t)strtoul(argv[0], NULL, 10) << 24;
+        *color |= (uint32_t)strtoul(argv[1], NULL, 10) << 16;
+        *color |= (uint32_t)strtoul(argv[2], NULL, 10) << 8;
+        *color |= (uint32_t)strtoul(argv[3], NULL, 10);
+        return false;
+    }
 
-    char* pc;
-    char tcp_buffer[1024];
-    char text[256];
-    char color_r[3];
-    char color_g[3];
-    char color_b[3];
+    return true;
+}
 
-    char text_color_r[3];
-    char text_color_g[3];
-    char text_color_b[3];
-
-    char extension[4];
-
-    char* endptr;
-
-    if (err == ERR_OK && p != NULL) {
-        tcp_recved(pcb, p->tot_len);
-        pc = (char*)p->payload;
-        len = p->tot_len;
-
-        for (size_t i = 0; i < len; i++) {
-            tcp_buffer[i] = pc[i];
+/**
+ * @brief This function removes the newline from a string the string can contain multiple lines
+ * @param[in,out] str The string to remove the newline from
+ */
+void tcp_cmd_remove_newline(char* str, size_t len) {
+    size_t i = 0;
+    size_t j = 0;
+    while (str[i] != '\0' && j < len) {
+        if (str[j] != '\n' && str[j] != '\r') {
+            str[i] = str[j];
+            i++;
         }
+        j++;
+    }
+    str[i] = '\0';
+}
 
-        if (!strncmp(tcp_buffer, "help", 4)) {
-            check = 1;
-            tcp_write(pcb,
-                      "help      : laat lijst zien met alle commando's\r\n"
-                      "text      : geeft tekst mee die op LCD komt (uw_text)\r\n"
-                      "color     : kleur achtergrond van scherm (255 255 255)\r\n"
-                      "textColor : kleur van tekst (255 255 255)\r\n"
-                      "listImages: laat een lijst zien van de mogelijke afbeeldingen\r\n"
-                      "setImage  : veranderd te afbeelding (naam_afbeelding)\r\n"
-                      "exit      : sluit de verbinding\r\n",
-                      354, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-        } else if (!strncmp(tcp_buffer, "text ", 5)) {
-            size_t i;
-            for (i = 0; i < len - 4; i++) {
-                text[i] = tcp_buffer[i + 5];
-            }
-            text[i - 1] = '\0';
-            lcd_clear_text();
-            lcd_display_text(text, 10, 10, result_txt, result_bg, LCD_FONT24);
-
-            check = 1;
-        } else if (!strncmp(tcp_buffer, "color", 5)) {
-            for (size_t i = 0; i < 3; i++) {
-                color_r[i] = tcp_buffer[i + 6];
-                color_g[i] = tcp_buffer[i + 10];
-                color_b[i] = tcp_buffer[i + 14];
-            }
-            result_bg |= strtoul(color_r, &endptr, 10) << 16;
-            result_bg |= strtoul(color_g, &endptr, 10) << 8;
-            result_bg |= strtoul(color_b, &endptr, 10);
-
-            check = 1;
-        } else if (!strncmp(tcp_buffer, "textColor", 9)) {
-            for (size_t i = 0; i < 3; i++) {
-                text_color_r[i] = tcp_buffer[i + 10];
-                text_color_g[i] = tcp_buffer[i + 14];
-                text_color_b[i] = tcp_buffer[i + 18];
-            }
-            result_txt |= strtoul(text_color_r, &endptr, 10) << 16;
-            result_txt |= strtoul(text_color_g, &endptr, 10) << 8;
-            result_txt |= strtoul(text_color_b, &endptr, 10);
-
-            check = 1;
-        } else if (!strncmp(tcp_buffer, "listImages", 10)) {
-            number_of_files = llfs_file_count();
-
-            if (number_of_files > 0) {
-                llfs_file_t file_list[number_of_files];
-                number_of_files = llfs_file_list(file_list, number_of_files, NULL);
-
-                for (size_t i = 0; i < number_of_files; i++) {
-                    tcp_write(pcb, file_list[i].name, strlen(file_list[i].name),
-                              TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-                    tcp_write(pcb, "\r\n", 2, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-                }
-            } else {
-                tcp_write(pcb, "NO files in filesystem\r\n", 24, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-            }
-
-            check = 1;
-        } else if (!strncmp(tcp_buffer, "setImage", 8)) {
-            char filename[len - 8];
-            for (size_t i = 0; i < len - 9; i++) {
-                filename[i] = tcp_buffer[i + 9];
-            }
-            for (size_t i = 0; i < 3; i++) {
-                extension[i] = tcp_buffer[i + len - 3];
-            }
-            filename[sizeof(filename) - 1] = '\0';
-            extension[3] = '\0';
-
-            number_of_files = llfs_file_count();
-
-            if (number_of_files > 0) {
-                llfs_file_t file_list[number_of_files];
-
-                number_of_files = llfs_file_list(file_list, number_of_files, NULL);
-
-                file_in_fs = 0;
-                for (size_t i = 0; i < number_of_files; i++) {
-                    if (!strcmp(filename, file_list[i].name)) {
-                        file_in_fs = 1;
-                    }
-                }
-
-                // Check which file extension is used and call right function
-                if (!strncmp(extension, "bmp", 3) && file_in_fs) {
-                    lcd_clear_images();
-                    lcd_draw_img_from_fs(filename, 10, 10);
-                } else if (!strncmp(extension, "gif", 3) && file_in_fs) {
-                    lcd_clear_images();
-                    lcd_draw_gif_from_fs(filename, 10, 10);
-                } else if (!file_in_fs) {
-                    tcp_write(pcb, "File NOT in filesystem\n\r", 24, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-                } else {
-                    tcp_write(pcb, "Extension NOT supported\n\r", 25, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-                }
-            } else {
-                tcp_write(pcb, "NO files in filesystem\r\n", 24, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-            }
-
-            check = 1;
-        } else if (!strncmp(tcp_buffer, "exit", 4)) {
-            lcd_clear_images();
-            lcd_clear_text();
-            tcp_cmd_close(pcb);
-            check = 1;
+/**
+ * @brief This function 'removes' the leading spaces from a string
+ * @param[in] str The string to remove the leading spaces from
+ * @param[in] len The length of the string
+ * @return char* A pointer to the first non-space character
+ */
+char* tcp_cmd_remove_leading_space(char* str, size_t len) {
+    size_t i = 0;
+    if (str == NULL || str[0] == '\0' || len == 0) {
+        return NULL;
+    }
+    while (str[i] != '\0' && i < len) {
+        if (isspace((int)str[i]) == 0) {
+            return &str[i];
         }
+        i++;
+    }
+    return NULL;
+}
 
-        if (!check && (strncmp(tcp_buffer, "\r\n", 2) != 0)) {
-            tcp_write(pcb, "Onbestaand commando: help voor lijst van commando's\r\n", 53,
-                      TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
+/**
+ * @brief This function converts a string to lowercase
+ * @param[in,out] str The string to convert
+ */
+void tcp_cmd_str_tolower(char* str) {
+    int i = 0;
+    while (str[i] != '\0') {
+        str[i] = (char)tolower((int)str[i]);
+        i++;
+    }
+}
+
+/**
+ * @brief This function finds the next token in the input string
+ * If the token is between quotes, return the whole string between quotes
+ * @param[in] input The input string
+ * @param[in] delimiter The delimiters to use
+ * @param[in,out] next The next token
+ * @return char* The next token
+ */
+char* tcp_cmd_get_next_token(char* input, const char* delimiters, char** next) {
+    if (input == NULL && *next == NULL) {
+        return NULL;
+    }
+    if (input == NULL) {
+        input = *next;
+    }
+
+    // Skip leading delimiters
+    input += strspn(input, delimiters);
+    if (*input == '\0') {
+        return NULL;
+    }
+
+    // If the token is between quotes, return the whole string between quotes
+    if (*input == '"') {
+        char* end = strchr(++input, '"');
+        if (end == NULL) {
+            return NULL;
         }
+        *end = '\0';
+        *next = end + 1;
+        return input;
+    }
 
-        pbuf_free(p);
-
-        if (len > tcp_sndbuf(pcb)) {
-            len = tcp_sndbuf(pcb);
-        }
-        tcp_sent(pcb, NULL);
+    // Find the end of the token
+    char* end = input + strcspn(input, delimiters);
+    if (*end == '\0') {
+        *next = end;
     } else {
-        pbuf_free(p);
+        *end = '\0';
+        *next = end + 1;
+    }
+    return input;
+}
+
+/**
+ * @brief This function is a wrapper for tcp_write and tcp_output
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] str The string to write
+ */
+static void tcp_cmd_write(struct tcp_pcb* pcb, const char* str) {
+    tcp_write(pcb, str, strlen(str), TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
+    tcp_output(pcb);
+}
+
+/**
+ * @brief This function prints the header of the tcp command interface
+ * @param[in] pcb The tcp_pcb struct to write to
+ */
+static void tcp_cmd_print_header(struct tcp_pcb* pcb) {
+    tcp_cmd_write(pcb, "    Welcome to the TCP CMD interface\n"
+                       "(Type help for a list of the commands! exit to close)\n"
+                       "============================================================\n"
+                       "$>");
+}
+
+/**
+ * @brief This function prints the help text
+ * @param[in] pcb The tcp_pcb struct to write to
+ */
+static void tcp_cmd_print_help(struct tcp_pcb* pcb) {
+    LOG_INFO(TAG, "Printing help");
+    tcp_cmd_write(pcb, "help                        : shows a list of commands\n"
+                       "clear text/images           : clears the text or images on the lcd\n"
+                       "text \"<text>\"               : puts text on the lcd\n"
+                       "bgColor (<a>) <r> <g> <b>   : set the background color of the lcd\n"
+                       "color (<a>) <r> <g> <b>     : set the color of the text\n"
+                       "listFiles                   : shows a list with images in the filesystem\n"
+                       "setImage <image_name>       : put an image on the screen\n"
+                       "setGif   <image_name>       : put a gif on the screen\n"
+                       "exit                        : closes the connection\n");
+}
+
+/**
+ * @brief This function handles the clear command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_clear(struct tcp_pcb* pcb, int argc, char** argv) {
+    if (argc == 0) {
+        LOG_INFO(TAG, "Clearing screen");
+        lcd_clear_text();
+        lcd_clear_images();
+        return;
+    }
+    if (argc == 1) {
+        if (strcmp(argv[0], "text") == 0) {
+            LOG_INFO(TAG, "Clearing text");
+            lcd_clear_text();
+            return;
+        }
+        if (strcmp(argv[0], "images") == 0) {
+            LOG_INFO(TAG, "Clearing images");
+            lcd_clear_images();
+            return;
+        }
+        LOG_WARN(TAG, "Bad usage of clear");
+        tcp_cmd_write(pcb, "Usage: clear\n");
+        tcp_cmd_write(pcb, "Usage: clear text\n");
+        tcp_cmd_write(pcb, "Usage: clear images\n");
+        return;
+    }
+}
+
+/**
+ * @brief This function handles the text command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_text(struct tcp_pcb* pcb, int argc, char** argv) {
+    if (argc == 1) {
+        LOG_INFO(TAG, "Setting text %s @ 10, 10", argv[0]);
+        lcd_display_text((const char*)argv[0], 10, 10, color_txt, color_bg, LCD_FONT24);
+        return;
+    }
+    if (argc == 3) {
+        LOG_INFO(TAG, "Setting text %s @ %lu, %lu", argv[0], (uint32_t)strtoul(argv[1], NULL, 10),
+                 (uint32_t)strtoul(argv[2], NULL, 10));
+        lcd_display_text((const char*)argv[0], (uint32_t)strtoul(argv[1], NULL, 10),
+                         (uint32_t)strtoul(argv[2], NULL, 10), color_txt, color_bg, LCD_FONT24);
+        return;
+    }
+    LOG_WARN(TAG, "Bad usage of text");
+    tcp_cmd_write(pcb, "Usage: text \"<text>\"\n");
+    tcp_cmd_write(pcb, "Usage: text <word>\n");
+    tcp_cmd_write(pcb, "Usage: text \"<text>\" <x> <y>\n");
+}
+
+/**
+ * @brief This function handles the bgcolor command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_bg_color(struct tcp_pcb* pcb, int argc, char** argv) {
+    if (!str_to_color(argc, argv, &color_bg)) {
+        LOG_INFO(TAG, "Setting background color to %08lX", color_bg);
+        return;
+    }
+    LOG_WARN(TAG, "Bad usage of bgcolor");
+    tcp_cmd_write(pcb, "Usage: bgcolor 0x<rrggbb>\n");
+    tcp_cmd_write(pcb, "Usage: bgcolor 0x<aarrggbb>\n");
+    tcp_cmd_write(pcb, "Usage: bgcolor <r> <g> <b>\n");
+    tcp_cmd_write(pcb, "Usage: bgcolor <a> <r> <g> <b>\n");
+}
+
+/**
+ * @brief This function handles the color command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_color(struct tcp_pcb* pcb, int argc, char** argv) {
+    if (!str_to_color(argc, argv, &color_txt)) {
+        LOG_INFO(TAG, "Setting color to %08lX", color_txt);
+        return;
+    }
+    LOG_WARN(TAG, "Bad usage of color");
+    tcp_cmd_write(pcb, "Usage: color 0x<rrggbb>\n");
+    tcp_cmd_write(pcb, "Usage: color 0x<aarrggbb>\n");
+    tcp_cmd_write(pcb, "Usage: color <r> <g> <b>\n");
+    tcp_cmd_write(pcb, "Usage: color <a> <r> <g> <b>\n");
+}
+
+/**
+ * @brief This function handles the listfiles command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_list_files(struct tcp_pcb* pcb, int argc, char** argv) {
+    UNUSED(argv);
+    if (argc == 0) {
+        LOG_INFO(TAG, "Listing files");
+        void* mem = NULL; // Pointer for internal use by the llfs library
+        llfs_file_t* file;
+        while ((file = llfs_next_file(&mem, NULL)) != NULL) {
+            tcp_cmd_write(pcb, file->name);
+            tcp_cmd_write(pcb, "\n");
+        }
+        return;
+    }
+    LOG_WARN(TAG, "Bad usage of listfiles");
+    tcp_cmd_write(pcb, "Usage: listfiles\n");
+    tcp_cmd_write(pcb, "Usage: ls\n");
+    return;
+}
+
+/**
+ * @brief This function handles the setimage command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_set_image(struct tcp_pcb* pcb, int argc, char** argv) {
+    const char* ext = NULL;
+    if (argc >= 1) {
+        ext = llfs_get_filename_ext(argv[0]);
+        if (strcmp(ext, "bmp") != 0) {
+            LOG_WARN(TAG, "setimage: File is not a bmp");
+            tcp_cmd_write(pcb, "File is not a bmp\n");
+            return;
+        }
+    }
+    if (argc == 1) {
+        LOG_INFO(TAG, "Setting image %s @ 0, 0", argv[0]);
+        lcd_draw_img_from_fs(argv[0], 0, 0);
+        return;
+    }
+    if (argc == 3) {
+        LOG_INFO(TAG, "Setting image %s @ %lu, %lu", argv[0], (uint32_t)strtoul(argv[1], NULL, 10),
+                 (uint32_t)strtoul(argv[2], NULL, 10));
+        lcd_draw_img_from_fs(argv[0], (uint32_t)strtoul(argv[1], NULL, 10), (uint32_t)strtoul(argv[2], NULL, 10));
+        return;
+    }
+    LOG_WARN(TAG, "Bad usage of setimage");
+    tcp_cmd_write(pcb, "Usage: setimage <filename>\n");
+    tcp_cmd_write(pcb, "Usage: setimage <filename> <x> <y>\n");
+}
+
+/**
+ * @brief This function handles the setgif command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_set_gif(struct tcp_pcb* pcb, int argc, char** argv) {
+    const char* ext = NULL;
+    if (argc >= 1) {
+        ext = llfs_get_filename_ext(argv[0]);
+        if (strcmp(ext, "gif") != 0) {
+            LOG_WARN(TAG, "setgif: File is not a gif");
+            tcp_cmd_write(pcb, "File is not a gif\n");
+            return;
+        }
+    }
+    if (argc == 1) {
+        LOG_INFO(TAG, "Setting gif %s @ 0, 0", argv[0]);
+        lcd_draw_gif_from_fs(argv[0], 0, 0);
+        return;
+    }
+    if (argc == 3) {
+        LOG_INFO(TAG, "Setting gif %s @ %lu, %lu", argv[0], (uint32_t)strtoul(argv[1], NULL, 10),
+                 (uint32_t)strtoul(argv[2], NULL, 10));
+        lcd_draw_gif_from_fs(argv[0], (uint32_t)strtoul(argv[1], NULL, 10), (uint32_t)strtoul(argv[2], NULL, 10));
+        return;
     }
 
-    if (err == ERR_OK && p == NULL) {
+    LOG_WARN(TAG, "Bad usage of setgif");
+    tcp_cmd_write(pcb, "Usage: setgif <filename>\n");
+    tcp_cmd_write(pcb, "Usage: setgif <filename> <x> <y>\n");
+}
+
+/**
+ * @brief This function handles the exit command
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] argc The number of arguments
+ * @param[in] argv The arguments
+ */
+static void tcp_cmd_exit(struct tcp_pcb* pcb, int argc, char** argv) {
+    UNUSED(argv);
+    if (argc == 0) {
+        LOG_INFO(TAG, "Closing connection");
+        tcp_cmd_write(pcb, "Exiting...\n");
+        lcd_clear_images();
+        lcd_clear_text();
+        tcp_close(pcb);
+        return;
+    }
+
+    LOG_WARN(TAG, "Bad usage of exit");
+    tcp_cmd_write(pcb, "Usage: exit\n");
+}
+
+/**
+ * @brief This function handles unknown commands
+ *
+ * @param[in] pcb The tcp_pcb struct to write to
+ * @param[in] cmd The command
+ */
+static void tcp_cmd_unknown_cmd(struct tcp_pcb* pcb, char* cmd) {
+    LOG_WARN(TAG, "Unknown command: %s", cmd);
+    tcp_cmd_write(pcb, "Unknown command: ");
+    tcp_cmd_write(pcb, cmd);
+    tcp_cmd_write(pcb, "\n");
+    tcp_cmd_write(pcb, "Type help for list of commands\n");
+}
+
+/**
+ * @brief This function parses the command and calls the appropriate function
+ *
+ * @param[in] pcb Pointer to the tcp_pcb struct to write to
+ * @param[in] argc Count of arguments
+ * @param[in] argv Array of arguments
+ * @return true Connection should be closed
+ * @return false Connection should be kept open
+ */
+static bool tcp_cmd_parser(struct tcp_pcb* pcb, int argc, char** argv) {
+    if (argc == 0) {
+        LOG_WARN(TAG, "No command given");
+        return false;
+    }
+    char* cmd = shift_args(&argc, &argv);
+    tcp_cmd_str_tolower(cmd);
+    if (strcmp(cmd, "help") == 0) {
+        tcp_cmd_print_help(pcb);
+        return false;
+    }
+    if (strcmp(cmd, "clear") == 0) {
+        tcp_cmd_clear(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "text") == 0) {
+        tcp_cmd_text(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "bgcolor") == 0) {
+        tcp_cmd_bg_color(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "color") == 0) {
+        tcp_cmd_color(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "listfiles") == 0) {
+        tcp_cmd_list_files(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "ls") == 0) {
+        tcp_cmd_list_files(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "setimage") == 0) {
+        tcp_cmd_set_image(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "setgif") == 0) {
+        tcp_cmd_set_gif(pcb, argc, argv);
+        return false;
+    }
+    if (strcmp(cmd, "exit") == 0) {
+        tcp_cmd_exit(pcb, argc, argv);
+        return true;
+    }
+    tcp_cmd_unknown_cmd(pcb, cmd);
+    return false;
+}
+
+/**
+ * @brief This function is called when the tcp connection receives data
+ * @param[in] arg The argument ptr
+ * @param[in] pcb The tcp_pcb struct
+ * @param[in] p The pbuf struct
+ * @param[in] err The error code from the tcp stack
+ * @return err_t ERR_OK if successful
+ */
+err_t tcp_cmd_recv(void* arg, struct tcp_pcb* pcb, struct pbuf* p, err_t err) {
+    int argc = 0;
+    char cmd[MAX_CMD_LEN] = {0};
+    char* cmd_ptr = NULL;
+    char* argv[MAX_TOKENS] = {0};
+    bool close_conn = false;
+    char* next = NULL;
+
+    LWIP_UNUSED_ARG(arg);
+
+    // Connection closed?
+    if (p == NULL && err == ERR_OK) {
+        LOG_INFO(TAG, "Remote closed connection");
+        return tcp_close(pcb);
+    }
+
+    if (err != ERR_OK) {
+        LOG_WARN(TAG, "TCP data received with error(%d): %s", err, lwip_strerr(err));
+        return ERR_OK;
+    }
+
+    if (p->tot_len >= MAX_CMD_LEN) {
+        LOG_WARN(TAG, "Command too long");
+        tcp_cmd_write(pcb, "Command too long");
+    }
+    uint16_t len = p->tot_len >= MAX_CMD_LEN ? MAX_CMD_LEN : p->tot_len;
+    // Make sure the string is null terminated
+    pbuf_copy_partial(p, cmd, len, 0);
+    cmd[len] = '\0';
+
+    // Tell the tcp stack that we have taken the data
+    tcp_recved(pcb, p->tot_len);
+
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    if (cmd[0] != '\0') {
+        LOG_INFO(TAG, "cmd: %s", cmd);
+    }
+    // Split string into tokens by delimiter (space)
+    cmd_ptr = tcp_cmd_remove_leading_space(cmd, strlen(cmd));
+    argv[0] = tcp_cmd_get_next_token(cmd_ptr, " ", &next);
+    if (argv[0] != NULL) {
+        argc = 1;
+    }
+    while (argv[argc - 1] != NULL && argc < MAX_TOKENS) {
+        // Check if the next token is 0 (end of string) strlen doesn't work here
+        if (*next == 0) {
+            argv[argc] = NULL;
+            break;
+        }
+        next = tcp_cmd_remove_leading_space(next, strlen(next));
+        argv[argc] = tcp_cmd_get_next_token(NULL, " ", &next);
+        if (argv[argc] == NULL) {
+            break;
+        }
+        argc++;
+    }
+
+    close_conn = tcp_cmd_parser(pcb, argc, argv);
+
+    if (close_conn) {
+        LOG_INFO(TAG, "Closing connection");
         tcp_cmd_close(pcb);
+#ifdef NCAT
+    } else {
+        tcp_cmd_write(pcb, "$>");
     }
-    if (strncmp(tcp_buffer, "\r\n", 2) != 0) {
-        tcp_write(pcb, "User: ", 6, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
+#else
+    } else if (argc > 0) {
+        tcp_cmd_write(pcb, "$>");
+    } else {
+        tcp_cmd_write(pcb, "");
     }
+#endif
 
+defer:
+    pbuf_free(p);
     return ERR_OK;
 }
 
+/**
+ * @brief This function is called when a new tcp connection is accepted
+ * @param[in] arg The argument
+ * @param[in] pcb The tcp_pcb struct
+ * @param[in] err The error
+ * @return err_t ERR_OK
+ */
 static err_t tcp_cmd_accept(void* arg, struct tcp_pcb* pcb, err_t err) {
     LWIP_UNUSED_ARG(arg);
     LWIP_UNUSED_ARG(err);
@@ -186,21 +594,46 @@ static err_t tcp_cmd_accept(void* arg, struct tcp_pcb* pcb, err_t err) {
     tcp_recv(pcb, tcp_cmd_recv);
     tcp_err(pcb, NULL);
     tcp_poll(pcb, NULL, 4);
-    tcp_write(pcb,
-              "    Welcom bij de TCP CMD Interface\r\n"
-              "(Typ help voor een lijst van de commando's! X om te sluiten)\r\n"
-              "============================================================\r\n"
-              "User: ",
-              168, TCP_WRITE_FLAG_COPY | TCP_WRITE_FLAG_MORE);
-    tcp_sent(pcb, NULL);
+
+    LOG_INFO(TAG, "New connection accepted");
+    tcp_cmd_print_header(pcb);
     return ERR_OK;
 }
+/**
+ * @brief This function closes the tcp connection
+ * @param[in] pcb The tcp_pcb struct to close
+ */
+static void tcp_cmd_close(struct tcp_pcb* pcb) {
+    tcp_arg(pcb, NULL);
+    tcp_sent(pcb, NULL);
+    tcp_recv(pcb, NULL);
+    tcp_close(pcb);
+}
 
+/**
+ * @brief This function initializes the tcp command interface
+ */
 void tcp_cmd_init(void) {
     struct tcp_pcb* tcp_pcb;
     tcp_pcb = tcp_new();
-    tcp_bind(tcp_pcb, IP_ADDR_ANY, 23);
+
+    if (tcp_pcb == NULL) {
+        LOG_CRIT(TAG, "Failed to allocate pcb");
+        return;
+    }
+
+    if (tcp_bind(tcp_pcb, IP_ADDR_ANY, 23) != ERR_OK) {
+        LOG_CRIT(TAG, "Failed to bind pcb");
+        free(tcp_pcb);
+        return;
+    }
 
     tcp_pcb = tcp_listen(tcp_pcb);
+    if (tcp_pcb == NULL) {
+        LOG_CRIT(TAG, "Failed to listen");
+        free(tcp_pcb);
+        return;
+    }
+
     tcp_accept(tcp_pcb, tcp_cmd_accept);
 }

tests/CMakeLists.txt

@@ -12,6 +12,7 @@ add_executable(tests)
 target_compile_definitions(tests
     PRIVATE
     "TESTING"
+    "DEBUG"
 )
 
 target_sources(tests
@@ -21,6 +22,7 @@ target_sources(tests
     ${CMAKE_SOURCE_DIR}/project/Core/Src/llfs_data.c
     ${CMAKE_SOURCE_DIR}/project/Core/Src/llfs.c
     ${CMAKE_SOURCE_DIR}/project/Core/Src/tftp.c
+    ${CMAKE_SOURCE_DIR}/project/Core/Src/tcp_cmd.c
 )
 
 target_compile_options(tests PRIVATE $<$<CONFIG:Debug>:

tests/mocs.c

@@ -1,49 +1,68 @@
 #include "mocs.h"
 #include "tftp.h"
+#include "mocs.h"
+#ifdef DEBUG
+#define dprint(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define dprint(fmt, ...)
+#endif
 
 void tftp_cleanup(void) {
+    dprint("tftp_cleanup\n");
 }
 uint32_t logger_get_timestamp(void) {
+    dprint("logger_get_timestamp\n");
+
     return 0;
 }
 
 int tftp_init(struct tftp_context* context) {
     UNUSED(context);
+
+    dprint("tftp_init\n");
     return 0;
 }
 
-void lcd_display_text(const char* text,
-                      uint16_t x_pos,
-                      uint16_t y_pos,
-                      uint32_t color,
-                      uint32_t bg_color,
-                      sFONT* font) {
+void lcd_display_text(const char* text, uint16_t x_pos, uint16_t y_pos, uint32_t color, uint32_t bg_color, sFONT* font) {
+    UNUSED(text);
+    UNUSED(x_pos);
+    UNUSED(y_pos);
     UNUSED(color);
     UNUSED(bg_color);
     UNUSED(font);
 
-    printf("lcd_display_text @ %d %d with color 0x%08X bg color 0x%08X\n%s\n", x_pos, y_pos, color, bg_color, text);
+    dprint("lcd_display_text @ %d %d with color 0x%08X bg color 0x%08X\n%s\n", x_pos, y_pos, color, bg_color, text);
 }
 
 void lcd_draw_img_from_fs(char* filename, uint32_t x_pos, uint32_t y_pos) {
-    printf("lcd_draw_img_from_fs\n%s @%d %d\n", filename, x_pos, y_pos);
+    UNUSED(filename);
+    UNUSED(x_pos);
+    UNUSED(y_pos);
+
+    dprint("lcd_draw_img_from_fs\n%s @%d %d\n", filename, x_pos, y_pos);
 }
 void lcd_draw_gif_from_fs(char* filename, uint32_t x_pos, uint32_t y_pos) {
-    printf("lcd_draw_gif_from_fs\n%s @%d %d\n", filename, x_pos, y_pos);
+    UNUSED(filename);
+    UNUSED(x_pos);
+    UNUSED(y_pos);
+
+    dprint("lcd_draw_gif_from_fs\n%s @%d %d\n", filename, x_pos, y_pos);
 }
 
 void lcd_clear_images(void) {
-    printf("lcd_clear_images\n");
+    dprint("lcd_clear_images\n");
 }
 
 void lcd_clear_text(void) {
-    printf("lcd_clear_text\n");
+    dprint("lcd_clear_text\n");
 }
 
 void lcd_draw_bmp_img(uint8_t* bmp_buff, uint32_t x_pos, uint32_t y_pos) {
     UNUSED(bmp_buff);
+    UNUSED(x_pos);
+    UNUSED(y_pos);
 
-    printf("lcd_draw_bmp_img @ %d %d\n", x_pos, y_pos);
+    dprint("lcd_draw_bmp_img @ %d %d\n", x_pos, y_pos);
 }
 
 void lcd_draw_gif(uint8_t* gif_buff, size_t len, uint32_t x_pos, uint32_t y_pos) {
@@ -52,10 +71,11 @@ void lcd_draw_gif(uint8_t* gif_buff, size_t len, uint32_t x_pos, uint32_t y_pos)
     UNUSED(x_pos);
     UNUSED(y_pos);
 
-    printf("lcd_draw_gif @ %d %d\n", x_pos, y_pos);
+    dprint("lcd_draw_gif @ %d %d\n", x_pos, y_pos);
 }
 
 struct tcp_pcb* tcp_new(void) {
+    dprint("tcp_new\n");
     return NULL;
 }
 
@@ -63,48 +83,69 @@ err_t tcp_bind(void* pcb, void* ipaddr, uint16_t port) {
     UNUSED(pcb);
     UNUSED(ipaddr);
     UNUSED(port);
+
+    dprint("tcp_bind @ %d\n", port);
     return ERR_OK;
 }
 
 struct tcp_pcb* tcp_listen(void* pcb) {
     UNUSED(pcb);
+
+    dprint("tcp_listen\n");
     return NULL;
 }
 
 void tcp_accept(void* pcb, tcp_accept_fn arg) {
     UNUSED(pcb);
     UNUSED(arg);
+
+    dprint("tcp_accept\n");
 }
 
 void tcp_arg(void* pcb, void* arg) {
     UNUSED(pcb);
     UNUSED(arg);
+
+    dprint("tcp_arg\n");
 }
 void tcp_sent(void* pcb, void* arg) {
     UNUSED(pcb);
     UNUSED(arg);
+
+    dprint("tcp_sent\n");
 }
 void tcp_recv(void* pcb, tcp_recv_fn arg) {
     UNUSED(pcb);
     UNUSED(arg);
+
+    dprint("tcp_recv\n");
 }
 
 void tcp_setprio(void* pcb, uint8_t prio) {
     UNUSED(pcb);
     UNUSED(prio);
+
+    dprint("tcp_setprio\n");
 }
 void tcp_err(void* pcb, void* err) {
     UNUSED(pcb);
     UNUSED(err);
+
+    dprint("tcp_err\n");
 }
 void tcp_poll(void* pcb, void* poll, uint8_t interval) {
     UNUSED(pcb);
     UNUSED(poll);
     UNUSED(interval);
+
+    dprint("tcp_poll\n");
 }
 
-void tcp_close(void* pcb) {
+err_t tcp_close(void* pcb) {
     UNUSED(pcb);
+
+    dprint("tcp_close\n");
+    return ERR_OK;
 }
 
 void tcp_write(void* pcb, const char* data, size_t len, uint8_t apiflags) {
@@ -112,23 +153,44 @@ void tcp_write(void* pcb, const char* data, size_t len, uint8_t apiflags) {
     UNUSED(apiflags);
     UNUSED(len);
 
-    printf("tcp_write:\n%s\n", data);
+    dprint("tcp_write:\n");
+    printf("%s\n", data);
 }
 
 void tcp_output(void* pcb) {
     UNUSED(pcb);
+
+    dprint("tcp_output\n");
 }
 
 void tcp_recved(void* pcb, uint16_t len) {
     UNUSED(pcb);
     UNUSED(len);
+
+    dprint("tcp_recved\n");
 }
 
 size_t tcp_sndbuf(void* pcb) {
     UNUSED(pcb);
+
+    dprint("tcp_sndbuf\n");
     return 0;
 }
 
 void pbuf_free(struct pbuf* p) {
     UNUSED(p);
+
+    dprint("pbuf_free\n");
+}
+
+uint16_t pbuf_copy_partial(const struct pbuf* buf, void* dataptr, uint16_t len, uint16_t offset) {
+    memcpy(dataptr, ((uint8_t*)(buf->payload)) + offset, len);
+    return len;
+}
+
+const char* lwip_strerr(err_t err) {
+    UNUSED(err);
+
+    dprint("lwip_sterr\n");
+    return NULL;
 }

tests/mocs.h

@@ -13,6 +13,15 @@ extern "C" {
 
 #define LWIP_UNUSED_ARG(x) UNUSED(x)
 
+typedef void sFONT;
+
+#define LCD_COLOR_BLACK 0
+#define LCD_COLOR_WHITE 1
+#define LCD_TRANSPARENT 2
+
+#define LCD_FONT16 0
+#define LCD_FONT24 (void*)1
+
 typedef int8_t err_t;
 /** Definitions for error constants. */
 typedef enum {
@@ -111,7 +120,7 @@ void tcp_recv(void* pcb, tcp_recv_fn arg);
 void tcp_setprio(void* pcb, uint8_t prio);
 void tcp_err(void* pcb, void* err);
 void tcp_poll(void* pcb, void* poll, uint8_t interval);
-void tcp_close(void* pcb);
+err_t tcp_close(void* pcb);
 
 void tcp_write(void* pcb, const char* data, size_t len, uint8_t apiflags);
 void tcp_output(void* pcb);
@@ -120,6 +129,10 @@ void pbuf_free(struct pbuf* p);
 
 size_t tcp_sndbuf(void* pcb);
 
+uint16_t pbuf_copy_partial(const struct pbuf* buf, void* dataptr, uint16_t len, uint16_t offset);
+
+const char* lwip_strerr(err_t err);
+
 #ifdef __cplusplus
 }
 #endif

tests/tcp_cmd.cpp

@@ -0,0 +1,252 @@
+#include <gtest/gtest.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "mocs.h"
+extern "C" {
+#include "tcp_cmd.h"
+}
+
+TEST(TCP_CMD, tcp_cmd_remove_newline) {
+    char* cmd = (char*)calloc(50, 1);
+    strcpy(cmd, "help\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "help");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "help\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\nhelp\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n\n\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n\n\n\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n\n\n\n\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n\n\n\n\n\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhelp\n\n\n\n\n\n\n\n\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+    strcpy(cmd, "\n\nhel\np\n");
+    tcp_cmd_remove_newline(cmd, strlen(cmd));
+    EXPECT_STREQ(cmd, "help");
+
+    free(cmd);
+}
+
+TEST(TCP_CMD, tcp_cmd_remove_leading_space) {
+    char* cmd = (char*)calloc(50, 1);
+    strcpy(cmd, "help");
+    EXPECT_STREQ(tcp_cmd_remove_leading_space(cmd, strlen(cmd)), "help");
+    strcpy(cmd, " help");
+    EXPECT_STREQ(tcp_cmd_remove_leading_space(cmd, strlen(cmd)), "help");
+    strcpy(cmd, "  help");
+    EXPECT_STREQ(tcp_cmd_remove_leading_space(cmd, strlen(cmd)), "help");
+    strcpy(cmd, "   help");
+    EXPECT_STREQ(tcp_cmd_remove_leading_space(cmd, strlen(cmd)), "help");
+    strcpy(cmd, "    help");
+    EXPECT_STREQ(tcp_cmd_remove_leading_space(cmd, strlen(cmd)), "help");
+    strcpy(cmd, "\n\t\r\n     help");
+    EXPECT_STREQ(tcp_cmd_remove_leading_space(cmd, strlen(cmd)), "help");
+}
+
+TEST(TCP_CMD, tcp_data_cb) {
+    char* cmd = (char*)calloc(50, 1);
+    std::string output;
+    pbuf p = {.next = NULL, .payload = (void*)cmd, .tot_len = 4, .len = 0, .type_internal = 0, .flags = 0, .if_idx = 0};
+    strcpy(cmd, "help");
+    p.tot_len = (uint16_t)strlen(cmd);
+
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output,   "help                        : shows a list of commands\n"
+                        "clear text/images           : clears the text or images on the lcd\n"
+                        "text \"<text>\"               : puts text on the lcd\n"
+                        "bgColor (<a>) <r> <g> <b>   : set the background color of the lcd\n"
+                        "color (<a>) <r> <g> <b>     : set the color of the text\n"
+                        "listFiles                   : shows a list with images in the filesystem\n"
+                        "setImage <image_name>       : put an image on the screen\n"
+                        "setGif   <image_name>       : put a gif on the screen\n"
+                        "exit                        : closes the connection\n\n$>\n");
+    #else
+    EXPECT_EQ(output,   "tcp_recved\ntcp_write:\n"
+                        "help                        : shows a list of commands\n"
+                        "clear text/images           : clears the text or images on the lcd\n"
+                        "text \"<text>\"               : puts text on the lcd\n"
+                        "bgColor (<a>) <r> <g> <b>   : set the background color of the lcd\n"
+                        "color (<a>) <r> <g> <b>     : set the color of the text\n"
+                        "listFiles                   : shows a list with images in the filesystem\n"
+                        "setImage <image_name>       : put an image on the screen\n"
+                        "setGif   <image_name>       : put a gif on the screen\n"
+                        "exit                        : closes the connection\n"
+                        "\ntcp_output\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "text \"This is printed on the display\"");
+    p.tot_len = (uint16_t)strlen(cmd);
+
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\nlcd_display_text @ 10 10 with color 0xFFFFFFFF bg color 0xFF000000\nThis is printed on the display\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "color 0x555555");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "bgcolor 0xAAAAAA");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "text \"This is printed on the display\"");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\nlcd_display_text @ 10 10 with color 0x00555555 bg color 0x00AAAAAA\nThis is printed on the display\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "color 255 255 255");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "bgcolor 255 255 255");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "text \"This is printed on the display\"");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\nlcd_display_text @ 10 10 with color 0xFFFFFFFF bg color 0xFFFFFFFF\nThis is printed on the display\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "setImage \"test.bmp\"");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\nlcd_draw_img_from_fs\ntest.bmp @0 0\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "setImage \"test.gif\"");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "[Warning]  (       0) [tcp_cmd]: setimage: File is not a bmp\r\nFile is not a bmp\n\n$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\nlogger_get_timestamp\n[Warning]  (       0) [tcp_cmd]: setimage: File is not a bmp\r\ntcp_write:\nFile is not a bmp\n\ntcp_output\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "setGif \"test.gif\"");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\nlcd_draw_gif_from_fs\ntest.gif @0 0\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "setGif \"test.bmp\"");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "[Warning]  (       0) [tcp_cmd]: setgif: File is not a gif\r\nFile is not a gif\n\n$>\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\nlogger_get_timestamp\n[Warning]  (       0) [tcp_cmd]: setgif: File is not a gif\r\ntcp_write:\nFile is not a gif\n\ntcp_output\ntcp_write:\n$>\ntcp_output\npbuf_free\n");
+    #endif
+
+    strcpy(cmd, "exit");
+    p.tot_len = (uint16_t)strlen(cmd);
+    
+    testing::internal::CaptureStdout();
+    tcp_cmd_recv(NULL, NULL, &p, ERR_OK);
+    output = testing::internal::GetCapturedStdout();
+    #ifndef DEBUG
+    EXPECT_EQ(output, "Exiting...\n\n");
+    #else
+    EXPECT_EQ(output, "tcp_recved\ntcp_write:\nExiting...\n\ntcp_output\nlcd_clear_images\nlcd_clear_text\ntcp_close\ntcp_arg\ntcp_sent\ntcp_recv\ntcp_close\npbuf_free\n");
+    #endif
+
+    free(cmd);
+}