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