707 lines
25 KiB
C
707 lines
25 KiB
C
#include "modbus_tcp.h"
|
|
#include "modbus_memory.h"
|
|
|
|
#include <string.h>
|
|
|
|
/* 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);
|
|
}
|
|
} |