/* Send a response to the received request.
Analyses the request and constructs a response.
If an error occurs, this function construct the response
accordingly.
*/
int modbus_reply(modbus_t *ctx, const uint8_t *req,
int req_length, modbus_mapping_t *mb_mapping)
{
int offset = ctx->backend->header_length;
int slave = req[offset - 1];
int function = req[offset];
uint16_t address = (req[offset + 1] << 8) + req[offset + 2];
uint8_t rsp[MAX_MESSAGE_LENGTH];
int rsp_length = 0;
sft_t sft;
sft.slave = slave;
sft.function = function;
sft.t_id = ctx->backend->prepare_response_tid(req, &req_length);
switch (function) {
case _FC_READ_COILS: {
int nb = (req[offset + 3] << 8) + req[offset + 4];
if ((address + nb) > mb_mapping->nb_bits) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in read_bits\n",
address + nb);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
rsp_length = response_io_status(address, nb,
mb_mapping->tab_bits,
rsp, rsp_length);
}
}
break;
case _FC_READ_DISCRETE_INPUTS: {
/* Similar to coil status (but too many arguments to use a
* function) */
int nb = (req[offset + 3] << 8) + req[offset + 4];
if ((address + nb) > mb_mapping->nb_input_bits) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in read_input_bits\n",
address + nb);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
rsp_length = response_io_status(address, nb,
mb_mapping->tab_input_bits,
rsp, rsp_length);
}
}
break;
case _FC_READ_HOLDING_REGISTERS: {
int nb = (req[offset + 3] << 8) + req[offset + 4];
if ((address + nb) > mb_mapping->nb_registers) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in read_registers\n",
address + nb);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
int i;
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
rsp[rsp_length++] = nb << 1;
for (i = address; i < address + nb; i++) {
rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
}
}
}
break;
case _FC_READ_INPUT_REGISTERS: {
/* Similar to holding registers (but too many arguments to use a
* function) */
int nb = (req[offset + 3] << 8) + req[offset + 4];
if ((address + nb) > mb_mapping->nb_input_registers) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in read_input_registers\n",
address + nb);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
int i;
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
rsp[rsp_length++] = nb << 1;
for (i = address; i < address + nb; i++) {
rsp[rsp_length++] = mb_mapping->tab_input_registers[i] >> 8;
rsp[rsp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
}
}
}
break;
case _FC_WRITE_SINGLE_COIL:
if (address >= mb_mapping->nb_bits) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in write_bit\n",
address);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
int data = (req[offset + 3] << 8) + req[offset + 4];
if (data == 0xFF00 || data == 0x0) {
mb_mapping->tab_bits[address] = (data) ? ON : OFF;
memcpy(rsp, req, req_length);
rsp_length = req_length;
} else {
if (ctx->debug) {
fprintf(stderr,
"Illegal data value %0X in write_bit request at address %0X\n",
data, address);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
}
}
break;
case _FC_WRITE_SINGLE_REGISTER:
if (address >= mb_mapping->nb_registers) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in write_register\n",
address);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
int data = (req[offset + 3] << 8) + req[offset + 4];
mb_mapping->tab_registers[address] = data;
memcpy(rsp, req, req_length);
rsp_length = req_length;
}
break;
case _FC_WRITE_MULTIPLE_COILS: {
int nb = (req[offset + 3] << 8) + req[offset + 4];
if ((address + nb) > mb_mapping->nb_bits) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in write_bits\n",
address + nb);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
/* 6 = byte count */
modbus_set_bits_from_bytes(mb_mapping->tab_bits, address, nb, &req[offset + 6]);
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
/* 4 to copy the bit address (2) and the quantity of bits */
memcpy(rsp + rsp_length, req + rsp_length, 4);
rsp_length += 4;
}
}
break;
case _FC_WRITE_MULTIPLE_REGISTERS: {
int nb = (req[offset + 3] << 8) + req[offset + 4];
if ((address + nb) > mb_mapping->nb_registers) {
if (ctx->debug) {
fprintf(stderr, "Illegal data address %0X in write_registers\n",
address + nb);
}
rsp_length = response_exception(
ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
int i, j;
for (i = address, j = 6; i < address + nb; i++, j += 2) {
/* 6 and 7 = first value */
mb_mapping->tab_registers[i] =
(req[offset + j] << 8) + req[offset + j + 1];
}
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
/* 4 to copy the address (2) and the no. of registers */
memcpy(rsp + rsp_length, req + rsp_length, 4);
rsp_length += 4;
}
}
break;
case _FC_REPORT_SLAVE_ID: {
int str_len;
int byte_count_pos;
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
/* Skip byte count for now */
byte_count_pos = rsp_length++;
rsp[rsp_length++] = _REPORT_SLAVE_ID;
/* Run indicator status to ON */
rsp[rsp_length++] = 0xFF;
/* LMB + length of LIBMODBUS_VERSION_STRING */
str_len = 3 + strlen(LIBMODBUS_VERSION_STRING);
memcpy(rsp + rsp_length, "LMB" LIBMODBUS_VERSION_STRING, str_len);
rsp_length += str_len;
rsp[byte_count_pos] = rsp_length - byte_count_pos - 1;
}
break;
case _FC_READ_EXCEPTION_STATUS:
if (ctx->debug) {
fprintf(stderr, "FIXME Not implemented\n");
}
errno = ENOPROTOOPT;
return -1;
break;
case _FC_WRITE_AND_READ_REGISTERS: {
int nb = (req[offset + 3] << 8) + req[offset + 4];
uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6];
int nb_write = (req[offset + 7] << 8) + req[offset + 8];
if ((address + nb) > mb_mapping->nb_registers ||
(address_write + nb_write) > mb_mapping->nb_registers) {
if (ctx->debug) {
fprintf(stderr,
"Illegal data read address %0X or write address %0X write_and_read_registers\n",
address + nb, address_write + nb_write);
}
rsp_length = response_exception(ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
} else {
int i, j;
rsp_length = ctx->backend->build_response_basis(&sft, rsp);
rsp[rsp_length++] = nb << 1;
/* Write first.
10 and 11 are the offset of the first values to write */
for (i = address_write, j = 10; i < address_write + nb_write; i++, j += 2) {
mb_mapping->tab_registers[i] =
(req[offset + j] << 8) + req[offset + j + 1];
}
/* and read the data for the response */
for (i = address; i < address + nb; i++) {
rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
}
}
}
break;
default:
rsp_length = response_exception(ctx, &sft,
MODBUS_EXCEPTION_ILLEGAL_FUNCTION,
rsp);
break;
}
return send_msg(ctx, rsp, rsp_length);
}
void modbus_slave_manage(const uint8_t *query, int query_length) {
int offset = HEADER_LENGTH_RTU;
int resp_length = 0;
int data, i;
sft_t sft;
uint8_t response[MAX_ADU_LENGTH_RTU];
sft.slave = query[offset - 1];
sft.function = query[offset];
sft.address = (query[offset + 1] << 8) + query[offset + 2];
if (sft.slave != MODBUS_SLAVE_ID && sft.slave != MODBUS_BROADCAST_ADDRESS)
{
// Ignores the query (not for me)
return;
}
query_length -= CHECKSUM_LENGTH_RTU;
switch (sft.function)
{
case FC_READ_COIL_STATUS:
sft.nb = (query[offset + 3] << 8) + query[offset + 4];
if ((sft.address / 8 + ((sft.nb)?1:0)) > MODBUS_MAX_BITADDRESSES) {
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS,response);
} else {
resp_length = build_response_basis_rtu(&sft, response);
// calculate amount of bytes to return
response[resp_length++] = (sft.nb / 8) + ((sft.nb % 8) ? 1 : 0);
// give him the bit pattern of coils requested
resp_length = response_io_status(
&sft,
((uint8_t*)modbus_bytes),
response,
resp_length);
}
break;
case FC_READ_INPUT_STATUS:
sft.nb = (query[offset + 3] << 8) + query[offset + 4];
if ((sft.address / 8 + ((sft.nb)?1:0)) > MODBUS_MAX_BITADDRESSES) {
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS,response);
} else {
resp_length = build_response_basis_rtu(&sft, response);
// calculate amount of bytes to return
response[resp_length++] = (sft.nb / 8) + ((sft.nb % 8) ? 1 : 0);
// give him the bit pattern of coils requested
resp_length = response_io_status(
&sft,
((uint8_t*)modbus_bytes),
response,
resp_length);
}
break;
case FC_READ_HOLDING_REGISTERS:
sft.nb = (query[offset + 3] << 8) + query[offset + 4];
if ((sft.address + sft.nb) >= MODBUS_MAX_WORDADDRESSES) {
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS,response);
} else {
resp_length = build_response_basis_rtu(&sft, response);
response[resp_length++] = sft.nb << 1;
for (i = (sft.address); i < (sft.address) + sft.nb; i++) {
response[resp_length++] = modbus_words[i] >> 8;
response[resp_length++] = modbus_words[i] & 0xFF;
}
}
break;
case FC_READ_INPUT_REGISTERS:
sft.nb = (query[offset + 3] << 8) + query[offset + 4];
if ((sft.address + sft.nb) > MODBUS_MAX_WORDADDRESSES) {
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS,response);
} else {
resp_length = build_response_basis_rtu(&sft, response);
response[resp_length++] = sft.nb << 1;
}
break;
case FC_FORCE_SINGLE_COIL:
data = (query[offset + 3] << 8) + query[offset + 4];
if (sft.address > MODBUS_MAX_BITADDRESSES) {
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS,
response);
} else {
if (data == 0xFF00 || data == 0x00) {
changedBit.cBit = sft.address;
if (data) {
changedBit.cVal = TRUE;
((uint8_t*)modbus_bytes)[(uint8_t)(sft.address / 8)] |= (1 << (sft.address % 8));
} else {
changedBit.cVal = FALSE;
((uint8_t*)modbus_bytes)[(uint8_t)(sft.address / 8)] &= ~(1 << (sft.address % 8));
}
// In RTU mode, the CRC is computed and added
// to the query by modbus_send, the computed
// CRC will be same and optimization is
// (FIXME) possible here .
memcpy(response, query, query_length);
resp_length = query_length;
} else {
resp_length = response_exception(&sft, ILLEGAL_DATA_VALUE,
response);
changedBit.cBit = 99;
}
}
break;
case FC_PRESET_SINGLE_REGISTER:
data = (query[offset + 3] << 8) + query[offset + 4];
if (sft.address >= MODBUS_MAX_WORDADDRESSES) {
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS,
response);
} else {
if ((sft.address < MODBUS_MAX_WORDADDRESSES)){
modbus_words[sft.address] = data;
break;
}
memcpy(response, query, query_length);
resp_length = query_length;
}
break;
case FC_FORCE_MULTIPLE_COILS:
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS, response);
break;
case FC_PRESET_MULTIPLE_REGISTERS:
sft.nb = (query[offset + 3] << 8) + query[offset + 4];
if ((sft.address + sft.nb) > MODBUS_MAX_WORDADDRESSES) {
resp_length = response_exception(&sft, ILLEGAL_DATA_ADDRESS,
response);
} else {
int i, j;
for (i = sft.address, j = 6; i < sft.address + sft.nb; i++, j += 2) {
// 6 and 7 = first value
modbus_words[i] = (query[offset + j] << 8) + query[offset + j + 1];
}
resp_length = build_response_basis_rtu(&sft, response);
// 4 to copy the address (2) and the no. of registers
memcpy(response + resp_length, query + resp_length, 4);
resp_length += 4;
}
break;
case FC_READ_EXCEPTION_STATUS:
break;
case FC_REPORT_SLAVE_ID:
break;
}
modbus_send(response, resp_length);
}
