Modbus: comparison mb

equal deleted inserted replaced

--1:000000000000
+:ae252e0fd9b8
+/*
+* 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.
+*/
+inline 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.
+*/
+inline 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.
+*/
+inline 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.
+*/
+inline 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 */
+inline 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.
+*/
+inline 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
+*/
+inline 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 */
+inline 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.
+*/
+inline 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
+*/
+inline 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 */
+inline 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 */
+inline 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();
+}

changeset 0	ae252e0fd9b8
child 1	59783e8ee3d2