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