/*
* Copyright (c) 2001-2003,2016 Mario de Sousa (msousa@fe.up.pt)
*
* This file is part of the Modbus library for Beremiz and matiec.
*
* This Modbus library is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
* General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this Modbus library. If not, see <http://www.gnu.org/licenses/>.
*
* This code is made available on the understanding that it will not be
* used in safety-critical situations without a full and competent review.
*/
/* mb_master.c */
#include <fcntl.h> /* File control definitions */
#include <stdio.h> /* Standard input/output */
#include <string.h>
#include <stdlib.h>
#include <termio.h> /* POSIX terminal control definitions */
#include <sys/time.h> /* Time structures for select() */
#include <unistd.h> /* POSIX Symbolic Constants */
#include <errno.h> /* Error definitions */
#include <pthread.h> /* pthread_mutex_[un]lock() */
#include <netinet/in.h> /* required for htons() and ntohs() */
#include "mb_layer1.h"
#include "mb_master.h"
#include "mb_master_private.h"
/* #define DEBUG */ /* uncomment to see the data sent and received */
#define modbus_write fptr_[layer1_fin].modbus_write
#define modbus_read fptr_[layer1_fin].modbus_read
#define modbus_init fptr_[layer1_fin].modbus_init
#define modbus_done fptr_[layer1_fin].modbus_done
#define modbus_connect fptr_[layer1_fin].modbus_connect
#define modbus_listen fptr_[layer1_fin].modbus_listen
#define modbus_close fptr_[layer1_fin].modbus_close
#define modbus_silence_init fptr_[layer1_fin].modbus_silence_init
#define modbus_get_min_timeout fptr_[layer1_fin].modbus_get_min_timeout
/* the lower two bits of ttyfd are used to store the index to layer1 function pointers */
/* layer1_fin index to fptr_[] is in lowest 2 bits of fd */
#define get_ttyfd() int layer1_fin = fd & 3; int ttyfd = fd / 4;
/******************************************/
/******************************************/
/** **/
/** Global Variables... **/
/** **/
/******************************************/
/******************************************/
/* The layer 1 (RTU, ASCII, TCP) implementation will be adding some headers and CRC (at the end)
* of the packet we build here (actually currently it is only at the end). Since we want to
* re-use the same buffer so as not to continuosly copy the same info from buffer to buffer,
* we need tp allocate more bytes than the ones we need for this layer. Therefore, the
* extra_bytes parameter.
*
* Note that we add one more extra byte. This is because some packets will not be
* starting off at byte 0, but rather at byte 1 of the buffer. This is in order to guarantee
* that the data that is sent on the buffer is aligned on even bytes (the 16 bit words!).
* This will allow us to reference this memory as an u16 *, without producing 'bus error'
* messages in some embedded devices that do not allow acessing u16 on odd numbered addresses.
*/
static int buff_extra_bytes_;
#define QUERY_BUFFER_SIZE (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)
/******************************************/
/******************************************/
/** **/
/** Local Utility functions... **/
/** **/
/******************************************/
/******************************************/
/*
* Function to determine next transaction id.
*
* We use a library wide transaction id, which means that we
* use a new transaction id no matter what slave to which we will
* be sending the request...
*/
static inline u16 next_transaction_id(void) {
static u16 next_id = 0;
return next_id++;
}
/*
* Functions to convert u16 variables
* between network and host byte order
*
* NOTE: Modbus uses MSByte first, just like
* tcp/ip, so we use the htons() and
* ntoh() functions to guarantee
* code portability.
*/
static inline u16 mb_hton(u16 h_value) {return htons(h_value);}
static inline u16 mb_ntoh(u16 m_value) {return ntohs(m_value);}
static inline u8 msb (u16 value) {return (value >> 8) & 0xFF;}
static inline u8 lsb (u16 value) {return value & 0xFF;}
/*************************************************/
/*************************************************/
/** **/
/** Common functions for Modbus Protocol. **/
/** **/
/*************************************************/
/*************************************************/
/* build the common elements of a query frame */
static inline int build_packet(u8 slave,
u8 function,
u16 start_addr,
u16 count,
u8 *packet) {
union {
u16 u16;
u8 u8[2];
} tmp;
packet[0] = slave,
packet[1] = function;
/* NOTE:
* Modbus uses high level addressing starting off from 1, but
* this is sent as 0 on the wire!
* We could expect the user to specify high level addressing
* starting at 1, and do the conversion to start off at 0 here.
* However, to do this we would then need to use an u32 data type
* to correctly hold the address supplied by the user (which could
* correctly be 65536, which does not fit in an u16), which would
* in turn require us to check whether the address supplied by the user
* is correct (i.e. <= 65536).
* I decided to go with the other option of using an u16, and
* requiring the user to use addressing starting off at 0!
*/
/* NOTE: we do not use up casting - i.e. the following
* *((u16 *)(packet+2)) = mb_hton(start_addr);
* because packet+2 is NOT aligned with an even address, and would
* therefore result in 'bus error' when using compilers that do not
* automatically do the required decomposing of this supposedly
* single bus access into two distinct bus accesses.
* (Note that some compilers do do this decomposing automatically
* in which case the following is not necessary).
* At the moment, I (Mario de Sousa) know of at least one cross-compiler
* that does not do the decomposing automatically, i.e. the
* AVR32 cross-compiler.
*/
tmp.u16 = mb_hton(start_addr);
packet[2] = tmp.u8[0];
packet[3] = tmp.u8[1];
tmp.u16 = mb_hton(count);
packet[4] = tmp.u8[0];
packet[5] = tmp.u8[1];
return 6;
}
/* Execute a Query/Response transaction between client and server */
/* returns: <0 -> ERROR: error codes
* >2 -> SUCCESS: frame length
* 0..2 -> will never be returned!
*/
static int mb_transaction(u8 *packet,
int query_length,
u8 **data,
int fd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
int error = TIMEOUT;
int response_length = INTERNAL_ERROR;
u16 send_transaction_id, recv_transaction_id;
get_ttyfd(); /* declare the ttyfd variable, ... */
/* We must also initialize the recv_transaction_id with the same value,
* since some layer 1 protocols do not support transaction id's, so
* simply return the recv_transaction_id variable without any changes...
*/
/* NOTE: we re-use the same transaction id for all send re-tries., since, in truth,
* it is still the same transaction. This will also simplify re-synchronising with
* some slaves that keep a buffer of outstanding requests, and will reply to all of
* them, in FIFO order. In this case, once an error occurs we will be swamping the
* slave with requests. By using the same transaction id, we may correctly consider
* the reply to the first request sent as the reply to the third request! This means
* we stop re-trying the sending of further requests, and no longer swamp the slave...
*/
send_transaction_id = recv_transaction_id = next_transaction_id();
for (send_retries++; send_retries > 0; send_retries--) {
error = TIMEOUT;
if (modbus_write(ttyfd, packet, query_length, send_transaction_id, response_timeout) < 0)
{error = PORT_FAILURE; continue;}
/* if we receive a correct response but with a wrong transaction id or wrong modbus function, we try to
* receive another frame instead of returning an error or re-sending the request! This first frame could
* have been a response to a previous request of ours that timed out waiting for a response, and the
* response we are waiting for could be coming 'any minute now'.
*/
do {
response_length = modbus_read(&ttyfd, data, &recv_transaction_id,
packet, query_length, response_timeout);
/* TIMEOUT condition */
/* However, if we had previously received an invalid frame, or some other error,
* we return that error instead!
* Note that the 'error' variable was initialised with the TIMEOUT error
* condition, so if no previous error ocurred, we will be returning the
* TIMEOUT error condition here!
*/
if(response_length == -2) return error;
/* NOTE we want to break out of this while loop without even running the while()
* condition, as that condition is only valid if response_length > 3 !!
*/
if(response_length < 0) {error = PORT_FAILURE; break;}
/* This should never occur! Modbus_read() should only return valid frames! */
if(response_length < 3) return INTERNAL_ERROR;
} while (/* we have the wrong transaction id */
(send_transaction_id != recv_transaction_id)
/* not a response frame to _our_ query */
||
(((*data)[1] & ~0x80) != packet[1])
/* NOTE: no need to check whether (*data)[0] = slave! */
/* This has already been done by the modbus_read() function! */
);
if(response_length < 0) {error = PORT_FAILURE; continue;}
/* Now check whether we received a Modbus Exception frame */
if (((*data)[1] & 0x80) != 0) { /* we have an exception frame! */
/* NOTE: we have already checked above that data[2] exists! */
if (error_code != NULL) *error_code = (*data)[2];
return MODBUS_ERROR;
}
/* success! Let's get out of the send retry loop... */
return response_length;
}
/* reached the end of the retries... */
return error;
}
/**************************************/
/**************************************/
/** **/
/** Modbus Protocol Functions. **/
/** **/
/**************************************/
/**************************************/
/* Execute a transaction for functions that READ BITS.
* Bits are stored on an int array, one bit per int.
* Called by: read_input_bits()
* read_output_bits()
*/
static int read_bits(u8 function,
u8 slave,
u16 start_addr,
u16 count,
u16 *dest,
int dest_size,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
u8 packet[QUERY_BUFFER_SIZE];
u8 *data;
int response_length, query_length;
int temp, i, bit, dest_pos = 0;
int coils_processed = 0;
query_length = build_packet(slave, function, start_addr, count, packet);
if (query_length < 0) return INTERNAL_ERROR;
response_length = mb_transaction(packet, query_length, &data, ttyfd,
send_retries, error_code, response_timeout);
if (response_length < 0) return response_length;
/* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
if (response_length != 3 + (count+7)/8) return INVALID_FRAME;
if (data[2] != (count+7)/8) return INVALID_FRAME;
if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
for( i = 0; (i < data[2]) && (i < dest_size); i++ ) {
temp = data[3 + i];
for( bit = 0x01; (bit & 0xff) && (coils_processed < count); ) {
dest[dest_pos] = (temp & bit)?1:0;
coils_processed++;
dest_pos++;
bit = bit << 1;
}
}
if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
return response_length;
}
/* Execute a transaction for functions that READ BITS.
* Bits are stored on an u32 array, 32 bits per u32.
* Unused bits in last u32 are set to 0.
* Called by: read_input_bits_u32()
* read_output_bits_u32()
*/
static int read_bits_u32(u8 function,
u8 slave,
u16 start_addr,
u16 count, /* number of bits !! */
u32 *dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
u8 packet[QUERY_BUFFER_SIZE];
u8 *data;
int response_length, query_length;
int byte_count, i, dest_pos = 0;
query_length = build_packet(slave, function, start_addr, count, packet);
if (query_length < 0) return INTERNAL_ERROR;
response_length = mb_transaction(packet, query_length, &data, ttyfd,
send_retries, error_code, response_timeout);
if (response_length < 0) return response_length;
/* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
if (response_length != 3 + (count+7)/8) return INVALID_FRAME;
if (data[2] != (count+7)/8) return INVALID_FRAME;
byte_count = data[2];
data += 3;
/* handle groups of 4 bytes... */
for(i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++)
dest[dest_pos] = data[i] + data[i+1]*0x100 + data[i+2]*0x10000 + data[i+3]*0x1000000;
/* handle any remaining bytes... begining with the last! */
if (i < byte_count) dest[dest_pos] = 0;
for(byte_count--; i <= byte_count; byte_count--)
dest[dest_pos] = dest[dest_pos]*0x100 + data[byte_count];
return response_length;
}
/* FUNCTION 0x01 - Read Coils
* Bits are stored on an int array, one bit per int.
*/
int read_output_bits(u8 slave,
u16 start_addr,
u16 count,
u16 *dest,
int dest_size,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
if( count > MAX_READ_BITS ) {
count = MAX_READ_BITS;
#ifdef DEBUG
fprintf( stderr, "Too many coils requested.\n" );
#endif
}
return read_bits(0x01 /* function */,
slave, start_addr, count, dest, dest_size, ttyfd,
send_retries, error_code, response_timeout, data_access_mutex);
}
/* FUNCTION 0x01 - Read Coils
* Bits are stored on an u32 array, 32 bits per u32.
* Unused bits in last u32 are set to 0.
*/
int read_output_bits_u32(u8 slave,
u16 start_addr,
u16 count,
u32 *dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
if( count > MAX_READ_BITS ) {
count = MAX_READ_BITS;
#ifdef DEBUG
fprintf( stderr, "Too many coils requested.\n" );
#endif
}
return read_bits_u32(0x01 /* function */,
slave, start_addr, count, dest, ttyfd,
send_retries, error_code, response_timeout);
}
/* FUNCTION 0x02 - Read Discrete Inputs
* Bits are stored on an int array, one bit per int.
*/
int read_input_bits(u8 slave,
u16 start_addr,
u16 count,
u16 *dest,
int dest_size,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
if( count > MAX_READ_BITS ) {
count = MAX_READ_BITS;
#ifdef DEBUG
fprintf( stderr, "Too many coils requested.\n" );
#endif
}
return read_bits(0x02 /* function */,
slave, start_addr, count, dest, dest_size, ttyfd,
send_retries, error_code, response_timeout, data_access_mutex);
}
/* FUNCTION 0x02 - Read Discrete Inputs
* Bits are stored on an u32 array, 32 bits per u32.
* Unused bits in last u32 are set to 0.
*/
int read_input_bits_u32(u8 slave,
u16 start_addr,
u16 count,
u32 *dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
if( count > MAX_READ_BITS ) {
count = MAX_READ_BITS;
#ifdef DEBUG
fprintf( stderr, "Too many coils requested.\n" );
#endif
}
return read_bits_u32(0x02 /* function */,
slave, start_addr, count, dest, ttyfd,
send_retries, error_code, response_timeout);
}
/* Execute a transaction for functions that READ REGISTERS.
* Called by: read_input_words()
* read_output_words()
*/
static int read_registers(u8 function,
u8 slave,
u16 start_addr,
u16 count,
u16 *dest,
int dest_size,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
u8 *data;
u8 packet[QUERY_BUFFER_SIZE];
int response_length;
int query_length;
int temp,i;
query_length = build_packet(slave, function, start_addr, count, packet);
if (query_length < 0) return INTERNAL_ERROR;
response_length = mb_transaction(packet, query_length, &data, ttyfd,
send_retries, error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 3 + 2*count) return INVALID_FRAME;
if (data[2] != 2*count) return INVALID_FRAME;
if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
for(i = 0; (i < (data[2]*2)) && (i < dest_size); i++ ) {
temp = data[3 + i *2] << 8; /* copy reg hi byte to temp hi byte*/
temp = temp | data[4 + i * 2]; /* copy reg lo byte to temp lo byte*/
dest[i] = temp;
}
if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
return response_length;
}
/* Execute a transaction for functions that READ REGISTERS.
* return the array with the data to the calling function
* Called by: read_input_words_u16_ref()
* read_output_words_u16_ref()
*/
static int read_registers_u16_ref(u8 function,
u8 slave,
u16 start_addr,
u16 count,
u16 **dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
u8 *data;
u8 packet[QUERY_BUFFER_SIZE];
int response_length;
int query_length;
int i, byte_count;
query_length = build_packet(slave, function, start_addr, count, packet);
if (query_length < 0) return INTERNAL_ERROR;
response_length = mb_transaction(packet, query_length, &data, ttyfd,
send_retries, error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 3 + 2*count) return INVALID_FRAME;
if (data[2] != 2*count) return INVALID_FRAME;
byte_count = data[2];
data = data + 3; /* & data[3] */
if (ntohs(0x0102) != 0x0102) {
/* little endian host... => we need to swap the bytes! */
for(i = 0; i < byte_count; i++ ) {
/* the following 3 lines result in the two values being exchanged! */
data[i ] = data[i] ^ data[i+1];
data[i+1] = data[i] ^ data[i+1];
data[i ] = data[i] ^ data[i+1];
}
}
*dest = (u16 *)data;
return byte_count;
}
/* Execute a transaction for functions that READ REGISTERS.
* u16 registers are stored in array of u32, two registers per u32.
* Unused bits of last u32 element are set to 0.
* Called by: read_input_words_u32()
* read_output_words_u32()
*/
static int read_registers_u32(u8 function,
u8 slave,
u16 start_addr,
u16 count,
u32 *dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
u8 *data;
u8 packet[QUERY_BUFFER_SIZE];
int response_length;
int query_length;
int i, byte_count, dest_pos;
query_length = build_packet(slave, function, start_addr, count, packet);
if (query_length < 0) return INTERNAL_ERROR;
response_length = mb_transaction(packet, query_length, &data, ttyfd,
send_retries, error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 3 + 2*count) return INVALID_FRAME;
if (data[2] != 2*count) return INVALID_FRAME;
byte_count = data[2];
data += 3;
if (ntohs(0x0102) == 0x0102) {
/* big endian host... */
/* handle groups of 4 bytes... */
for(i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++) {
*(((u8 *)(dest + dest_pos))+ 0) = *(data+i+3);
*(((u8 *)(dest + dest_pos))+ 1) = *(data+i+4);
*(((u8 *)(dest + dest_pos))+ 2) = *(data+i+0);
*(((u8 *)(dest + dest_pos))+ 3) = *(data+i+1);
}
/* handle any remaining bytes...
* since byte_count is supposed to be multiple of 2,
* (and has already been verified above 'if (data[2] != 2*count)')
* this will be either 2, or none at all!
*/
if (i + 1 < byte_count)
*(((u8 *)(dest + dest_pos))+ 0) = 0;
*(((u8 *)(dest + dest_pos))+ 1) = 0;
*(((u8 *)(dest + dest_pos))+ 2) = *(data+i+0);
*(((u8 *)(dest + dest_pos))+ 3) = *(data+i+1);
} else {
/* little endian host... */
/* handle groups of 4 bytes... */
for(i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++) {
*(((u8 *)(dest + dest_pos))+ 0) = *(data+i+1);
*(((u8 *)(dest + dest_pos))+ 1) = *(data+i+0);
*(((u8 *)(dest + dest_pos))+ 2) = *(data+i+3);
*(((u8 *)(dest + dest_pos))+ 3) = *(data+i+2);
}
/* handle any remaining bytes...
* since byte_count is supposed to be multiple of 2,
* (and has already been verified above 'if (data[2] != 2*count)')
* this will be either 2, or none at all!
*/
if (i + 1 < byte_count)
*(((u8 *)(dest + dest_pos))+ 0) = *(data+i+1);
*(((u8 *)(dest + dest_pos))+ 1) = *(data+i+0);
*(((u8 *)(dest + dest_pos))+ 2) = 0;
*(((u8 *)(dest + dest_pos))+ 3) = 0;
}
return response_length;
}
/* FUNCTION 0x03 - Read Holding Registers */
int read_output_words(u8 slave,
u16 start_addr,
u16 count,
u16 *dest,
int dest_size,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
if( count > MAX_READ_REGS ) {
count = MAX_READ_REGS;
#ifdef DEBUG
fprintf( stderr, "Too many registers requested.\n" );
#endif
}
return read_registers(0x03 /* function */,
slave, start_addr, count, dest, dest_size, ttyfd,
send_retries, error_code, response_timeout, data_access_mutex);
}
/* FUNCTION 0x03 - Read Holding Registers
* u16 registers are stored in array of u32, two registers per u32.
* Unused bits of last u32 element are set to 0.
*/
int read_output_words_u32(u8 slave,
u16 start_addr,
u16 count,
u32 *dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
if( count > MAX_READ_REGS ) {
count = MAX_READ_REGS;
#ifdef DEBUG
fprintf( stderr, "Too many registers requested.\n" );
#endif
}
return read_registers_u32(0x03 /* function */,
slave, start_addr, count, dest, ttyfd,
send_retries, error_code, response_timeout);
}
/* FUNCTION 0x03 - Read Holding Registers
* return the array with the data to the calling function
*/
int read_output_words_u16_ref(u8 slave,
u16 start_addr,
u16 count,
u16 **dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
if( count > MAX_READ_REGS ) {
count = MAX_READ_REGS;
#ifdef DEBUG
fprintf( stderr, "Too many registers requested.\n" );
#endif
}
return read_registers_u16_ref(0x03 /* function */,
slave, start_addr, count, dest, ttyfd, send_retries,
error_code, response_timeout);
}
/* FUNCTION 0x04 - Read Input Registers */
int read_input_words(u8 slave,
u16 start_addr,
u16 count,
u16 *dest,
int dest_size,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
if( count > MAX_READ_REGS ) {
count = MAX_READ_REGS;
#ifdef DEBUG
fprintf( stderr, "Too many input registers requested.\n" );
#endif
}
return read_registers(0x04 /* function */,
slave, start_addr, count, dest, dest_size, ttyfd, send_retries,
error_code, response_timeout, data_access_mutex);
}
/* FUNCTION 0x04 - Read Input Registers
* u16 registers are stored in array of u32, two registers per u32.
* Unused bits of last u32 element are set to 0.
*/
int read_input_words_u32(u8 slave,
u16 start_addr,
u16 count,
u32 *dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
if( count > MAX_READ_REGS ) {
count = MAX_READ_REGS;
#ifdef DEBUG
fprintf( stderr, "Too many input registers requested.\n" );
#endif
}
return read_registers_u32(0x04 /* function */,
slave, start_addr, count, dest, ttyfd, send_retries,
error_code, response_timeout);
}
/* FUNCTION 0x04 - Read Input Registers
* return the array with the data to the calling function
*/
int read_input_words_u16_ref(u8 slave,
u16 start_addr,
u16 count,
u16 **dest,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
if( count > MAX_READ_REGS ) {
count = MAX_READ_REGS;
#ifdef DEBUG
fprintf( stderr, "Too many input registers requested.\n" );
#endif
}
return read_registers_u16_ref(0x04 /* function */,
slave, start_addr, count, dest, ttyfd, send_retries,
error_code, response_timeout);
}
/* Execute a transaction for functions that WRITE a sinlge BIT.
* Called by: write_output_bit()
* write_output_word()
*/
static int set_single(u8 function,
u8 slave,
u16 addr,
u16 value,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
u8 packet[QUERY_BUFFER_SIZE];
u8 *data;
int query_length, response_length;
if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
query_length = build_packet(slave, function, addr, value, packet);
if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
if (query_length < 0) return INTERNAL_ERROR;
response_length = mb_transaction(packet, query_length, &data, ttyfd, send_retries,
error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 6) return INVALID_FRAME;
if ((data[2] != packet[2]) || (data[3] != packet[3]) ||
(data[4] != packet[4]) || (data[5] != packet[5]))
return INVALID_FRAME;
return response_length;
}
/* FUNCTION 0x05 - Force Single Coil */
int write_output_bit(u8 slave,
u16 coil_addr,
u16 state,
int fd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
if (state) state = 0xFF00;
return set_single(0x05 /* function */,
slave, coil_addr, state, fd, send_retries,
error_code, response_timeout, data_access_mutex);
}
/* FUNCTION 0x06 - Write Single Register */
int write_output_word(u8 slave,
u16 reg_addr,
u16 value,
int fd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
return set_single(0x06 /* function */,
slave, reg_addr, value, fd, send_retries,
error_code, response_timeout, data_access_mutex);
}
/* FUNCTION 0x0F - Force Multiple Coils */
int write_output_bits(u8 slave,
u16 start_addr,
u16 coil_count,
u16 *data,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
int byte_count, i;
u8 bit;
int coil_check = 0;
int data_array_pos = 0;
int query_length, response_length;
u8 packet[QUERY_BUFFER_SIZE];
u8 *rdata;
if( coil_count > MAX_WRITE_COILS ) {
coil_count = MAX_WRITE_COILS;
#ifdef DEBUG
fprintf( stderr, "Writing to too many coils.\n" );
#endif
}
query_length = build_packet(slave, 0x0F /* function */,
start_addr, coil_count, packet);
if (query_length < 0) return INTERNAL_ERROR;
/* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
byte_count = (coil_count+7)/8;
packet[query_length] = byte_count;
if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
bit = 0x01;
for(i = 0; i < byte_count; i++) {
packet[++query_length] = 0;
while((bit & 0xFF) && (coil_check++ < coil_count)) {
if(data[data_array_pos++]) {packet[query_length] |= bit;}
else {packet[query_length] &= ~bit;}
bit <<= 1;
}
bit = 0x01;
}
if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 6) return INVALID_FRAME;
if ((rdata[2] != packet[2]) ||
(rdata[3] != packet[3]) ||
(rdata[4] != packet[4]) ||
(rdata[5] != packet[5])) return INVALID_FRAME;
return response_length;
}
/* FUNCTION 0x0F - Force Multiple Coils
* Bits should be stored on an u32 array, 32 bits per u32.
* Unused bits in last u32 should be set to 0.
*/
int write_output_bits_u32(u8 slave,
u16 start_addr,
u16 coil_count,
u32 *data,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
int org_pos, byte_count, i;
int query_length, response_length;
u8 packet[QUERY_BUFFER_SIZE];
u8 *rdata;
if( coil_count > MAX_WRITE_COILS ) {
coil_count = MAX_WRITE_COILS;
#ifdef DEBUG
fprintf( stderr, "Writing to too many coils.\n" );
#endif
}
query_length = build_packet(slave, 0x0F /* function */,
start_addr, coil_count, packet);
if (query_length < 0) return INTERNAL_ERROR;
/* NOTE: Integer division. This is equivalent of determining the ceil(count/8) */
byte_count = (coil_count+7)/8;
packet[query_length] = byte_count;
/* handle groups of 4 bytes... */
for(i = 0, org_pos = 0; i + 3 < byte_count; i += 4, org_pos++) {
packet[++query_length] = data[org_pos] & 0xFF; data[org_pos] >>= 8;
packet[++query_length] = data[org_pos] & 0xFF; data[org_pos] >>= 8;
packet[++query_length] = data[org_pos] & 0xFF; data[org_pos] >>= 8;
packet[++query_length] = data[org_pos] & 0xFF;
}
/* handle any remaining bytes... */
for(; i < byte_count; i++) {
packet[++query_length] = data[org_pos] & 0xFF;
data[org_pos] >>= 8;
}
response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 6) return INVALID_FRAME;
if ((rdata[2] != packet[2]) ||
(rdata[3] != packet[3]) ||
(rdata[4] != packet[4]) ||
(rdata[5] != packet[5])) return INVALID_FRAME;
return response_length;
}
/* FUNCTION 0x10 - Force Multiple Registers */
int write_output_words(u8 slave,
u16 start_addr,
u16 reg_count,
u16 *data,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout,
pthread_mutex_t *data_access_mutex) {
u8 byte_count;
int i, query_length, response_length;
u8 packet[QUERY_BUFFER_SIZE];
u8 *rdata;
if( reg_count > MAX_WRITE_REGS ) {
reg_count = MAX_WRITE_REGS;
#ifdef DEBUG
fprintf( stderr, "Trying to write to too many registers.\n" );
#endif
}
query_length = build_packet(slave, 0x10 /* function */,
start_addr, reg_count, packet);
if (query_length < 0) return INTERNAL_ERROR;
byte_count = reg_count*2;
packet[query_length] = byte_count;
if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
for( i = 0; i < reg_count; i++ ) {
packet[++query_length] = data[i] >> 8;
packet[++query_length] = data[i] & 0x00FF;
}
if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 6) return INVALID_FRAME;
if ((rdata[2] != packet[2]) ||
(rdata[3] != packet[3]) ||
(rdata[4] != packet[4]) ||
(rdata[5] != packet[5])) return INVALID_FRAME;
return response_length;
}
/* FUNCTION 0x10 - Force Multiple Registers
* u16 registers are stored in array of u32, two registers per u32.
* Unused bits of last u32 element are set to 0.
*/
int write_output_words_u32(u8 slave,
u16 start_addr,
/* number of 16 bit registers packed in the u32 array! */
u16 reg_count,
u32 *data,
int ttyfd,
int send_retries,
u8 *error_code,
const struct timespec *response_timeout) {
u8 byte_count;
int i, query_length, response_length;
u8 packet_[QUERY_BUFFER_SIZE];
u8 *packet = packet_; /* remove the const'ness of packet_ */
u8 *rdata;
if( reg_count > MAX_WRITE_REGS ) {
reg_count = MAX_WRITE_REGS;
#ifdef DEBUG
fprintf( stderr, "Trying to write to too many registers.\n" );
#endif
}
/* Make sure that the de-referencing and up-casting going on later on in
* this function, i.e. code like the following line:
* *((u16 *)packet) = XXX
* will result in u16 words starting off on even addresses.
* If we don't do this, some compilers (e.g. AVR32 cross-compiler) will
* generate code which, when executed, will result in 'bus error'.
*
* The following packet++ means that the first byte of the packet array is
* essentially never used. Notice too that the size of thepacket array
* already takes into account this un-used byte.
*/
packet++;
query_length = build_packet(slave, 0x10 /* function */,
start_addr, reg_count, packet);
if (query_length < 0) return INTERNAL_ERROR;
byte_count = reg_count*2;
packet[query_length] = byte_count;
/* handle groups of 4 bytes... */
for(i = 0; 4*i + 3 < byte_count; i++) {
*((u16 *)(packet+(++query_length))) = mb_hton(data[i]); ++query_length;
*((u16 *)(packet+(++query_length))) = mb_hton(data[i] >> 16); ++query_length;
}
/* handle any remaining bytes...
* since byte_count is supposed to be multiple of 2,
* (and has already been verified above 'if (data[2] != 2*count)')
* this will be either 2, or none at all!
*/
if (4*i + 1 < byte_count) {
*((u16 *)(packet+(++query_length))) = mb_hton(data[i]); ++query_length;
}
response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
error_code, response_timeout);
if (response_length < 0) return response_length;
if (response_length != 6) return INVALID_FRAME;
if ((rdata[2] != packet[2]) ||
(rdata[3] != packet[3]) ||
(rdata[4] != packet[4]) ||
(rdata[5] != packet[5])) return INVALID_FRAME;
return response_length;
}
/************************************************/
/************************************************/
/** **/
/** Modbus Library Management Functions. **/
/** **/
/************************************************/
/************************************************/
/* Initialise the Modbus Master Layer */
int mb_master_init__(int extra_bytes) {
#ifdef DEBUG
fprintf(stderr, "mb_master_init__(extra_bytes=%d), QUERY_BUFFER_SIZE=%d\n", extra_bytes, QUERY_BUFFER_SIZE);
#endif
buff_extra_bytes_ = extra_bytes;
return 0;
}
/* Shut down the Modbus Master Layer */
int mb_master_done__(void) {
return 0;
}
#if 0
int mb_master_init(int nd_count) {
int extra_bytes;
#ifdef DEBUG
fprintf( stderr, "mb_master_init()\n");
fprintf( stderr, "creating %d nodes\n", nd_count);
#endif
/* initialise layer 1 library */
if (modbus_init(nd_count, DEF_OPTIMIZATION, &extra_bytes) < 0)
goto error_exit_0;
/* initialise this library */
if (mb_master_init__(extra_bytes) < 0)
goto error_exit_1;
return 0;
error_exit_1:
modbus_done();
error_exit_0:
return -1;
}
int mb_master_done(void) {
mb_master_done__();
return modbus_done();
}
#endif
/* Establish a connection to a remote server/slave */
/* NOTE: We use the lower 2 bits of the returned node id to identify which
* layer1 implementation to use.
* 0 -> TCP
* 1 -> RTU
* 2 -> ASCII
* 4 -> unused
* The node id used by the layer1 is shifted left 2 bits
* before returning the node id to the caller!
*/
int mb_master_connect(node_addr_t node_addr) {
int res = -1;
#ifdef DEBUG
fprintf( stderr, "mb_master_tcp connect()\n");
#endif
/* call layer 1 library */
switch(node_addr.naf) {
case naf_tcp:
res = modbus_tcp_connect(node_addr);
if (res >= 0) res = res*4 + 0 /* offset into fptr_ with TCP functions */;
return res;
case naf_rtu:
res = modbus_rtu_connect(node_addr);
if (res >= 0) res = res*4 + 1 /* offset into fptr_ with RTU functions */;
return res;
case naf_ascii:
res = modbus_ascii_connect(node_addr);
if (res >= 0) res = res*4 + 2 /* offset into fptr_ with ASCII functions */;
return res;
}
return -1;
}
/* Shut down a connection to a remote server/slave */
int mb_master_close(int fd) {
#ifdef DEBUG
fprintf( stderr, "mb_master_close(): nd = %d\n", fd);
#endif
get_ttyfd(); /* declare the ttyfd variable, ... */
/* call layer 1 library */
return modbus_close(ttyfd);
}
/* Tell the library that communications will be suspended for some time. */
/* RTU and ASCII versions ignore this function
* TCP version closes all the open tcp connections (connections are automatically
* re-established the next time an IO function to the slave is requested).
* To be more precise, the TCP version makes an estimate of how long
* the silence will be based on previous invocations to this exact same
* function, and will only close the connections if this silence is
* expected to be longer than 1 second!
* (The closing of connections is specified in Modbus specification)
*/
int mb_master_tcp_silence_init(void) {
#ifdef DEBUG
fprintf( stderr, "mb_master_silence_init():\n");
#endif
/* call layer 1 library */
return modbus_tcp_silence_init();
}