msousa@0: /*
msousa@0: * Copyright (c) 2001,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_slave.c */
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 /* required for htons() and ntohs() */
msousa@0: #include "mb_layer1.h"
msousa@0: #include "mb_slave.h"
msousa@0: #include "mb_slave_private.h"
msousa@0:
msousa@0: /* #define DEBUG */ /* uncomment to see the data sent and received */
msousa@0:
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: if (fd < 0) {ttyfd = fd; layer1_fin = 0; /* use modbusTCP */}
msousa@0:
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) implementations will be adding some
msousa@0: * header and tail bytes (e.g. CRC) to the packet we build here. Since
msousa@0: * layer1 will re-use the same buffer allocated in this slave layer
msousa@0: * (so as not to continuosly copy the same info from buffer to buffer),
msousa@0: * we need to allocate more bytes than those strictly required for this
msousa@0: * slave layer. Therefore, the extra_bytes parameter.
msousa@0: *
msousa@0: * Note that we add one more extra byte to the response buffer.
msousa@0: * This is because some response packets will not be starting off
msousa@0: * at byte 0, but rather at byte 1 of the buffer. This is in order
msousa@0: * to guarantee that the data that is sent on the buffer is aligned
msousa@0: * on even bytes (the 16 bit words!). This will allow the application
msousa@0: * (layer above the one implemented in this file - i.e. the callback
msousa@0: * functions) to reference this memory as an u16 *, without producing
msousa@0: * 'bus error' messages in some embedded devices that do not allow
msousa@0: * acessing u16 on odd numbered addresses.
msousa@0: */
msousa@0: static int buff_extra_bytes_;
msousa@0: #define RESP_BUFFER_SIZE (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)
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 could be tempted to use the htons() and
msousa@0: * ntohs() functions to guarantee code portability.
msousa@0: *
msousa@0: * However, on some embedded systems running Linux
msousa@0: * these functions only work if the 16 bit words are
msousa@0: * stored on even addresses. This is not always the
msousa@0: * case in our code, so we have to define our own
msousa@0: * conversion functions...
msousa@0: */
msousa@0:
msousa@0: /* if using gcc, use it to determine byte order... */
msousa@0: #ifndef __BYTE_ORDER
msousa@0: #if defined(__GNUC__)
msousa@0: /* We have GCC, which should define __LITTLE_ENDIAN__ */
msousa@0: # if defined(__LITTLE_ENDIAN__)
msousa@0: # define __BYTE_ORDER __LITTLE_ENDIAN
msousa@0: # else
msousa@0: # define __BYTE_ORDER __BIG_ENDIAN
msousa@0: # endif
msousa@0: #endif /* __GNUC__ */
msousa@0: #endif /* __BYTE_ORDER */
msousa@0:
msousa@0:
msousa@0: /* If we still don't know byte order, try to get it from */
msousa@0: #ifndef __BYTE_ORDER
msousa@0: #include
msousa@0: #endif
msousa@0:
msousa@0:
msousa@0: #ifndef __BYTE_ORDER
msousa@0: # ifdef BYTE_ORDER
msousa@0: # if BYTE_ORDER == LITTLE_ENDIAN
msousa@0: # define __BYTE_ORDER __LITTLE_ENDIAN
msousa@0: # else
msousa@0: # if BYTE_ORDER == BIG_ENDIAN
msousa@0: # define __BYTE_ORDER __BIG_ENDIAN
msousa@0: # endif
msousa@0: # endif
msousa@0: # endif /* BYTE_ORDER */
msousa@0: #endif /* __BYTE_ORDER */
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: #ifdef __BYTE_ORDER
msousa@0: # if __BYTE_ORDER == __LITTLE_ENDIAN
msousa@0:
msousa@0: /**************************************************************/
msousa@0: /* u16 conversion functions to use on little endian platforms */
msousa@0: /**************************************************************/
msousa@0:
blaz@5: /* NOTE: The input parameter must be the address
blaz@5: * of an u16 passed as a pointer to u8
blaz@5: *
blaz@5: * We use u8 *ptr as input parameter and read both (ptr+0) and (ptr+1)
blaz@5: * instead of using u16 *ptr because we sometimes receive data in packtes
blaz@5: * that are not aligned on even addresses, so some compilers recognize that
blaz@5: * the given odd address cannot be used as a pointer to u16 and therefore
blaz@5: * adjust the pointer by (+1) or (-1), basicaly breacking our code!
blaz@5: * So, we revert to u8 pointers... for u16 values.
blaz@5: */
blaz@5: static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
blaz@5: u16_to_ptr[0] = u16_from_ptr[1];
blaz@5: u16_to_ptr[1] = u16_from_ptr[0];
blaz@5: }
blaz@5: #define mb_ntoh(a, b) mb_hton(a, b)
blaz@5:
blaz@5:
blaz@5: static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {
blaz@5: unsigned i;
blaz@5: for (i = 0; i < count*2; i += 2) {
msousa@0: /* swap the bytes around...
msousa@0: * a = a ^ b;
msousa@0: * b = a ^ b;
msousa@0: * a = a ^ b;
msousa@0: */
blaz@5: (u16_ptr+i)[0] ^= (u16_ptr+i)[1];
blaz@5: (u16_ptr+i)[1] ^= (u16_ptr+i)[0];
blaz@5: (u16_ptr+i)[0] ^= (u16_ptr+i)[1];
msousa@0: }
msousa@0: }
msousa@0: #define mb_ntoh_count(w, count) mb_hton_count(w, count)
msousa@0:
msousa@0:
msousa@0:
msousa@0: # else
msousa@0: # if __BYTE_ORDER == __BIG_ENDIAN
msousa@0: /***********************************************************/
msousa@0: /* u16 conversion functions to use on big endian platforms */
msousa@0: /***********************************************************/
msousa@0:
blaz@5: /* We don't need to swap the bytes around! */
blaz@5: static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
blaz@5: u16_to_ptr[0] = u16_from_ptr[0];
blaz@5: u16_to_ptr[1] = u16_from_ptr[1];
blaz@5: }
blaz@5: #define mb_ntoh(a, b) mb_hton(a, b)
blaz@5:
msousa@0: #define mb_hton_count(w, count) /* empty ! */
msousa@0: #define mb_ntoh_count(w, count) /* empty ! */
msousa@0:
msousa@0:
msousa@0: # else
msousa@0: /********************************************************/
msousa@0: /* u16 conversion functions to use on generic platforms */
msousa@0: /********************************************************/
msousa@0:
blaz@5:
blaz@5: /* We can't determine endiannes at compile time, so we do it at runtime.
blaz@5: * With any luck the compiler will be able to determine the result of the
blaz@5: * comparison at compile time and end up discarding the non-used code
blaz@5: * and the 'if' itself from the final executable.
blaz@5: */
blaz@5:
blaz@5: static union {u16 u16;
blaz@5: u8 u8[2];} endian_ = 0x0102;
blaz@5:
blaz@5:
blaz@5: static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
blaz@5: if (endian_.u8[0] == 0x01) {
blaz@5: /* machine is big endian -> no swapping */
blaz@5: u16_to_ptr[0] = u16_from_ptr[0];
blaz@5: u16_to_ptr[1] = u16_from_ptr[1];
blaz@5: } else {
blaz@5: /* machine is little endian -> we swap bytes around */
blaz@5: u16_to_ptr[0] = u16_from_ptr[1];
blaz@5: u16_to_ptr[1] = u16_from_ptr[0];
blaz@5: }
blaz@5: }
blaz@5: #define mb_ntoh(a, b) mb_hton(a, b)
blaz@5:
blaz@5:
blaz@5: static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {
blaz@5: unsigned i;
blaz@5:
blaz@5: if (endian_.u8[0] == 0x01)
blaz@5: /* machine is big endian. Nothing to do */
blaz@5: return;
blaz@5:
blaz@5: /* machine is little endian -> we swap bytes around */
blaz@5: for (i = 0; i < count*2; i += 2) {
blaz@5: /* swap the bytes around...
blaz@5: * a = a ^ b;
blaz@5: * b = a ^ b;
blaz@5: * a = a ^ b;
blaz@5: */
blaz@5: (u16_ptr+i)[0] ^= (u16_ptr+i)[1];
blaz@5: (u16_ptr+i)[1] ^= (u16_ptr+i)[0];
blaz@5: (u16_ptr+i)[0] ^= (u16_ptr+i)[1];
blaz@5: }
blaz@5: }
blaz@5: #define mb_ntoh_count(w, count) mb_hton_count(w, count)
msousa@0:
msousa@0: # endif
msousa@0: # endif
msousa@0: #endif /* __BYTE_ORDER */
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0: /** **/
msousa@0: /** Handle requests from master/client **/
msousa@0: /** **/
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0:
msousa@0:
msousa@0: /* Handle functions 0x01 and 0x02 */
msousa@0: typedef int (*read_bits_callback_t)(void *arg, u16 start_addr, u16 bit_count, u8 *data_bytes);
msousa@0: static int handle_read_bits (u8 *query_packet,
msousa@0: u8 **resp_packet_ptr,
msousa@0: u8 *error_code,
msousa@0: read_bits_callback_t read_bits_callback,
msousa@0: void *callback_arg
msousa@0: ) {
msousa@0: u16 start_addr, count;
msousa@0: int res;
msousa@0: u8 *resp_packet;
msousa@0:
msousa@0: /* If no callback, handle as if function is not supported... */
msousa@0: if (read_bits_callback == NULL)
msousa@0: {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
msousa@0:
msousa@0: /* in oprder for the data in this packet to be aligned on even numbered addresses, this
msousa@0: * response packet will start off at an odd numbered byte...
msousa@0: * We therefore add 1 to the address where the packet starts.
msousa@0: */
msousa@0: (*resp_packet_ptr)++;
msousa@0: resp_packet = *resp_packet_ptr;
msousa@0:
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: */
blaz@5: mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
blaz@5: mb_ntoh(&(query_packet[4]), (u8 *)&count);
msousa@0:
msousa@0: #ifdef DEBUG
msousa@0: printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
msousa@0: query_packet[0], query_packet[1], start_addr, count);
msousa@0: #endif
msousa@0:
msousa@0: if ((count > MAX_READ_BITS) || (count < 1))
msousa@0: {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
msousa@0:
msousa@0: /* Remember, we are using addressing starting off at 0, in the start_addr variable! */
msousa@0: /* This means that he highest acceptable address is 65535, when count=1 .... */
msousa@0: /* Note the use of 65536 in the comparison will force automatic upgrade of u16 variables! */
msousa@0: /* => start_addr + count will nver overflow the u16 type! */
msousa@0: if (start_addr + count > 65536)
msousa@0: {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0:
msousa@0: /* start building response frame... */
msousa@0: resp_packet[0] = query_packet[0]; /* slave */
msousa@0: resp_packet[1] = query_packet[1]; /* function (either 0x01 or 0x02 ! */
msousa@0: resp_packet[2] = (count + 7) / 8; /* number of data bytes = ceil(count/8) */
msousa@0:
msousa@0: res = read_bits_callback(callback_arg, start_addr, count, &(resp_packet[3]));
msousa@0: if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0: if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
msousa@0:
msousa@0: return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0: /* Handle function 0x01 */
msousa@0: int handle_read_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)
msousa@0: {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_outbits, callbacks->arg);}
msousa@0:
msousa@0: /* Handle function 0x02 */
msousa@0: int handle_read_input_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)
msousa@0: {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_inbits, callbacks->arg);}
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: /* Handle functions 0x03 and 0x04 */
msousa@0: typedef int (*read_words_callback_t)(void *arg, u16 start_addr, u16 word_count, u16 *data_words);
msousa@0: static int handle_read_words (u8 *query_packet,
msousa@0: u8 **resp_packet_ptr,
msousa@0: u8 *error_code,
msousa@0: read_words_callback_t read_words_callback,
msousa@0: void *callback_arg
msousa@0: ) {
msousa@0: u16 start_addr, count;
msousa@0: int res;
msousa@0: u8 *resp_packet;
msousa@0:
msousa@0: /* If no callback, handle as if function is not supported... */
msousa@0: if (read_words_callback == NULL)
msousa@0: {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
msousa@0: (*resp_packet_ptr)++;
msousa@0: resp_packet = *resp_packet_ptr;
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
blaz@5: mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
blaz@5: mb_ntoh(&(query_packet[4]), (u8 *)&count);
msousa@0:
msousa@0: #ifdef DEBUG
msousa@0: printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
msousa@0: query_packet[0], query_packet[1], start_addr, count);
msousa@0: #endif
msousa@0:
msousa@0: if ((count > MAX_READ_REGS) || (count < 1))
msousa@0: {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
msousa@0: if (start_addr + count > 65536)
msousa@0: {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0:
msousa@0: /* start building response frame... */
msousa@0: resp_packet[0] = query_packet[0]; /* slave */
msousa@0: resp_packet[1] = query_packet[1]; /* function code, either 0x03 or 0x04 !!!*/
msousa@0: resp_packet[2] = count * 2; /* number of bytes of data... */
msousa@0:
msousa@0: res = read_words_callback(callback_arg, start_addr, count, (u16 *)&(resp_packet[3]));
msousa@0: if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0: if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
msousa@0:
msousa@0: /* convert all data from host to network byte order. */
blaz@5: mb_hton_count(&(resp_packet[3]), count);
msousa@0:
msousa@0: return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: /* Handle function 0x03 */
msousa@0: int handle_read_output_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)
msousa@0: {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_outwords, callbacks->arg);}
msousa@0:
msousa@0: /* Handle function 0x04 */
msousa@0: int handle_read_input_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)
msousa@0: {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_inwords, callbacks->arg);}
msousa@0:
msousa@0:
msousa@0:
msousa@0: /* Handle function 0x05 */
msousa@0: int handle_write_output_bit (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
msousa@0: u16 start_addr;
msousa@0: int res;
msousa@0: u8 *resp_packet;
msousa@0:
msousa@0: /* If no callback, handle as if function is not supported... */
msousa@0: if (callbacks->write_outbits == NULL)
msousa@0: {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
msousa@0:
msousa@0: resp_packet = *resp_packet_ptr;
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
blaz@5: mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
msousa@0:
msousa@0: #ifdef DEBUG
msousa@0: printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n",
msousa@0: query_packet[0], query_packet[1], start_addr);
msousa@0: #endif
msousa@0:
msousa@0: // byte 5 Must be 0x00, byte 4 must be 0x00 or 0xFF !!
msousa@0: if ( (query_packet[5] != 0) ||
msousa@0: ((query_packet[4] != 0) && (query_packet[4] != 0xFF)))
msousa@0: {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
msousa@0:
msousa@0: /* Address will always be valid, no need to check! */
msousa@0: // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0:
msousa@0: /* start building response frame... */
msousa@0: resp_packet[0] = query_packet[0]; /* slave */
msousa@0: resp_packet[1] = query_packet[1]; /* function */
msousa@0: resp_packet[2] = query_packet[2]; /* start address - hi byte */
msousa@0: resp_packet[3] = query_packet[3]; /* start address - lo byte */
msousa@0: resp_packet[4] = query_packet[4]; /* value: 0x00 or 0xFF */
msousa@0: resp_packet[5] = query_packet[5]; /* value: must be 0x00 */
msousa@0:
msousa@0: res = (callbacks->write_outbits)(callbacks->arg, start_addr, 1, &(query_packet[4]));
msousa@0: if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0: if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
msousa@0:
msousa@0: return 6; /* response packet size, including slave id in byte 0 */
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0: /* Handle function 0x06 */
msousa@0: int handle_write_output_word (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
msousa@0: u16 start_addr;
msousa@0: int res;
msousa@0: u8 *resp_packet;
msousa@0:
msousa@0: /* If no callback, handle as if function is not supported... */
msousa@0: if (callbacks->write_outwords == NULL)
msousa@0: {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
msousa@0:
msousa@0: resp_packet = *resp_packet_ptr;
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
blaz@5: mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
blaz@5:
msousa@0: #ifdef DEBUG
msousa@0: printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n",
msousa@0: query_packet[0], query_packet[1], start_addr);
msousa@0: #endif
msousa@0:
msousa@0: /* Address will always be valid, no need to check! */
msousa@0: // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0:
msousa@0: /* start building response frame... */
msousa@0: resp_packet[0] = query_packet[0]; /* slave */
msousa@0: resp_packet[1] = query_packet[1]; /* function */
msousa@0: resp_packet[2] = query_packet[2]; /* start address - hi byte */
msousa@0: resp_packet[3] = query_packet[3]; /* start address - lo byte */
msousa@0: resp_packet[4] = query_packet[4]; /* value - hi byte */
msousa@0: resp_packet[5] = query_packet[5]; /* value - lo byte */
msousa@0:
msousa@0: /* convert data from network to host byte order */
blaz@5: mb_ntoh_count(&(query_packet[4]), 1);
msousa@0:
msousa@0: res = (callbacks->write_outwords)(callbacks->arg, start_addr, 1, (u16 *)&(query_packet[4]));
msousa@0: if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0: if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
msousa@0:
msousa@0: return 6; /* packet size is 6 -> slave, function, addr(2), value(2) */
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0: /* Handle function 0x0F */
msousa@0: int handle_write_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
msousa@0: u16 start_addr, count;
msousa@0: int res;
msousa@0: u8 *resp_packet;
msousa@0:
msousa@0: /* If no callback, handle as if function is not supported... */
msousa@0: if (callbacks->write_outbits == NULL)
msousa@0: {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
msousa@0:
msousa@0: resp_packet = *resp_packet_ptr;
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
blaz@5: mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
blaz@5: mb_ntoh(&(query_packet[4]), (u8 *)&count);
msousa@0:
msousa@0: #ifdef DEBUG
msousa@0: printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
msousa@0: query_packet[0], query_packet[1], start_addr, count);
msousa@0: #endif
msousa@0:
msousa@0: if ((count > MAX_WRITE_COILS) || (count < 1) || ((count+7)/8 != query_packet[6]) )
msousa@0: {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
msousa@0: if (start_addr + count > 65536)
msousa@0: {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0:
msousa@0: /* start building response frame... */
msousa@0: resp_packet[0] = query_packet[0]; /* slave */
msousa@0: resp_packet[1] = query_packet[1]; /* function */
msousa@0: resp_packet[2] = query_packet[2]; /* start address - hi byte */
msousa@0: resp_packet[3] = query_packet[3]; /* start address - lo byte */
msousa@0: resp_packet[4] = query_packet[4]; /* count - hi byte */
msousa@0: resp_packet[5] = query_packet[5]; /* count - lo byte */
msousa@0:
msousa@0: res = (callbacks->write_outbits)(callbacks->arg, start_addr, count, &(query_packet[7]));
msousa@0: if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0: if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
msousa@0:
msousa@0: return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: /* Handle function 0x10 */
msousa@0: int handle_write_output_words(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
msousa@0: u16 start_addr, count;
msousa@0: int res;
msousa@0: u8 *resp_packet;
msousa@0:
msousa@0: /* If no callback, handle as if function is not supported... */
msousa@0: if (callbacks->write_outwords == NULL)
msousa@0: {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
msousa@0:
msousa@0: resp_packet = *resp_packet_ptr;
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
blaz@5: mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
blaz@5: mb_ntoh(&(query_packet[4]), (u8 *)&count);
blaz@5:
msousa@0: if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) )
msousa@0: {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
msousa@0:
msousa@0: /* See equivalent comment in handle_read_bits() */
msousa@0: if (start_addr + count > 65536)
msousa@0: {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0:
msousa@0: /* start building response frame... */
msousa@0: resp_packet[0] = query_packet[0]; /* slave */
msousa@0: resp_packet[1] = query_packet[1]; /* function */
msousa@0: resp_packet[2] = query_packet[2]; /* start address - hi byte */
msousa@0: resp_packet[3] = query_packet[3]; /* start address - lo byte */
msousa@0: resp_packet[4] = query_packet[4]; /* count - hi byte */
msousa@0: resp_packet[5] = query_packet[5]; /* count - lo byte */
msousa@0:
msousa@0: /* convert all data from network to host byte order */
blaz@5: mb_ntoh_count(&(query_packet[7]), count);
msousa@0:
msousa@0: res = (callbacks->write_outwords)(callbacks->arg, start_addr, count, (u16 *)&(query_packet[7]));
msousa@0: if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
msousa@0: if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
msousa@0:
msousa@0: return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0: /** **/
msousa@0: /** initialise / shutdown the library **/
msousa@0: /** **/
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0:
msousa@0: int mb_slave_init__(int extra_bytes) {
msousa@0: buff_extra_bytes_ = extra_bytes;
msousa@0: return 0;
msousa@0: }
msousa@0:
msousa@0:
msousa@0: int mb_slave_done__(void)
msousa@0: {return 0;}
msousa@0:
msousa@0:
msousa@0: #if 0
msousa@0: int mb_slave_init(int nd_count) {
msousa@0: int extra_bytes;
msousa@0:
msousa@0: #ifdef DEBUG
msousa@0: fprintf( stderr, "mb_slave_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_slave_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_slave_done(void) {
msousa@0: mb_slave_done__(void)
msousa@0: return modbus_done();
msousa@0: }
msousa@0: #endif
msousa@0:
msousa@0:
msousa@0:
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0: /** **/
msousa@0: /** open/close slave connection **/
msousa@0: /** **/
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0:
msousa@0: /* Create a new slave/server */
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_slave_new(node_addr_t node_addr) {
msousa@0: int res = -1;
msousa@0: #ifdef DEBUG
msousa@0: fprintf( stderr, "mb_slave_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_listen(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_listen(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_listen(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: int mb_slave_close(int fd) {
msousa@0: #ifdef DEBUG
msousa@0: fprintf( stderr, "mb_slave_close(): nd = %d\n", fd);
msousa@0: #endif
msousa@0: get_ttyfd(); /* declare the ttyfd variable!! */
msousa@0: /* call layer 1 library */
msousa@0: /* will call one of modbus_tcp_close(), modbus_rtu_close(), modbus_ascii_close() */
msousa@0: return modbus_close(ttyfd);
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0: /** **/
msousa@0: /** Run the slave **/
msousa@0: /** **/
msousa@0: /***********************************************/
msousa@0: /***********************************************/
msousa@0:
msousa@0: /* Execute infinite loop waiting and replying to requests coming from clients/master
msousa@0: * This function enters an infinite loop wating for new connection requests,
msousa@0: * and for modbus requests over previoulsy open connections...
msousa@0: *
msousa@0: * The frames are read from:
msousa@0: * - the node descriptor nd, if nd >= 0
msousa@0: * When using TCP, if the referenced node nd was created to listen for new connections
msousa@0: * [mb_slave_listen()], then this function will also reply to Modbus data requests arriving
msousa@0: * on other nodes that were created as a consequence of accepting connections requests to
msousa@0: * the referenced node nd.
msousa@0: * All other nodes are ignored!
msousa@0: *
msousa@0: * - any valid and initialised TCP node descriptor, if nd = -1
msousa@0: * In this case, will also accept connection requests arriving from a previously
msousa@0: * created node to listen for new connection requests [mb_slave_listen() ].
msousa@0: * NOTE: (only avaliable if using TCP)
msousa@0: *
msousa@0: * slaveid identifies the address (RTU and ASCII) or slaveid (TCP) that we implement.
msousa@0: * Any requests that we receive sent with a slaveid different
msousa@0: * than the one specified, and also different to 0, will be silently ignored!
msousa@0: * Whatever the slaveid specified, we always reply to requests
msousa@0: * to slaveid 0 (the modbus broadcast address).
msousa@0: * Calling this function with a slaveid of 0 means to ignore this
msousa@0: * parameter and to reply to all requests (whatever the slaveid
msousa@0: * used in the request). This should mostly be used by TCP servers...
msousa@0: */
msousa@0:
msousa@0: int mb_slave_run(int fd, mb_slave_callback_t callback_functions, u8 slaveid) {
msousa@0: int byte_count;
msousa@0: u16 transaction_id;
msousa@0: int nd;
msousa@0: u8 function, error_code = 0;
msousa@0: int resp_length;
msousa@0: u8 *query_packet = NULL;
msousa@0: u8 *resp_packet;
msousa@0: u8 resp_buffer_[RESP_BUFFER_SIZE];
msousa@0: u8 slave;
msousa@0:
msousa@0: get_ttyfd(); /* declare the ttyfd variable!! */
msousa@0:
msousa@0: #ifdef DEBUG
msousa@0: fprintf(stderr,"[%lu] mb_slave_run(): Called... fd=%d, ttyfd=%d\n", pthread_self(), fd, ttyfd);
msousa@0: #endif
msousa@0:
msousa@0: while(1) {
msousa@0: nd = ttyfd;
msousa@0: /* will call one of modbus_tcp_read(), modbus_rtu_read(), modbus_ascii_read() */
msousa@0: do {
msousa@0: byte_count = modbus_read(&nd, /* node descriptor */
msousa@0: &query_packet, /* u8 **recv_data_ptr, */
msousa@0: &transaction_id, /* u16 *transaction_id, */
msousa@0: NULL, /* const u8 *send_data, */
msousa@0: 0, /* int send_length, */
msousa@0: NULL /* wait indefenitely */ /* const struct timespec *recv_timeout); */
msousa@0: );
msousa@0: } while (byte_count <= 2);
msousa@0:
msousa@0: #ifdef DEBUG
msousa@0: {/* display the hex code of each character received */
msousa@0: int i;
msousa@0: printf("[%lu] mb_slave_run() received %d bytes (ptr=%p): \n", pthread_self(), byte_count, query_packet);
msousa@0: for (i=0; i < byte_count; i++)
msousa@0: printf("<0x%2X>", query_packet[i]);
msousa@0: printf("\n");
msousa@0: }
msousa@0: #endif
msousa@0:
msousa@0: slave = query_packet[0];
msousa@0: function = query_packet[1];
msousa@0:
msousa@0: /* We only reply if:
msousa@0: * - request was sent to broadcast address (slave == 0)
msousa@0: * OR - we were asked to reply to every request (slaveid == 0)
msousa@0: * OR - request matches the slaveid we were asked to accept (slave == slaveid)
msousa@0: *
msousa@0: * Otherwise, silently ignore the received request!!!
msousa@0: */
msousa@0: if ((slaveid == 0) || (slave == 0) || (slave == slaveid)) {
msousa@0: resp_packet = resp_buffer_;
msousa@0:
msousa@0: switch(function) {
msousa@0: case 0x01: resp_length = handle_read_output_bits (query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: case 0x02: resp_length = handle_read_input_bits (query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: case 0x03: resp_length = handle_read_output_words (query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: case 0x04: resp_length = handle_read_input_words (query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: case 0x05: resp_length = handle_write_output_bit (query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: case 0x06: resp_length = handle_write_output_word (query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: case 0x0F: resp_length = handle_write_output_bits (query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: case 0x10: resp_length = handle_write_output_words(query_packet, &resp_packet, &error_code, &callback_functions); break;
msousa@0: /* return exception code 0x01 -> function not supported! */
msousa@0: default : resp_length = -1; error_code = 0x01; break;
msousa@0: }; /* switch(function) */
msousa@0:
msousa@0: if (resp_length < 0) {
msousa@0: /* return error... */
msousa@0: /* build exception response frame... */
msousa@0: resp_packet = resp_buffer_;
msousa@0: resp_packet[0] = query_packet[0]; /* slave */
msousa@0: resp_packet[1] = query_packet[1] | 0x80; /* function code with error bit activated! */
msousa@0: resp_packet[2] = error_code;
msousa@0: resp_length = 3;
msousa@0: }
msousa@0: modbus_write(nd, resp_packet, resp_length, transaction_id, NULL /*transmit_timeout*/);
msousa@0: }; /* if not ignore request */
msousa@0: }; /* while(1) */
msousa@0:
msousa@0: /* humour the compiler... */
msousa@0: return 0;
msousa@0: }
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0:
msousa@0: