From 4181b98f839cf64a9565c15c5ef5300543b2dcaa Mon Sep 17 00:00:00 2001 From: L-diy Date: Wed, 29 Nov 2023 14:04:32 +0100 Subject: [PATCH] Improve the Modbus TCP server The 'modbus_tcp.c' file has been extensively rewritten to handle parsing TCP Application Data Units (ADUs), handling incoming Modbus requests, and generating appropriate responses. The modbus tcp server now interacts better with the LWIP TCP functionality, due to better utilization of the callback functions. temp: modbus refactor --- project/Core/Inc/modbus_tcp.h | 19 +- project/Core/Src/modbus_tcp.c | 705 +++++++++++++++++++++++++++------- 2 files changed, 574 insertions(+), 150 deletions(-) diff --git a/project/Core/Inc/modbus_tcp.h b/project/Core/Inc/modbus_tcp.h index 1ea1bf8..9d65930 100644 --- a/project/Core/Inc/modbus_tcp.h +++ b/project/Core/Inc/modbus_tcp.h @@ -1,27 +1,20 @@ /** * @file modbus_tcp.h - * - * @brief TCP Modbus handler - * @date Nov 6, 2023 + * @brief TCP Modbus server + * @date Nov 29, 2023 * @author Obe + * @author Lorenz C. */ #ifndef INC_MODBUS_H_ #define INC_MODBUS_H_ -#define MODBUSPORT 502 // 502 is the default +#define MODBUS_TCP_PORT 502 -#include -#include -#include -#include -#include "lcd_api.h" -#include "llfs.h" /** - * @fn void modbus_init - * @brief Initializes the modbus tcp + * @brief Initializes the modbus tcp server */ -void modbus_init(void); +void modbus_tcp_init(void); #endif /* INC_MODBUS_H_ */ diff --git a/project/Core/Src/modbus_tcp.c b/project/Core/Src/modbus_tcp.c index fd06279..a845789 100644 --- a/project/Core/Src/modbus_tcp.c +++ b/project/Core/Src/modbus_tcp.c @@ -1,163 +1,594 @@ /** * @file modbus_tcp.c - * - * @brief TCP Modbus handler - * @date Nov 6, 2023 + * @brief TCP Modbus server + * @date Nov 29, 2023 * @author Obe + * @author Lorenz C. */ -// Includes -#include "modbus_tcp.h" +#include +#include +#include +#include +#include "lcd_api.h" +#include "llfs.h" +#define LOGGER_LEVEL_ALL #include "log.h" +#include "modbus_tcp.h" -// 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 +// TCP server constants +#define TCP_POLL_INTERVAL 10 // About 5 seconds -#define REG_COLOR_B_RED 14 -#define REG_COLOR_B_GREEN 16 -#define REG_COLOR_B_BLUE 18 +// 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_F_RED 20 -#define REG_COLOR_F_GREEN 22 -#define REG_COLOR_F_BLUE 24 +#define MBAP_HEADER_LENGTH 7 +#define PROTOCOL_ID_MODBUS 0x0000 -#define REG_IMAGE_NR 26 +#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 -// Global variables -static char* TAG = "Modbus_TCP"; // Tag used in logs +#define EXCEPTION_OFFSET 0x80 -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); +// 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 /** - * @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 + * @brief Error codes for internal use in the modbus tcp server. */ -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; - - LWIP_UNUSED_ARG(arg); // This is used to prevent a warning - - // Putting underscores in the whole array - memset(text, '_', TEXT_LENGTH); - text[TEXT_LENGTH - 1] = '\0'; - - if (p != NULL) { - LOG_INFO(TAG, "data is valid\n"); - // Process the modbus data - tcp_recved(pcb, p->tot_len); - - // 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]; - } - - 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"); - - 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]); - - counter = 0; - for (int i = START_DATA; i < REG_LENGTH; i++) { - if (i % 2 == 0) { - text[counter] = registers[i]; - counter++; - } - } - - 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]; - - 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]; - - // Processing the image index - size_t number_of_files = llfs_file_count(); // How many files that there are - - 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); - - lcd_clear_text(); - lcd_clear_images(); - lcd_stop_all_gifs(); - - lcd_display_text(text, 10, 10, text_foreground_color, result_background, LCD_FONT24); - - 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); - } - } - } - - } else { - LOG_INFO(TAG, "not in writing multiple register mode!!!\n"); - } - } else if (err == ERR_OK) { - tcp_close(pcb); // When everything was ok close the TCP connection - } - return ERR_OK; -} +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; /** - * @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 + * @brief Modbus function codes */ -static err_t modbus_accept(void* arg, struct tcp_pcb* pcb, err_t err) { - LWIP_UNUSED_ARG(arg); - LWIP_UNUSED_ARG(err); - - // Sets the priority of a connection. - tcp_setprio(pcb, TCP_PRIO_MIN); - - // Sets which function is being called when new data arrives - tcp_recv(pcb, modbus_incoming_data); - - return ERR_OK; -} +enum { + WRITE_MULTIPLE_REGISTERS = 0x10, +}; /** - * @fn void modbus_init - * @brief Initializes the modbus tcp + * @brief Modbus exception codes */ -void modbus_init(void) { - LOG_INFO(TAG, "Initializing"); - // Creating a new tcp pcb +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; + +/** + * @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; + +/** + * @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; + } - // Bind the modbus_pcb to port 502 - tcp_bind(modbus_pcb, IP_ADDR_ANY, MODBUSPORT); + // 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); - // Set callback function for incoming connections - tcp_accept(modbus_pcb, modbus_accept); - LOG_INFO(TAG, "initialized"); + 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; + } + 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; +} + +/** + * @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 + + // 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; + + // 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; + } + + rsp_adu->data[0] = exception_code; +} + +/** + * @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; + + 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; + } + + LOG_DEBUG(TAG, "Sending modbus response with length: %d", adu_length); + + // 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; + + // 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 (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); + } + } else { + LOG_WARN(TAG, "No image found"); + } +} + +/** + * @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 + */ +static const char* img_num_to_filename(uint16_t img_num) { + size_t number_of_files = llfs_file_count(); + + LOG_DEBUG(TAG, "Converting image number %d to filename, %d files found", img_num, number_of_files); + + 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; + } + + llfs_file_t files[number_of_files]; + llfs_file_list(files, number_of_files, NULL); + return files[img_num].name; +} \ No newline at end of file