#include "modbus_tcp.h" #include "modbus_memory.h" #include /* Modbus spec limits */ #define MODBUS_MAX_READ_BITS 2000 #define MODBUS_MAX_READ_REGS 125 #define MODBUS_MAX_WRITE_REGS 123 #define MODBUS_MAX_WRITE_COILS 1968 static uint16_t read_u16_be(const uint8_t *p) { return (uint16_t)(((uint16_t)p[0] << 8) | (uint16_t)p[1]); } static void write_u16_be(uint8_t *p, uint16_t value) { p[0] = (uint8_t)((value >> 8) & 0xFFU); p[1] = (uint8_t)(value & 0xFFU); } static bool parse_mbap(const uint8_t *req, size_t req_len, modbus_mbap_t *mbap) { if (req == NULL || mbap == NULL) return false; if (req_len < MODBUS_TCP_MBAP_LEN) return false; mbap->transaction_id = read_u16_be(&req[0]); mbap->protocol_id = read_u16_be(&req[2]); mbap->length = read_u16_be(&req[4]); mbap->unit_id = req[6]; if (mbap->protocol_id != 0U) return false; /* length includes unit id + PDU bytes */ if (mbap->length < 2U) return false; if ((size_t)(6U + mbap->length) != req_len) return false; return true; } static bool build_mbap(uint8_t *resp, size_t resp_max_len, const modbus_mbap_t *req_mbap, uint16_t pdu_len, size_t *adu_len) { uint16_t length; if (resp == NULL || req_mbap == NULL || adu_len == NULL) return false; length = (uint16_t)(1U + pdu_len); /* unit_id + pdu */ if ((size_t)(MODBUS_TCP_MBAP_LEN + pdu_len) > resp_max_len) return false; write_u16_be(&resp[0], req_mbap->transaction_id); write_u16_be(&resp[2], 0U); write_u16_be(&resp[4], length); resp[6] = req_mbap->unit_id; *adu_len = (size_t)MODBUS_TCP_MBAP_LEN + (size_t)pdu_len; return true; } static bool build_exception_response(const modbus_mbap_t *mbap, uint8_t function_code, uint8_t exception_code, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { size_t adu_len; if (!build_mbap(resp, resp_max_len, mbap, 2U, &adu_len)) return false; resp[7] = (uint8_t)(function_code | 0x80U); resp[8] = exception_code; *resp_len = adu_len; return true; } static uint8_t get_bit_packed_byte_count(uint16_t quantity) { return (uint8_t)((quantity + 7U) / 8U); } static bool handle_read_bits(modbus_memory_t *mem, const modbus_mbap_t *mbap, uint8_t function_code, modbus_mem_type_t mem_type, const uint8_t *pdu, size_t pdu_len, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { uint16_t start_addr; uint16_t quantity; uint8_t byte_count; uint8_t bits[MODBUS_MAX_READ_BITS]; size_t adu_len; if (pdu == NULL || pdu_len != 5U) { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } start_addr = read_u16_be(&pdu[1]); quantity = read_u16_be(&pdu[3]); if (quantity == 0U || quantity > MODBUS_MAX_READ_BITS) { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } if (!modbus_memory_valid_bit_range(mem, mem_type, start_addr, quantity)) { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_DATA_ADDRESS, resp, resp_max_len, resp_len); } if (mem_type == MB_MEM_COIL) { if (!modbus_memory_read_coils(mem, start_addr, quantity, bits)) { return build_exception_response(mbap, function_code, MODBUS_EX_SERVER_DEVICE_FAILURE, resp, resp_max_len, resp_len); } } else if (mem_type == MB_MEM_DISCRETE_INPUT) { if (!modbus_memory_read_discrete_inputs(mem, start_addr, quantity, bits)) { return build_exception_response(mbap, function_code, MODBUS_EX_SERVER_DEVICE_FAILURE, resp, resp_max_len, resp_len); } } else { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_FUNCTION, resp, resp_max_len, resp_len); } byte_count = get_bit_packed_byte_count(quantity); if (!build_mbap(resp, resp_max_len, mbap, (uint16_t)(2U + byte_count), &adu_len)) return false; resp[7] = function_code; resp[8] = byte_count; memset(&resp[9], 0, byte_count); for (uint16_t i = 0; i < quantity; i++) { if (bits[i] != 0U) resp[9 + (i / 8U)] |= (uint8_t)(1U << (i % 8U)); } *resp_len = adu_len; return true; } static bool handle_read_registers(modbus_memory_t *mem, const modbus_mbap_t *mbap, uint8_t function_code, modbus_mem_type_t mem_type, const uint8_t *pdu, size_t pdu_len, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { uint16_t start_addr; uint16_t quantity; uint16_t regs[MODBUS_MAX_READ_REGS]; uint8_t byte_count; size_t adu_len; if (pdu == NULL || pdu_len != 5U) { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } start_addr = read_u16_be(&pdu[1]); quantity = read_u16_be(&pdu[3]); if (quantity == 0U || quantity > MODBUS_MAX_READ_REGS) { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } if (!modbus_memory_valid_reg_range(mem, mem_type, start_addr, quantity)) { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_DATA_ADDRESS, resp, resp_max_len, resp_len); } if (mem_type == MB_MEM_HOLDING_REGISTER) { if (!modbus_memory_read_holding_registers(mem, start_addr, quantity, regs)) { return build_exception_response(mbap, function_code, MODBUS_EX_SERVER_DEVICE_FAILURE, resp, resp_max_len, resp_len); } } else if (mem_type == MB_MEM_INPUT_REGISTER) { if (!modbus_memory_read_input_registers(mem, start_addr, quantity, regs)) { return build_exception_response(mbap, function_code, MODBUS_EX_SERVER_DEVICE_FAILURE, resp, resp_max_len, resp_len); } } else { return build_exception_response(mbap, function_code, MODBUS_EX_ILLEGAL_FUNCTION, resp, resp_max_len, resp_len); } byte_count = (uint8_t)(quantity * 2U); if (!build_mbap(resp, resp_max_len, mbap, (uint16_t)(2U + byte_count), &adu_len)) return false; resp[7] = function_code; resp[8] = byte_count; for (uint16_t i = 0; i < quantity; i++) write_u16_be(&resp[9 + ((size_t)i * 2U)], regs[i]); *resp_len = adu_len; return true; } static bool handle_write_single_coil(modbus_memory_t *mem, const modbus_mbap_t *mbap, const uint8_t *pdu, size_t pdu_len, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { uint16_t addr; uint16_t value; size_t adu_len; uint8_t bit_value; if (pdu == NULL || pdu_len != 5U) { return build_exception_response(mbap, MODBUS_FC_WRITE_SINGLE_COIL, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } addr = read_u16_be(&pdu[1]); value = read_u16_be(&pdu[3]); if (value == 0xFF00U) bit_value = 1U; else if (value == 0x0000U) bit_value = 0U; else { return build_exception_response(mbap, MODBUS_FC_WRITE_SINGLE_COIL, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } if (!modbus_memory_write_coil(mem, addr, bit_value)) { return build_exception_response(mbap, MODBUS_FC_WRITE_SINGLE_COIL, MODBUS_EX_ILLEGAL_DATA_ADDRESS, resp, resp_max_len, resp_len); } if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len)) return false; memcpy(&resp[7], pdu, 5U); *resp_len = adu_len; return true; } static bool handle_write_single_register(modbus_memory_t *mem, const modbus_mbap_t *mbap, const uint8_t *pdu, size_t pdu_len, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { uint16_t addr; uint16_t value; size_t adu_len; if (pdu == NULL || pdu_len != 5U) { return build_exception_response(mbap, MODBUS_FC_WRITE_SINGLE_REGISTER, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } addr = read_u16_be(&pdu[1]); value = read_u16_be(&pdu[3]); if (!modbus_memory_write_holding_register(mem, addr, value)) { return build_exception_response(mbap, MODBUS_FC_WRITE_SINGLE_REGISTER, MODBUS_EX_ILLEGAL_DATA_ADDRESS, resp, resp_max_len, resp_len); } if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len)) return false; memcpy(&resp[7], pdu, 5U); *resp_len = adu_len; return true; } static bool handle_write_multiple_coils(modbus_memory_t *mem, const modbus_mbap_t *mbap, const uint8_t *pdu, size_t pdu_len, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { uint16_t start_addr; uint16_t quantity; uint8_t byte_count; uint8_t bits[MODBUS_MAX_WRITE_COILS]; size_t adu_len; size_t expected_pdu_len; if (pdu == NULL || pdu_len < 6U) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_COILS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } start_addr = read_u16_be(&pdu[1]); quantity = read_u16_be(&pdu[3]); byte_count = pdu[5]; if (quantity == 0U || quantity > MODBUS_MAX_WRITE_COILS) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_COILS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } if (byte_count != get_bit_packed_byte_count(quantity)) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_COILS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } expected_pdu_len = (size_t)(6U + byte_count); if (pdu_len != expected_pdu_len) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_COILS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } if (!modbus_memory_valid_bit_range(mem, MB_MEM_COIL, start_addr, quantity)) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_COILS, MODBUS_EX_ILLEGAL_DATA_ADDRESS, resp, resp_max_len, resp_len); } for (uint16_t i = 0; i < quantity; i++) bits[i] = (uint8_t)((pdu[6 + (i / 8U)] >> (i % 8U)) & 0x01U); if (!modbus_memory_write_coils(mem, start_addr, quantity, bits)) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_COILS, MODBUS_EX_SERVER_DEVICE_FAILURE, resp, resp_max_len, resp_len); } if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len)) return false; resp[7] = MODBUS_FC_WRITE_MULTIPLE_COILS; write_u16_be(&resp[8], start_addr); write_u16_be(&resp[10], quantity); *resp_len = adu_len; return true; } static bool handle_write_multiple_registers(modbus_memory_t *mem, const modbus_mbap_t *mbap, const uint8_t *pdu, size_t pdu_len, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { uint16_t start_addr; uint16_t quantity; uint8_t byte_count; uint16_t regs[MODBUS_MAX_WRITE_REGS]; size_t adu_len; size_t expected_pdu_len; if (pdu == NULL || pdu_len < 6U) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } start_addr = read_u16_be(&pdu[1]); quantity = read_u16_be(&pdu[3]); byte_count = pdu[5]; if (quantity == 0U || quantity > MODBUS_MAX_WRITE_REGS) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } if (byte_count != (uint8_t)(quantity * 2U)) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } expected_pdu_len = (size_t)(6U + byte_count); if (pdu_len != expected_pdu_len) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, MODBUS_EX_ILLEGAL_DATA_VALUE, resp, resp_max_len, resp_len); } if (!modbus_memory_valid_reg_range(mem, MB_MEM_HOLDING_REGISTER, start_addr, quantity)) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, MODBUS_EX_ILLEGAL_DATA_ADDRESS, resp, resp_max_len, resp_len); } for (uint16_t i = 0; i < quantity; i++) regs[i] = read_u16_be(&pdu[6 + ((size_t)i * 2U)]); if (!modbus_memory_write_holding_registers(mem, start_addr, quantity, regs)) { return build_exception_response(mbap, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, MODBUS_EX_SERVER_DEVICE_FAILURE, resp, resp_max_len, resp_len); } if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len)) return false; resp[7] = MODBUS_FC_WRITE_MULTIPLE_REGISTERS; write_u16_be(&resp[8], start_addr); write_u16_be(&resp[10], quantity); *resp_len = adu_len; return true; } bool modbus_tcp_process_request(modbus_memory_t *mem, const uint8_t *req, size_t req_len, uint8_t *resp, size_t resp_max_len, size_t *resp_len) { modbus_mbap_t mbap; const uint8_t *pdu; size_t pdu_len; uint8_t function_code; if (req == NULL || resp == NULL || resp_len == NULL || mem == NULL) return false; *resp_len = 0U; if (!parse_mbap(req, req_len, &mbap)) return false; pdu = &req[7]; pdu_len = req_len - MODBUS_TCP_MBAP_LEN; if (pdu_len < 1U) return false; function_code = pdu[0]; switch (function_code) { case MODBUS_FC_READ_COILS: return handle_read_bits(mem, &mbap, function_code, MB_MEM_COIL, pdu, pdu_len, resp, resp_max_len, resp_len); case MODBUS_FC_READ_DISCRETE_INPUTS: return handle_read_bits(mem, &mbap, function_code, MB_MEM_DISCRETE_INPUT, pdu, pdu_len, resp, resp_max_len, resp_len); case MODBUS_FC_READ_HOLDING_REGISTERS: return handle_read_registers(mem, &mbap, function_code, MB_MEM_HOLDING_REGISTER, pdu, pdu_len, resp, resp_max_len, resp_len); case MODBUS_FC_READ_INPUT_REGISTERS: return handle_read_registers(mem, &mbap, function_code, MB_MEM_INPUT_REGISTER, pdu, pdu_len, resp, resp_max_len, resp_len); case MODBUS_FC_WRITE_SINGLE_COIL: return handle_write_single_coil(mem, &mbap, pdu, pdu_len, resp, resp_max_len, resp_len); case MODBUS_FC_WRITE_SINGLE_REGISTER: return handle_write_single_register(mem, &mbap, pdu, pdu_len, resp, resp_max_len, resp_len); case MODBUS_FC_WRITE_MULTIPLE_COILS: return handle_write_multiple_coils(mem, &mbap, pdu, pdu_len, resp, resp_max_len, resp_len); case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: return handle_write_multiple_registers(mem, &mbap, pdu, pdu_len, resp, resp_max_len, resp_len); default: return build_exception_response(&mbap, function_code, MODBUS_EX_ILLEGAL_FUNCTION, resp, resp_max_len, resp_len); } }