Jason Engelman
First Working Master with Adaptronic simulator
Protocol/modbus.c
- Committer:
- tecnosys
- Date:
- 2011-07-24
- Revision:
- 0:150538eb0bf3
File content as of revision 0:150538eb0bf3:
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>
#include <errno.h>
#include <limits.h>
#include "modbus.h"
#include "mbed.h"
extern Serial serial;
#define UNKNOWN_ERROR_MSG "Not defined in modbus specification"
/* This structure reduces the number of params in functions and so
* optimizes the speed of execution (~ 37%). */
typedef struct {
int slave;
int function;
int t_id;
} sft_t;
static const uint8_t NB_TAB_ERROR_MSG = 12;
static const char *TAB_ERROR_MSG[] = {
/* 0x00 */ UNKNOWN_ERROR_MSG,
/* 0x01 */ "Illegal function code",
/* 0x02 */ "Illegal data address",
/* 0x03 */ "Illegal data value",
/* 0x04 */ "Slave device or server failure",
/* 0x05 */ "Acknowledge",
/* 0x06 */ "Slave device or server busy",
/* 0x07 */ "Negative acknowledge",
/* 0x08 */ "Memory parity error",
/* 0x09 */ UNKNOWN_ERROR_MSG,
/* 0x0A */ "Gateway path unavailable",
/* 0x0B */ "Target device failed to respond"
};
/* Table of CRC values for high-order byte */
static uint8_t table_crc_hi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};
/* Table of CRC values for low-order byte */
static uint8_t table_crc_lo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};
/* Treats errors and flush or close connection if necessary */
static void error_treat(modbus_param_t *mb_param, int code, const char *string)
{
printf("\nERROR %s (%d)\n", string, code);
if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) {
switch (code) {
case ILLEGAL_DATA_VALUE:
case ILLEGAL_DATA_ADDRESS:
case ILLEGAL_FUNCTION:
break;
default:
// tcflush(mb_param->fd, TCIOFLUSH);
}
}
}
/* Computes the length of the expected response */
static unsigned int compute_response_length(modbus_param_t *mb_param,
uint8_t *query)
{
int length;
int offset;
offset = mb_param->header_length;
switch (query[offset + 1]) {
case FC_READ_COIL_STATUS:
case FC_READ_INPUT_STATUS: {
/* Header + nb values (code from force_multiple_coils) */
int nb = (query[offset + 4] << 8) | query[offset + 5];
length = 3 + (nb / 8) + ((nb % 8) ? 1 : 0);
}
break;
case FC_READ_HOLDING_REGISTERS:
case FC_READ_INPUT_REGISTERS:
/* Header + 2 * nb values */
length = 3 + 2 * (query[offset + 4] << 8 |
query[offset + 5]);
break;
case FC_READ_EXCEPTION_STATUS:
length = 4;
break;
default:
length = 6;
}
return length + offset + mb_param->checksum_length;
}
/* Builds a RTU query header */
int build_query_basis_rtu(int slave, int function,
int start_addr, int nb,
uint8_t *query)
{
query[0] = slave;
query[1] = function;
query[2] = start_addr >> 8;
query[3] = start_addr & 0x00ff;
query[4] = nb >> 8;
query[5] = nb & 0x00ff;
return PRESET_QUERY_LENGTH_RTU;
}
static int build_query_basis(modbus_param_t *mb_param, int slave,
int function, int start_addr,
int nb, uint8_t *query)
{
return build_query_basis_rtu(slave, function, start_addr,
nb, query);
}
/* Builds a RTU response header */
static int build_response_basis_rtu(sft_t *sft, uint8_t *response)
{
response[0] = sft->slave;
response[1] = sft->function;
return PRESET_RESPONSE_LENGTH_RTU;
}
static int build_response_basis(modbus_param_t *mb_param, sft_t *sft,
uint8_t *response)
{
return build_response_basis_rtu(sft, response);
}
/* Fast CRC */
static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length)
{
uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
unsigned int i; /* will index into CRC lookup */
/* pass through message buffer */
while (buffer_length--) {
i = crc_hi ^ *buffer++; /* calculate the CRC */
crc_hi = crc_lo ^ table_crc_hi[i];
crc_lo = table_crc_lo[i];
}
return (crc_hi << 8 | crc_lo);
}
/* If CRC is correct returns 0 else returns INVALID_CRC */
static int check_crc16(modbus_param_t *mb_param,
uint8_t *msg,
const int msg_length)
{
int ret;
uint16_t crc_calc;
uint16_t crc_received;
crc_calc = crc16(msg, msg_length - 2);
crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
/* Check CRC of msg */
if (crc_calc == crc_received) {
ret = 0;
} else {
char s_error[64];
sprintf(s_error,
"invalid crc received %0X - crc_calc %0X",
crc_received, crc_calc);
ret = INVALID_CRC;
error_treat(mb_param, ret, s_error);
}
return ret;
}
int modbus_send(modbus_param_t *mb_param, uint8_t *query,
int query_length)
{
int ret;
uint16_t s_crc;
int i;
s_crc = crc16(query, query_length);
query[query_length++] = s_crc >> 8;
query[query_length++] = s_crc & 0x00FF;
if (mb_param->debug) {
for (i = 0; i < query_length; i++)
printf("[%.2X]", query[i]);
printf("\n");
}
//mbed
for (i = 0; i < query_length; i++) serial.putc(query[i]);
// ret = write(mb_param->fd, query, query_length);
/* Return the number of bytes written (0 to n)
or PORT_SOCKET_FAILURE on error */
/*
if ((ret == -1) || (ret != query_length)) {
ret = PORT_SOCKET_FAILURE;
error_treat(mb_param, ret, "Write port/socket failure");
}
*/
return ret;
}
/* Computes the length of the header following the function code */
static uint8_t compute_query_length_header(int function)
{
int length;
if (function <= FC_FORCE_SINGLE_COIL ||
function == FC_PRESET_SINGLE_REGISTER)
/* Read and single write */
length = 4;
else if (function == FC_FORCE_MULTIPLE_COILS ||
function == FC_PRESET_MULTIPLE_REGISTERS)
/* Multiple write */
length = 5;
else
length = 0;
return length;
}
/* Computes the length of the data to write in the query */
static int compute_query_length_data(modbus_param_t *mb_param, uint8_t *msg)
{
int function = msg[mb_param->header_length + 1];
int length;
if (function == FC_FORCE_MULTIPLE_COILS ||
function == FC_PRESET_MULTIPLE_REGISTERS)
length = msg[mb_param->header_length + 6];
else
length = 0;
length += mb_param->checksum_length;
return length;
}
/* Waits a reply from a modbus slave or a query from a modbus master.
This function blocks for timeout seconds if there is no reply.
In
- msg_length_computed must be set to MSG_LENGTH_UNDEFINED if undefined
Out
- msg is an array of uint8_t to receive the message
- p_msg_length, the variable is assigned to the number of
characters received. This value won't be greater than
msg_length_computed.
Returns 0 in success or a negative value if an error occured.
*/
int receive_msg(modbus_param_t *mb_param,
int msg_length_computed,
uint8_t *msg, int *p_msg_length)
{
int select_ret;
int read_ret;
// fd_set rfds;
// struct timeval tv;
int length_to_read;
uint8_t *p_msg;
enum { FUNCTION, BYTE, COMPLETE };
int state;
if (mb_param->debug) {
if (msg_length_computed == MSG_LENGTH_UNDEFINED)
printf("Waiting for a message...\n");
else
printf("Waiting for a message (%d bytes)...\n",
msg_length_computed);
}
printf("0");
/* Add a file descriptor to the set */
//FD_ZERO(&rfds);
//FD_SET(mb_param->fd, &rfds);
if (msg_length_computed == MSG_LENGTH_UNDEFINED) {
/* Wait for a message */
//tv.tv_sec = 60;
//tv.tv_usec = 0;
/* The message length is undefined (query receiving) so
* we need to analyse the message step by step.
* At the first step, we want to reach the function
* code because all packets have that information. */
msg_length_computed = mb_param->header_length + 2;
state = FUNCTION;
} else {
// tv.tv_sec = 0;
// tv.tv_usec = TIME_OUT_BEGIN_OF_TRAME;
state = COMPLETE;
}
length_to_read = msg_length_computed;
select_ret = 1;
// WAIT_DATA();
/* Initialize the readin the message */
(*p_msg_length) = 0;
p_msg = msg;
while (select_ret) {
read_ret=0;
//read_ret = read(mb_param->fd, p_msg, length_to_read);
printf("1");
while(serial.readable()) {
p_msg[read_ret] = serial.getc();
printf("x");
read_ret++;
}
printf("2");
/* Sums bytes received */
(*p_msg_length) += read_ret;
/* Display the hex code of each character received */
if (mb_param->debug) {
int i;
for (i=0; i < read_ret; i++)
printf("<%.2X>", p_msg[i]);
}
if ((*p_msg_length) < msg_length_computed) {
/* Message incomplete */
length_to_read = msg_length_computed - (*p_msg_length);
} else {
switch (state) {
case FUNCTION:
/* Function code position */
length_to_read = compute_query_length_header(msg[mb_param->header_length + 1]);
msg_length_computed += length_to_read;
/* It's useless to check
p_msg_length_computed value in this
case (only defined values are used). */
state = BYTE;
break;
case BYTE:
length_to_read = compute_query_length_data(mb_param, msg);
msg_length_computed += length_to_read;
if (msg_length_computed > MAX_MESSAGE_LENGTH) {
error_treat(mb_param, TOO_MANY_DATA, "Too many data");
return TOO_MANY_DATA;
}
state = COMPLETE;
break;
case COMPLETE:
length_to_read = 0;
break;
}
}
/* Moves the pointer to receive other data */
p_msg = &(p_msg[read_ret]);
if (length_to_read > 0) {
/* If no character at the buffer wait
TIME_OUT_END_OF_TRAME before to generate an error. */
//tv.tv_sec = 0;
//tv.tv_usec = TIME_OUT_END_OF_TRAME;
//WAIT_DATA();
} else {
/* All chars are received */
select_ret = FALSE;
}
}
printf("8");
if (mb_param->debug)
printf("\n");
if (mb_param->type_com == RTU) {
printf("CRC");
check_crc16(mb_param, msg, (*p_msg_length));
}
/* OK */
return 0;
}
/* Receives the response and checks values (and checksum in RTU).
Returns:
- the number of values (bits or word) if success or the response
length if no value is returned
- less than 0 for exception errors
Note: all functions used to send or receive data with modbus return
these values. */
int modbus_receive(modbus_param_t *mb_param,
uint8_t *query,
uint8_t *response)
{
int ret;
int response_length;
int response_length_computed;
int offset = mb_param->header_length;
response_length_computed = compute_response_length(mb_param, query);
ret = receive_msg(mb_param, response_length_computed,
response, &response_length);
if (ret == 0) {
/* GOOD RESPONSE */
int query_nb_value;
int response_nb_value;
/* The number of values is returned if it's corresponding
* to the query */
switch (response[offset + 1]) {
case FC_READ_COIL_STATUS:
case FC_READ_INPUT_STATUS:
/* Read functions, 8 values in a byte (nb
* of values in the query and byte count in
* the response. */
query_nb_value = (query[offset+4] << 8) + query[offset+5];
query_nb_value = (query_nb_value / 8) + ((query_nb_value % 8) ? 1 : 0);
response_nb_value = response[offset + 2];
break;
case FC_READ_HOLDING_REGISTERS:
case FC_READ_INPUT_REGISTERS:
/* Read functions 1 value = 2 bytes */
query_nb_value = (query[offset+4] << 8) + query[offset+5];
response_nb_value = (response[offset + 2] / 2);
break;
case FC_FORCE_MULTIPLE_COILS:
case FC_PRESET_MULTIPLE_REGISTERS:
/* N Write functions */
query_nb_value = (query[offset+4] << 8) + query[offset+5];
response_nb_value = (response[offset + 4] << 8) | response[offset + 5];
break;
case FC_REPORT_SLAVE_ID:
/* Report slave ID (bytes received) */
query_nb_value = response_nb_value = response_length;
break;
default:
/* 1 Write functions & others */
query_nb_value = response_nb_value = 1;
}
if (query_nb_value == response_nb_value) {
ret = response_nb_value;
} else {
char *s_error = (char *)malloc(64 * sizeof(char));
sprintf(s_error, "Quantity (%d) not corresponding to the query (%d)",
response_nb_value, query_nb_value);
ret = ILLEGAL_DATA_VALUE;
error_treat(mb_param, ILLEGAL_DATA_VALUE, s_error);
free(s_error);
}
} else if (ret == COMM_TIME_OUT) {
if (response_length == (offset + 3 + mb_param->checksum_length)) {
/* EXCEPTION CODE RECEIVED */
/* Optimization allowed because exception response is
the smallest trame in modbus protocol (3) so always
raise a timeout error */
/* CRC must be checked here (not done in receive_msg) */
if (mb_param->type_com == RTU) {
ret = check_crc16(mb_param, response, response_length);
if (ret != 0)
return ret;
}
/* Check for exception response.
0x80 + function is stored in the exception
response. */
if (0x80 + query[offset + 1] == response[offset + 1]) {
int exception_code = response[offset + 2];
// FIXME check test
if (exception_code < NB_TAB_ERROR_MSG) {
error_treat(mb_param, -exception_code,
TAB_ERROR_MSG[response[offset + 2]]);
/* RETURN THE EXCEPTION CODE */
/* Modbus error code is negative */
return -exception_code;
} else {
/* The chances are low to hit this
case but it can avoid a vicious
segfault */
char *s_error = (char *)malloc(64 * sizeof(char));
sprintf(s_error,
"Invalid exception code %d",
response[offset + 2]);
error_treat(mb_param, INVALID_EXCEPTION_CODE,
s_error);
free(s_error);
return INVALID_EXCEPTION_CODE;
}
}
/* If doesn't return previously, return as
TIME OUT here */
}
/* COMMUNICATION TIME OUT */
error_treat(mb_param, ret, "Communication time out");
return ret;
}
return ret;
}
static int response_io_status(int address, int nb,
uint8_t *tab_io_status,
uint8_t *response, int offset)
{
int shift = 0;
int byte = 0;
int i;
for (i = address; i < address+nb; i++) {
byte |= tab_io_status[i] << shift;
if (shift == 7) {
/* Byte is full */
response[offset++] = byte;
byte = shift = 0;
} else {
shift++;
}
}
if (shift != 0)
response[offset++] = byte;
return offset;
}
/* Build the exception response */
static int response_exception(modbus_param_t *mb_param, sft_t *sft,
int exception_code, uint8_t *response)
{
int response_length;
sft->function = sft->function + 0x80;
response_length = build_response_basis(mb_param, sft, response);
/* Positive exception code */
response[response_length++] = -exception_code;
return response_length;
}
/* Manages the received query.
Analyses the query and constructs a response.
If an error occurs, this function construct the response
accordingly.
*/
void modbus_manage_query(modbus_param_t *mb_param, const uint8_t *query,
int query_length, modbus_mapping_t *mb_mapping)
{
int offset = mb_param->header_length;
int slave = query[offset];
int function = query[offset+1];
uint16_t address = (query[offset+2] << 8) + query[offset+3];
uint8_t response[MAX_MESSAGE_LENGTH];
int resp_length = 0;
sft_t sft;
sft.slave = slave;
sft.function = function;
// if (mb_param->type_com == TCP) {
// sft.t_id = (query[0] << 8) + query[1];
// } else {
sft.t_id = 0;
query_length -= CHECKSUM_LENGTH_RTU;
// }
switch (function) {
case FC_READ_COIL_STATUS: {
int nb = (query[offset+4] << 8) + query[offset+5];
if ((address + nb) > mb_mapping->nb_coil_status) {
printf("Illegal data address %0X in read_coil_status\n",
address + nb);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
resp_length = build_response_basis(mb_param, &sft, response);
response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
resp_length = response_io_status(address, nb,
mb_mapping->tab_coil_status,
response, resp_length);
}
}
break;
case FC_READ_INPUT_STATUS: {
/* Similar to coil status (but too much arguments to use a
* function) */
int nb = (query[offset+4] << 8) + query[offset+5];
if ((address + nb) > mb_mapping->nb_input_status) {
printf("Illegal data address %0X in read_input_status\n",
address + nb);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
resp_length = build_response_basis(mb_param, &sft, response);
response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
resp_length = response_io_status(address, nb,
mb_mapping->tab_input_status,
response, resp_length);
}
}
break;
case FC_READ_HOLDING_REGISTERS: {
int nb = (query[offset+4] << 8) + query[offset+5];
if ((address + nb) > mb_mapping->nb_holding_registers) {
printf("Illegal data address %0X in read_holding_registers\n",
address + nb);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
int i;
resp_length = build_response_basis(mb_param, &sft, response);
response[resp_length++] = nb << 1;
for (i = address; i < address + nb; i++) {
response[resp_length++] = mb_mapping->tab_holding_registers[i] >> 8;
response[resp_length++] = mb_mapping->tab_holding_registers[i] & 0xFF;
}
}
}
break;
case FC_READ_INPUT_REGISTERS: {
/* Similar to holding registers (but too much arguments to use a
* function) */
int nb = (query[offset+4] << 8) + query[offset+5];
if ((address + nb) > mb_mapping->nb_input_registers) {
printf("Illegal data address %0X in read_input_registers\n",
address + nb);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
int i;
resp_length = build_response_basis(mb_param, &sft, response);
response[resp_length++] = nb << 1;
for (i = address; i < address + nb; i++) {
response[resp_length++] = mb_mapping->tab_input_registers[i] >> 8;
response[resp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
}
}
}
break;
case FC_FORCE_SINGLE_COIL:
if (address >= mb_mapping->nb_coil_status) {
printf("Illegal data address %0X in force_singe_coil\n", address);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
int data = (query[offset+4] << 8) + query[offset+5];
if (data == 0xFF00 || data == 0x0) {
mb_mapping->tab_coil_status[address] = (data) ? ON : OFF;
/* In RTU mode, the CRC is computed and added
to the query by modbus_send, the computed
CRC will be same and optimisation is
possible here (FIXME). */
memcpy(response, query, query_length);
resp_length = query_length;
} else {
printf("Illegal data value %0X in force_single_coil request at address %0X\n",
data, address);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_VALUE, response);
}
}
break;
case FC_PRESET_SINGLE_REGISTER:
if (address >= mb_mapping->nb_holding_registers) {
printf("Illegal data address %0X in preset_holding_register\n", address);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
int data = (query[offset+4] << 8) + query[offset+5];
mb_mapping->tab_holding_registers[address] = data;
memcpy(response, query, query_length);
resp_length = query_length;
}
break;
case FC_FORCE_MULTIPLE_COILS: {
int nb = (query[offset+4] << 8) + query[offset+5];
if ((address + nb) > mb_mapping->nb_coil_status) {
printf("Illegal data address %0X in force_multiple_coils\n",
address + nb);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
/* 6 = byte count, 7 = first byte of data */
set_bits_from_bytes(mb_mapping->tab_coil_status, address, nb, &query[offset + 7]);
resp_length = build_response_basis(mb_param, &sft, response);
/* 4 to copy the coil address (2) and the quantity of coils */
memcpy(response + resp_length, query + resp_length, 4);
resp_length += 4;
}
}
break;
case FC_PRESET_MULTIPLE_REGISTERS: {
int nb = (query[offset+4] << 8) + query[offset+5];
if ((address + nb) > mb_mapping->nb_holding_registers) {
printf("Illegal data address %0X in preset_multiple_registers\n",
address + nb);
resp_length = response_exception(mb_param, &sft,
ILLEGAL_DATA_ADDRESS, response);
} else {
int i, j;
for (i = address, j = 0; i < address + nb; i++, j += 2) {
/* 6 = byte count, 7 and 8 = first value */
mb_mapping->tab_holding_registers[i] =
(query[offset + 7 + j] << 8) + query[offset + 8 + j];
}
resp_length = build_response_basis(mb_param, &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:
case FC_REPORT_SLAVE_ID:
printf("Not implemented\n");
break;
}
modbus_send(mb_param, response, resp_length);
}
/* Listens any message on a socket or file descriptor.
Returns:
- 0 if OK, or a negative error number if the request fails
- query, message received
- query_length, length in bytes of the message */
int modbus_listen(modbus_param_t *mb_param, uint8_t *query, int *query_length)
{
int ret;
/* The length of the query to receive isn't known. */
ret = receive_msg(mb_param, MSG_LENGTH_UNDEFINED, query, query_length);
return ret;
}
/* Reads IO status */
static int read_io_status(modbus_param_t *mb_param, int slave, int function,
int start_addr, int nb, uint8_t *data_dest)
{
int ret;
int query_length;
uint8_t query[MIN_QUERY_LENGTH];
uint8_t response[MAX_MESSAGE_LENGTH];
query_length = build_query_basis(mb_param, slave, function,
start_addr, nb, query);
ret = modbus_send(mb_param, query, query_length);
if (ret > 0) {
int i, temp, bit;
int pos = 0;
int offset;
int offset_length;
ret = modbus_receive(mb_param, query, response);
if (ret < 0)
return ret;
offset = mb_param->header_length;
offset_length = offset + ret;
for (i = offset; i < offset_length; i++) {
/* Shift reg hi_byte to temp */
temp = response[3 + i];
for (bit = 0x01; (bit & 0xff) && (pos < nb);) {
data_dest[pos++] = (temp & bit) ? TRUE : FALSE;
bit = bit << 1;
}
}
}
return ret;
}
/* Reads the boolean status of coils and sets the array elements
in the destination to TRUE or FALSE. */
int read_coil_status(modbus_param_t *mb_param, int slave, int start_addr,
int nb, uint8_t *data_dest)
{
int status;
if (nb > MAX_STATUS) {
printf("ERROR Too many coils status requested (%d > %d)\n",
nb, MAX_STATUS);
return TOO_MANY_DATA;
}
status = read_io_status(mb_param, slave, FC_READ_COIL_STATUS,
start_addr, nb, data_dest);
if (status > 0)
status = nb;
return status;
}
/* Same as read_coil_status but reads the slaves input table */
int read_input_status(modbus_param_t *mb_param, int slave, int start_addr,
int nb, uint8_t *data_dest)
{
int status;
if (nb > MAX_STATUS) {
printf("ERROR Too many input status requested (%d > %d)\n",
nb, MAX_STATUS);
return TOO_MANY_DATA;
}
status = read_io_status(mb_param, slave, FC_READ_INPUT_STATUS,
start_addr, nb, data_dest);
if (status > 0)
status = nb;
return status;
}
/* Reads the data from a modbus slave and put that data into an array */
static int read_registers(modbus_param_t *mb_param, int slave, int function,
int start_addr, int nb, uint16_t *data_dest)
{
int ret;
int query_length;
uint8_t query[MIN_QUERY_LENGTH];
uint8_t response[MAX_MESSAGE_LENGTH];
if (nb > MAX_REGISTERS) {
printf("EROOR Too many holding registers requested (%d > %d)\n",
nb, MAX_REGISTERS);
return TOO_MANY_DATA;
}
query_length = build_query_basis(mb_param, slave, function,
start_addr, nb, query);
ret = modbus_send(mb_param, query, query_length);
/*
if (ret > 0) {
int offset;
int i;
ret = modbus_receive(mb_param, query, response);
offset = mb_param->header_length;
// If ret is negative, the loop is jumped !
for (i = 0; i < ret; i++) {
// shift reg hi_byte to temp OR with lo_byte
data_dest[i] = (response[offset + 3 + (i << 1)] << 8) |
response[offset + 4 + (i << 1)];
}
}
*/
return ret;
}
/* Reads the holding registers in a slave and put the data into an
array */
int read_holding_registers(modbus_param_t *mb_param, int slave,
int start_addr, int nb, uint16_t *data_dest)
{
int status;
if (nb > MAX_REGISTERS) {
printf("ERROR Too many holding registers requested (%d > %d)\n",
nb, MAX_REGISTERS);
return TOO_MANY_DATA;
}
status = read_registers(mb_param, slave, FC_READ_HOLDING_REGISTERS,
start_addr, nb, data_dest);
return status;
}
/* Reads the input registers in a slave and put the data into
an array */
int read_input_registers(modbus_param_t *mb_param, int slave,
int start_addr, int nb, uint16_t *data_dest)
{
int status;
if (nb > MAX_REGISTERS) {
printf("ERROR Too many input registers requested (%d > %d)\n",
nb, MAX_REGISTERS);
return TOO_MANY_DATA;
}
status = read_registers(mb_param, slave, FC_READ_INPUT_REGISTERS,
start_addr, nb, data_dest);
return status;
}
/* Sends a value to a register in a slave.
Used by force_single_coil and preset_single_register */
static int set_single(modbus_param_t *mb_param, int slave, int function,
int addr, int value)
{
int ret;
int query_length;
uint8_t query[MIN_QUERY_LENGTH];
query_length = build_query_basis(mb_param, slave, function,
addr, value, query);
ret = modbus_send(mb_param, query, query_length);
if (ret > 0) {
/* Used by force_single_coil and
* preset_single_register */
uint8_t response[MIN_QUERY_LENGTH];
ret = modbus_receive(mb_param, query, response);
}
return ret;
}
/* Turns ON or OFF a single coil in the slave device */
int force_single_coil(modbus_param_t *mb_param, int slave,
int coil_addr, int state)
{
int status;
if (state)
state = 0xFF00;
status = set_single(mb_param, slave, FC_FORCE_SINGLE_COIL,
coil_addr, state);
return status;
}
/* Sets a value in one holding register in the slave device */
int preset_single_register(modbus_param_t *mb_param, int slave,
int reg_addr, int value)
{
int status;
status = set_single(mb_param, slave, FC_PRESET_SINGLE_REGISTER,
reg_addr, value);
return status;
}
/* Sets/resets the coils in the slave from an array in argument */
int force_multiple_coils(modbus_param_t *mb_param, int slave,
int start_addr, int nb,
const uint8_t *data_src)
{
int ret;
int i;
int byte_count;
int query_length;
int coil_check = 0;
int pos = 0;
uint8_t query[MAX_MESSAGE_LENGTH];
if (nb > MAX_STATUS) {
printf("ERROR Writing to too many coils (%d > %d)\n",
nb, MAX_STATUS);
return TOO_MANY_DATA;
}
query_length = build_query_basis(mb_param, slave,
FC_FORCE_MULTIPLE_COILS,
start_addr, nb, query);
byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
query[query_length++] = byte_count;
for (i = 0; i < byte_count; i++) {
int bit;
bit = 0x01;
query[query_length] = 0;
while ((bit & 0xFF) && (coil_check++ < nb)) {
if (data_src[pos++])
query[query_length] |= bit;
else
query[query_length] &=~ bit;
bit = bit << 1;
}
query_length++;
}
ret = modbus_send(mb_param, query, query_length);
if (ret > 0) {
uint8_t response[MAX_MESSAGE_LENGTH];
ret = modbus_receive(mb_param, query, response);
}
return ret;
}
/* Copies the values in the slave from the array given in argument */
int preset_multiple_registers(modbus_param_t *mb_param, int slave,
int start_addr, int nb,
const uint16_t *data_src)
{
int ret;
int i;
int query_length;
int byte_count;
uint8_t query[MAX_MESSAGE_LENGTH];
if (nb > MAX_REGISTERS) {
printf("ERROR Trying to write to too many registers (%d > %d)\n",
nb, MAX_REGISTERS);
return TOO_MANY_DATA;
}
query_length = build_query_basis(mb_param, slave,
FC_PRESET_MULTIPLE_REGISTERS,
start_addr, nb, query);
byte_count = nb * 2;
query[query_length++] = byte_count;
for (i = 0; i < nb; i++) {
query[query_length++] = data_src[i] >> 8;
query[query_length++] = data_src[i] & 0x00FF;
}
ret = modbus_send(mb_param, query, query_length);
if (ret > 0) {
uint8_t response[MAX_MESSAGE_LENGTH];
ret = modbus_receive(mb_param, query, response);
}
return ret;
}
/* Returns the slave id! */
int report_slave_id(modbus_param_t *mb_param, int slave,
uint8_t *data_dest)
{
int ret;
int query_length;
uint8_t query[MIN_QUERY_LENGTH];
query_length = build_query_basis(mb_param, slave, FC_REPORT_SLAVE_ID,
0, 0, query);
/* HACKISH, start_addr and count are not used */
query_length -= 4;
ret = modbus_send(mb_param, query, query_length);
if (ret > 0) {
int i;
int offset;
int offset_length;
uint8_t response[MAX_MESSAGE_LENGTH];
/* Byte count, slave id, run indicator status,
additional data */
ret = modbus_receive(mb_param, query, response);
if (ret < 0)
return ret;
offset = mb_param->header_length;
offset_length = offset + ret;
for (i = offset; i < offset_length; i++)
data_dest[i] = response[i];
}
return ret;
}
/* Initializes the modbus_param_t structure for RTU
- device: "/dev/ttyS0"
- baud: 9600, 19200, 57600, 115200, etc
- parity: "even", "odd" or "none"
- data_bits: 5, 6, 7, 8
- stop_bits: 1, 2
*/
void modbus_init_rtu(modbus_param_t *mb_param, const char *device,
int baud, const char *parity, int data_bit,
int stop_bit)
{
memset(mb_param, 0, sizeof(modbus_param_t));
strcpy(mb_param->device, device);
mb_param->baud = baud;
strcpy(mb_param->parity, parity);
mb_param->debug = FALSE;
mb_param->data_bit = data_bit;
mb_param->stop_bit = stop_bit;
mb_param->type_com = RTU;
mb_param->header_length = HEADER_LENGTH_RTU;
mb_param->checksum_length = CHECKSUM_LENGTH_RTU;
}
/* By default, the error handling mode used is FLUSH_OR_RECONNECT_ON_ERROR.
With FLUSH_OR_RECONNECT_ON_ERROR, the library will flush to I/O
port in RTU mode or attempt an immediate reconnection which may
hang for several seconds if the network to the remote target unit
is down in TCP mode.
With NOP_ON_ERROR, it is expected that the application will
check for error returns and deal with them as necessary.
*/
void modbus_set_error_handling(modbus_param_t *mb_param,
error_handling_t error_handling)
{
if (error_handling == FLUSH_OR_RECONNECT_ON_ERROR ||
error_handling == NOP_ON_ERROR) {
mb_param->error_handling = error_handling;
} else {
printf("Invalid setting for error handling (not changed)\n");
}
}
/* Sets up a serial port for RTU communications */
static int modbus_connect_rtu(modbus_param_t *mb_param)
{
return 0;
}
/* Establishes a modbus connexion.
Returns -1 if an error occured. */
int modbus_connect(modbus_param_t *mb_param)
{
int ret;
ret = modbus_connect_rtu(mb_param);
return ret;
}
/* Closes the file descriptor in RTU mode */
static void modbus_close_rtu(modbus_param_t *mb_param)
{
}
/* Closes a modbus connection */
void modbus_close(modbus_param_t *mb_param)
{
modbus_close_rtu(mb_param);
}
/* Activates the debug messages */
void modbus_set_debug(modbus_param_t *mb_param, int boolean)
{
mb_param->debug = boolean;
}
/* Allocates 4 arrays to store coils, input status, input registers and
holding registers. The pointers are stored in modbus_mapping structure.
Returns: TRUE if ok, FALSE on failure
*/
int modbus_mapping_new(modbus_mapping_t *mb_mapping,
int nb_coil_status, int nb_input_status,
int nb_holding_registers, int nb_input_registers)
{
/* 0X */
mb_mapping->nb_coil_status = nb_coil_status;
mb_mapping->tab_coil_status =
(uint8_t *) malloc(nb_coil_status * sizeof(uint8_t));
memset(mb_mapping->tab_coil_status, 0,
nb_coil_status * sizeof(uint8_t));
if (mb_mapping->tab_coil_status == NULL)
return FALSE;
/* 1X */
mb_mapping->nb_input_status = nb_input_status;
mb_mapping->tab_input_status =
(uint8_t *) malloc(nb_input_status * sizeof(uint8_t));
memset(mb_mapping->tab_input_status, 0,
nb_input_status * sizeof(uint8_t));
if (mb_mapping->tab_input_status == NULL) {
free(mb_mapping->tab_coil_status);
return FALSE;
}
/* 4X */
mb_mapping->nb_holding_registers = nb_holding_registers;
mb_mapping->tab_holding_registers =
(uint16_t *) malloc(nb_holding_registers * sizeof(uint16_t));
memset(mb_mapping->tab_holding_registers, 0,
nb_holding_registers * sizeof(uint16_t));
if (mb_mapping->tab_holding_registers == NULL) {
free(mb_mapping->tab_coil_status);
free(mb_mapping->tab_input_status);
return FALSE;
}
/* 3X */
mb_mapping->nb_input_registers = nb_input_registers;
mb_mapping->tab_input_registers =
(uint16_t *) malloc(nb_input_registers * sizeof(uint16_t));
memset(mb_mapping->tab_input_registers, 0,
nb_input_registers * sizeof(uint16_t));
if (mb_mapping->tab_input_registers == NULL) {
free(mb_mapping->tab_coil_status);
free(mb_mapping->tab_input_status);
free(mb_mapping->tab_holding_registers);
return FALSE;
}
return TRUE;
}
/* Frees the 4 arrays */
void modbus_mapping_free(modbus_mapping_t *mb_mapping)
{
free(mb_mapping->tab_coil_status);
free(mb_mapping->tab_input_status);
free(mb_mapping->tab_holding_registers);
free(mb_mapping->tab_input_registers);
}
/** Utils **/
/* Sets many input/coil status from a single byte value (all 8 bits of
the byte value are setted) */
void set_bits_from_byte(uint8_t *dest, int address, const uint8_t value)
{
int i;
for (i=0; i<8; i++) {
dest[address+i] = (value & (1 << i)) ? ON : OFF;
}
}
/* Sets many input/coil status from a table of bytes (only the bits
between address and address + nb_bits are setted) */
void set_bits_from_bytes(uint8_t *dest, int address, int nb_bits,
const uint8_t tab_byte[])
{
int i;
int shift = 0;
for (i = address; i < address + nb_bits; i++) {
dest[i] = tab_byte[(i - address) / 8] & (1 << shift) ? ON : OFF;
/* gcc doesn't like: shift = (++shift) % 8; */
shift++;
shift %= 8;
}
}
/* Gets the byte value from many input/coil status.
To obtain a full byte, set nb_bits to 8. */
uint8_t get_byte_from_bits(const uint8_t *src, int address, int nb_bits)
{
int i;
uint8_t value = 0;
if (nb_bits > 8) {
printf("Error: nb_bits is too big\n");
nb_bits = 8;
}
for (i=0; i < nb_bits; i++) {
value |= (src[address+i] << i);
}
return value;
}