|
|
@@ -0,0 +1,1334 @@
|
|
|
+/*
|
|
|
+ Copyright (C) 2001-2005 Stéphane Raimbault <stephane.raimbault@free.fr>
|
|
|
+
|
|
|
+ This 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 2 of the License, or (at your option) any later version.
|
|
|
+
|
|
|
+ This library 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 library; if not, write to the Free Software
|
|
|
+ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
|
|
|
+
|
|
|
+ The functions included here have been derived from the Modicon
|
|
|
+ Modbus Protocol Reference Guide which can be obtained from
|
|
|
+ Schneider at www.schneiderautomation.com.
|
|
|
+
|
|
|
+ Documentation:
|
|
|
+ http://www.easysw.com/~mike/serial/serial.html
|
|
|
+*/
|
|
|
+
|
|
|
+#include <fcntl.h>
|
|
|
+#include <stdio.h>
|
|
|
+#include <string.h>
|
|
|
+#include <stdlib.h>
|
|
|
+#include <termio.h>
|
|
|
+#include <sys/time.h>
|
|
|
+#include <unistd.h>
|
|
|
+#include <errno.h>
|
|
|
+#include <glib.h>
|
|
|
+
|
|
|
+/* TCP */
|
|
|
+#include <sys/types.h>
|
|
|
+#include <sys/socket.h>
|
|
|
+#include <sys/ioctl.h>
|
|
|
+#include <netinet/in.h>
|
|
|
+#include <netinet/ip.h>
|
|
|
+#include <netinet/tcp.h>
|
|
|
+
|
|
|
+#include "modbus.h"
|
|
|
+
|
|
|
+#define UNKNOWN_ERROR_MSG "Not defined in modbus specification"
|
|
|
+
|
|
|
+static const int SIZE_TAB_ERROR_MSG = 12;
|
|
|
+static const char *TAB_ERROR_MSG[] = {
|
|
|
+ /* 0x00 */ UNKNOWN_ERROR_MSG,
|
|
|
+ /* 0x01 */ "Illegal function code",
|
|
|
+ /* 0x02 */ "Illegal data address",
|
|
|
+ /* 0x03 */ "Illegal data value",
|
|
|
+ /* 0x04 */ "Slave device or server failure",
|
|
|
+ /* 0x05 */ "Acknowledge",
|
|
|
+ /* 0x06 */ "Slave device or server busy",
|
|
|
+ /* 0x07 */ "Negative acknowledge",
|
|
|
+ /* 0x08 */ "Memory parity error",
|
|
|
+ /* 0x09 */ UNKNOWN_ERROR_MSG,
|
|
|
+ /* 0x0A */ "Gateway path unavailable",
|
|
|
+ /* 0x0B */ "Target device failed to respond"
|
|
|
+};
|
|
|
+
|
|
|
+/* Table of CRC values for high-order byte */
|
|
|
+static unsigned char table_crc_hi[] = {
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
|
|
|
+ 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
|
|
|
+ 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
|
|
|
+ 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
|
|
|
+ 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
|
|
|
+ 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
|
|
|
+ 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
|
|
|
+ 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
|
|
|
+ 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
|
|
|
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
|
|
|
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
|
|
|
+};
|
|
|
+
|
|
|
+/* Table of CRC values for low-order byte */
|
|
|
+static unsigned char table_crc_lo[] = {
|
|
|
+ 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
|
|
|
+ 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
|
|
|
+ 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
|
|
|
+ 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
|
|
|
+ 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
|
|
|
+ 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
|
|
|
+ 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
|
|
|
+ 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
|
|
|
+ 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
|
|
|
+ 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
|
|
|
+ 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
|
|
|
+ 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
|
|
|
+ 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
|
|
|
+ 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
|
|
|
+ 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
|
|
|
+ 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
|
|
|
+ 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
|
|
|
+ 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
|
|
|
+ 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
|
|
|
+ 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
|
|
|
+ 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
|
|
|
+ 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
|
|
|
+ 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
|
|
|
+ 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
|
|
|
+ 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
|
|
|
+ 0x43, 0x83, 0x41, 0x81, 0x80, 0x40
|
|
|
+};
|
|
|
+
|
|
|
+/* Local declaration */
|
|
|
+static int read_reg_response(modbus_param_t *mb_param,
|
|
|
+ int *data_dest,
|
|
|
+ unsigned char *query);
|
|
|
+
|
|
|
+static void error_treat(int ret, const char *message, modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ if (ret == -1)
|
|
|
+ perror(message);
|
|
|
+ g_print("\n\nERROR %s\n\n", message);
|
|
|
+
|
|
|
+ if (mb_param->type_com == RTU) {
|
|
|
+ tcflush(mb_param->fd, TCIOFLUSH);
|
|
|
+ } else {
|
|
|
+ modbus_close(mb_param);
|
|
|
+ modbus_connect(mb_param);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static unsigned int compute_response_size(modbus_param_t *mb_param,
|
|
|
+ unsigned char *query)
|
|
|
+{
|
|
|
+ int response_size_computed;
|
|
|
+ int offset;
|
|
|
+
|
|
|
+ offset = mb_param->header_length;
|
|
|
+
|
|
|
+ switch (query[offset + 1]) {
|
|
|
+ case 0x01: {
|
|
|
+ /* Header + nb values (code from force_multiple_coils) */
|
|
|
+ int coil_count = (query[offset + 4] << 8) | query[offset + 5];
|
|
|
+ response_size_computed = 3 +
|
|
|
+ (coil_count / 8) + ((coil_count % 8) ? 1 : 0);
|
|
|
+ } break;
|
|
|
+ case 0x03:
|
|
|
+ /* Header + 2 * nb values */
|
|
|
+ response_size_computed = 3 +
|
|
|
+ 2 * (query[offset + 4] << 8 | query[offset + 5]);
|
|
|
+ break;
|
|
|
+ case 0x07:
|
|
|
+ response_size_computed = 4;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ response_size_computed = 6;
|
|
|
+ }
|
|
|
+
|
|
|
+ response_size_computed += offset + mb_param->checksum_size;
|
|
|
+
|
|
|
+ return response_size_computed;
|
|
|
+}
|
|
|
+
|
|
|
+/* The following functions construct the required query into
|
|
|
+ a modbus query packet */
|
|
|
+int build_request_packet_rtu(int slave, int function, int start_addr,
|
|
|
+ int count, unsigned char *packet)
|
|
|
+{
|
|
|
+ packet[0] = slave;
|
|
|
+ packet[1] = function;
|
|
|
+ packet[2] = start_addr >> 8;
|
|
|
+ packet[3] = start_addr & 0x00ff;
|
|
|
+ packet[4] = count >> 8;
|
|
|
+ packet[5] = count & 0x00ff;
|
|
|
+
|
|
|
+ return PRESET_QUERY_SIZE_RTU;
|
|
|
+}
|
|
|
+
|
|
|
+int build_request_packet_tcp(int slave, int function, int start_addr,
|
|
|
+ int count, unsigned char *packet)
|
|
|
+{
|
|
|
+ static unsigned short t_id = 0;
|
|
|
+
|
|
|
+ /* Transaction ID */
|
|
|
+ if (t_id < USHRT_MAX)
|
|
|
+ t_id++;
|
|
|
+ else
|
|
|
+ t_id = 0;
|
|
|
+ packet[0] = t_id >> 8;
|
|
|
+ packet[1] = t_id & 0x00ff;
|
|
|
+
|
|
|
+ /* Protocol Modbus */
|
|
|
+ packet[2] = 0;
|
|
|
+ packet[3] = 0;
|
|
|
+
|
|
|
+ /* Length to fix later with set_packet_length_tcp (4 and 5) */
|
|
|
+
|
|
|
+ packet[6] = 0xFF;
|
|
|
+ packet[7] = function;
|
|
|
+ packet[8] = start_addr >> 8;
|
|
|
+ packet[9] = start_addr & 0x00ff;
|
|
|
+ packet[10] = count >> 8;
|
|
|
+ packet[11] = count & 0x00ff;
|
|
|
+
|
|
|
+ return PRESET_QUERY_SIZE_TCP;
|
|
|
+}
|
|
|
+
|
|
|
+void set_packet_length_tcp(unsigned char *packet, size_t packet_size)
|
|
|
+{
|
|
|
+ unsigned short mbap_length;
|
|
|
+
|
|
|
+ /* Substract MBAP header length */
|
|
|
+ mbap_length = packet_size - 6;
|
|
|
+
|
|
|
+ packet[4] = mbap_length >> 8;
|
|
|
+ packet[5] = mbap_length & 0x00FF;
|
|
|
+}
|
|
|
+
|
|
|
+int build_request_packet(modbus_param_t *mb_param, int slave,
|
|
|
+ int function, int start_addr,
|
|
|
+ int count, unsigned char *packet)
|
|
|
+{
|
|
|
+ if (mb_param->type_com == RTU)
|
|
|
+ return build_request_packet_rtu(slave, function, start_addr,
|
|
|
+ count, packet);
|
|
|
+ else
|
|
|
+ return build_request_packet_tcp(slave, function, start_addr,
|
|
|
+ count, packet);
|
|
|
+}
|
|
|
+
|
|
|
+/* Fast CRC */
|
|
|
+static unsigned short crc16(unsigned char *buffer,
|
|
|
+ unsigned short buffer_length)
|
|
|
+{
|
|
|
+ unsigned char crc_hi = 0xFF; /* high CRC byte initialized */
|
|
|
+ unsigned char crc_lo = 0xFF; /* low CRC byte initialized */
|
|
|
+ unsigned int i; /* will index into CRC lookup */
|
|
|
+
|
|
|
+ /* pass through message buffer */
|
|
|
+ while (buffer_length--) {
|
|
|
+ i = crc_hi ^ *buffer++; /* calculate the CRC */
|
|
|
+ crc_hi = crc_lo ^ table_crc_hi[i];
|
|
|
+ crc_lo = table_crc_lo[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ return (crc_hi << 8 | crc_lo);
|
|
|
+}
|
|
|
+
|
|
|
+/* Function to send a query out to a modbus slave */
|
|
|
+static int modbus_query(modbus_param_t *mb_param, unsigned char *query,
|
|
|
+ size_t query_size)
|
|
|
+{
|
|
|
+ int write_ret;
|
|
|
+ unsigned short s_crc;
|
|
|
+ int i;
|
|
|
+
|
|
|
+ if (mb_param->type_com == RTU) {
|
|
|
+ s_crc = crc16(query, query_size);
|
|
|
+ query[query_size++] = s_crc >> 8;
|
|
|
+ query[query_size++] = s_crc & 0x00FF;
|
|
|
+ } else {
|
|
|
+ set_packet_length_tcp(query, query_size);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (mb_param->debug) {
|
|
|
+ g_print("\n");
|
|
|
+ for (i = 0; i < query_size; i++)
|
|
|
+ g_print("[%.2X]", query[i]);
|
|
|
+
|
|
|
+ g_print("\n");
|
|
|
+ }
|
|
|
+
|
|
|
+ if (mb_param->type_com == RTU)
|
|
|
+ write_ret = write(mb_param->fd, query, query_size);
|
|
|
+ else
|
|
|
+ write_ret = send(mb_param->fd, query, query_size, 0);
|
|
|
+
|
|
|
+ /* Return the number of bytes written (0 to n)
|
|
|
+ or PORT_SOCKET_FAILURE on error */
|
|
|
+ if ((write_ret == -1) || (write_ret != query_size)) {
|
|
|
+ error_treat(write_ret, "Write port/socket failure", mb_param);
|
|
|
+ write_ret = PORT_SOCKET_FAILURE;
|
|
|
+ }
|
|
|
+
|
|
|
+ return write_ret;
|
|
|
+}
|
|
|
+
|
|
|
+#define WAIT_DATA() \
|
|
|
+{ \
|
|
|
+ while ((select_ret = select(mb_param->fd+1, &rfds, NULL, NULL, &tv)) == -1) { \
|
|
|
+ if (errno == EINTR) { \
|
|
|
+ g_print("A non blocked signal was caught\n"); \
|
|
|
+ /* Necessary after an error */ \
|
|
|
+ FD_ZERO(&rfds); \
|
|
|
+ FD_SET(mb_param->fd, &rfds); \
|
|
|
+ } else { \
|
|
|
+ error_treat(select_ret, "Select failure", mb_param); \
|
|
|
+ return SELECT_FAILURE; \
|
|
|
+ } \
|
|
|
+ } \
|
|
|
+ \
|
|
|
+ if (select_ret == 0) { \
|
|
|
+ /* Call to error_treat is done later to manage exceptions */ \
|
|
|
+ return COMM_TIME_OUT; \
|
|
|
+ } \
|
|
|
+}
|
|
|
+
|
|
|
+/* Function to monitor for the reply from the modbus slave.
|
|
|
+ This function blocks for timeout seconds if there is no reply.
|
|
|
+
|
|
|
+ Returns:
|
|
|
+ - error_code 0 == OK, < 0 == error
|
|
|
+ - (arg) total number of characters received.
|
|
|
+*/
|
|
|
+int receive_response(modbus_param_t *mb_param,
|
|
|
+ int response_size_computed,
|
|
|
+ unsigned char *response,
|
|
|
+ int *response_size)
|
|
|
+{
|
|
|
+ int select_ret;
|
|
|
+ int read_ret;
|
|
|
+ fd_set rfds;
|
|
|
+ struct timeval tv;
|
|
|
+ int size_to_read;
|
|
|
+ unsigned char *p_response;
|
|
|
+
|
|
|
+ if (mb_param->debug)
|
|
|
+ g_print("Waiting for response (%d)...\n", response_size_computed);
|
|
|
+
|
|
|
+ /* Add a file descriptor to the set */
|
|
|
+ FD_ZERO(&rfds);
|
|
|
+ FD_SET(mb_param->fd, &rfds);
|
|
|
+
|
|
|
+ /* Wait for a response */
|
|
|
+ tv.tv_sec = 0;
|
|
|
+ tv.tv_usec = TIME_OUT_BEGIN_OF_TRAME;
|
|
|
+
|
|
|
+ WAIT_DATA();
|
|
|
+
|
|
|
+ /* Read the trame */
|
|
|
+ (*response_size) = 0;
|
|
|
+ size_to_read = response_size_computed;
|
|
|
+ p_response = response;
|
|
|
+
|
|
|
+ while (select_ret) {
|
|
|
+ if (mb_param->type_com == RTU)
|
|
|
+ read_ret = read(mb_param->fd, p_response, size_to_read);
|
|
|
+ else
|
|
|
+ read_ret = recv(mb_param->fd, p_response, size_to_read, 0);
|
|
|
+
|
|
|
+ if (read_ret == -1) {
|
|
|
+ error_treat(read_ret, "Read port/socket failure", mb_param);
|
|
|
+ return PORT_SOCKET_FAILURE;
|
|
|
+ } else {
|
|
|
+ /* Sums bytes received */
|
|
|
+ (*response_size) += read_ret;
|
|
|
+
|
|
|
+ /* Display the hex code of each
|
|
|
+ * character received */
|
|
|
+ if (mb_param->debug) {
|
|
|
+ int i;
|
|
|
+ for (i=0; i < read_ret; i++)
|
|
|
+ g_print("<%.2X>", p_response[i]);
|
|
|
+ }
|
|
|
+ /* Moves the pointer to receive other datas */
|
|
|
+ p_response = &(p_response[read_ret]);
|
|
|
+ size_to_read = response_size_computed - (*response_size);
|
|
|
+
|
|
|
+ if ((*response_size) > MAX_PACKET_SIZE) {
|
|
|
+ error_treat(0, "Too many datas", mb_param);
|
|
|
+ return TOO_MANY_DATAS;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (size_to_read > 0) {
|
|
|
+ /* If no character at the buffer wait
|
|
|
+ TIME_OUT_END_OF_TRAME before to generate an error.
|
|
|
+ */
|
|
|
+ tv.tv_sec = 0;
|
|
|
+ tv.tv_usec = TIME_OUT_END_OF_TRAME;
|
|
|
+
|
|
|
+ WAIT_DATA();
|
|
|
+ } else {
|
|
|
+ /* All chars are received */
|
|
|
+ select_ret = FALSE;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (mb_param->debug)
|
|
|
+ g_print("\n");
|
|
|
+
|
|
|
+ /* OK */
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int check_crc16(modbus_param_t *mb_param,
|
|
|
+ unsigned char *response,
|
|
|
+ int response_size)
|
|
|
+{
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ if (mb_param->type_com == RTU) {
|
|
|
+ unsigned short crc_calc;
|
|
|
+ unsigned short crc_received;
|
|
|
+ unsigned char recv_crc_hi;
|
|
|
+ unsigned char recv_crc_lo;
|
|
|
+
|
|
|
+ crc_calc = crc16(response, response_size - 2);
|
|
|
+
|
|
|
+ recv_crc_hi = (unsigned) response[response_size - 2];
|
|
|
+ recv_crc_lo = (unsigned) response[response_size - 1];
|
|
|
+
|
|
|
+ crc_received = response[response_size - 2];
|
|
|
+ crc_received = (unsigned) crc_received << 8;
|
|
|
+ crc_received = crc_received |
|
|
|
+ (unsigned) response[response_size - 1];
|
|
|
+
|
|
|
+ /* Check CRC of response */
|
|
|
+ if (crc_calc == crc_received) {
|
|
|
+ ret = TRUE;
|
|
|
+ } else {
|
|
|
+ char *message;
|
|
|
+ message = g_strdup_printf(
|
|
|
+ "invalid crc received %0X - crc_calc %0X",
|
|
|
+ crc_received, crc_calc);
|
|
|
+ error_treat(0, message, mb_param);
|
|
|
+ g_free(message);
|
|
|
+ ret = INVALID_CRC;
|
|
|
+ }
|
|
|
+ } else {
|
|
|
+ /* In TCP, CRC doesn't exist but it doesn't check
|
|
|
+ length because it's not really useful */
|
|
|
+ ret = TRUE;
|
|
|
+ }
|
|
|
+
|
|
|
+ return ret;
|
|
|
+}
|
|
|
+
|
|
|
+/* Function to the correct response is returned and that the checksum
|
|
|
+ is correct.
|
|
|
+
|
|
|
+ Returns:
|
|
|
+ - the numbers of values (bits or word) if success
|
|
|
+ - less than 0 for exception errors
|
|
|
+
|
|
|
+ Note: All functions used for sending or receiving data via modbus
|
|
|
+ return these values.
|
|
|
+*/
|
|
|
+
|
|
|
+static int modbus_response(modbus_param_t *mb_param,
|
|
|
+ unsigned char *query,
|
|
|
+ unsigned char *response)
|
|
|
+{
|
|
|
+ int response_size;
|
|
|
+ int response_size_computed;
|
|
|
+ int offset = mb_param->header_length;
|
|
|
+ int error_code;
|
|
|
+
|
|
|
+ response_size_computed = compute_response_size(mb_param, query);
|
|
|
+ error_code = receive_response(mb_param, response_size_computed,
|
|
|
+ response, &response_size);
|
|
|
+ if (error_code == 0) {
|
|
|
+ /* No error */
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ ret = check_crc16(mb_param, response, response_size);
|
|
|
+ if (ret != TRUE)
|
|
|
+ return ret;
|
|
|
+
|
|
|
+ /* Good response */
|
|
|
+ switch (response[offset + 1]) {
|
|
|
+ case 0x01:
|
|
|
+ case 0x02:
|
|
|
+ /* Read functions 1 value = 1 byte */
|
|
|
+ response_size = response[offset + 2];
|
|
|
+ break;
|
|
|
+ case 0x03:
|
|
|
+ case 0x04:
|
|
|
+ /* Read functions 1 value = 2 bytes */
|
|
|
+ response_size = response[offset + 2] / 2;
|
|
|
+ break;
|
|
|
+ case 0x0F:
|
|
|
+ case 0x10:
|
|
|
+ /* N Write functions */
|
|
|
+ response_size = response[offset + 4] << 8 |
|
|
|
+ response[offset + 5];
|
|
|
+ break;
|
|
|
+ case 0x11:
|
|
|
+ /* Report slave ID (bytes received) */
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ /* 1 Write functions & others */
|
|
|
+ response_size = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ } else if (error_code == COMM_TIME_OUT &&
|
|
|
+ response_size == offset + 3 + mb_param->checksum_size) {
|
|
|
+ /* Optimisation allowed because exception response is
|
|
|
+ the smallest trame in modbus protocol (3) so always
|
|
|
+ raise an timeout error */
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ /* CRC */
|
|
|
+ ret = check_crc16(mb_param, response, response_size);
|
|
|
+ if (ret != TRUE)
|
|
|
+ return ret;
|
|
|
+
|
|
|
+ /* Check for exception response
|
|
|
+ 0x80 + function */
|
|
|
+ if (0x80 + query[offset + 1] == response[offset + 1]) {
|
|
|
+
|
|
|
+ if (response[offset + 2] < SIZE_TAB_ERROR_MSG) {
|
|
|
+ error_treat(0,
|
|
|
+ TAB_ERROR_MSG[response[offset + 2]],
|
|
|
+ mb_param);
|
|
|
+ /* Modbus error code (negative) */
|
|
|
+ return -response[offset + 2];
|
|
|
+ } else {
|
|
|
+ /* The chances are low to hit this
|
|
|
+ case but can avoid a vicious
|
|
|
+ segfault */
|
|
|
+ char *message;
|
|
|
+ message = g_strdup_printf(
|
|
|
+ "Invalid exception code %d",
|
|
|
+ response[offset + 2]);
|
|
|
+ error_treat(0, message, mb_param);
|
|
|
+ g_free(message);
|
|
|
+ return INVALID_EXCEPTION_CODE;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ } else if (error_code == COMM_TIME_OUT) {
|
|
|
+ error_treat(0, "Communication time out", mb_param);
|
|
|
+ return COMM_TIME_OUT;
|
|
|
+ } else {
|
|
|
+ return error_code;
|
|
|
+ }
|
|
|
+
|
|
|
+ return response_size;
|
|
|
+}
|
|
|
+
|
|
|
+/* Read IO status */
|
|
|
+static int read_io_status(modbus_param_t *mb_param, int slave, int function,
|
|
|
+ int start_addr, int count, int *data_dest)
|
|
|
+{
|
|
|
+ int query_size;
|
|
|
+ int query_ret;
|
|
|
+ int response_ret;
|
|
|
+
|
|
|
+ unsigned char query[MIN_QUERY_SIZE];
|
|
|
+ unsigned char response[MAX_PACKET_SIZE];
|
|
|
+
|
|
|
+ query_size = build_request_packet(mb_param, slave, function,
|
|
|
+ start_addr, count, query);
|
|
|
+
|
|
|
+ query_ret = modbus_query(mb_param, query, query_size);
|
|
|
+ if (query_ret > 0) {
|
|
|
+ int i, temp, bit;
|
|
|
+ int pos = 0;
|
|
|
+ int processed = 0;
|
|
|
+ int offset;
|
|
|
+ int offset_length;
|
|
|
+
|
|
|
+ response_ret = modbus_response(mb_param, query, response);
|
|
|
+ offset = mb_param->header_length;
|
|
|
+
|
|
|
+ offset_length = offset + response_ret;
|
|
|
+ for (i = offset; i < offset_length; i++) {
|
|
|
+ /* Shift reg hi_byte to temp */
|
|
|
+ temp = response[3 + i];
|
|
|
+
|
|
|
+ for (bit = 0x01;
|
|
|
+ (bit & 0xff) && (processed < count);) {
|
|
|
+ data_dest[pos++] =
|
|
|
+ (temp & bit) ? TRUE : FALSE;
|
|
|
+ processed++;
|
|
|
+ bit = bit << 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ } else {
|
|
|
+ response_ret = query_ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ return response_ret;
|
|
|
+}
|
|
|
+
|
|
|
+/* Reads the boolean status of coils and sets the array elements
|
|
|
+ in the destination to TRUE or FALSE */
|
|
|
+int read_coil_status(modbus_param_t *mb_param, int slave, int start_addr,
|
|
|
+ int count, int *data_dest)
|
|
|
+{
|
|
|
+ int function = 0x01;
|
|
|
+ int status;
|
|
|
+
|
|
|
+ status = read_io_status(mb_param, slave, function, start_addr,
|
|
|
+ count, data_dest);
|
|
|
+
|
|
|
+ if (status > 0)
|
|
|
+ status = count;
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Same as read_coil_status but reads the slaves input table */
|
|
|
+int read_input_status(modbus_param_t *mb_param, int slave, int start_addr,
|
|
|
+ int count, int *data_dest)
|
|
|
+{
|
|
|
+ int function = 0x02;
|
|
|
+ int status;
|
|
|
+
|
|
|
+ status = read_io_status(mb_param, slave, function, start_addr,
|
|
|
+ count, data_dest);
|
|
|
+
|
|
|
+ if (status > 0)
|
|
|
+ status = count;
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Read the data from a modbus slave and put that data into an array */
|
|
|
+static int read_registers(modbus_param_t *mb_param, int slave, int function,
|
|
|
+ int start_addr, int count, int *data_dest)
|
|
|
+{
|
|
|
+ int query_size;
|
|
|
+ int status;
|
|
|
+ int query_ret;
|
|
|
+ unsigned char query[MIN_QUERY_SIZE];
|
|
|
+
|
|
|
+ query_size = build_request_packet(mb_param, slave, function,
|
|
|
+ start_addr, count, query);
|
|
|
+
|
|
|
+ query_ret = modbus_query(mb_param, query, query_size);
|
|
|
+ if (query_ret > 0)
|
|
|
+ status = read_reg_response(mb_param, data_dest, query);
|
|
|
+ else
|
|
|
+ status = query_ret;
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Read the holding registers in a slave and put the data into an
|
|
|
+ array */
|
|
|
+int read_holding_registers(modbus_param_t *mb_param, int slave,
|
|
|
+ int start_addr, int count, int *data_dest)
|
|
|
+{
|
|
|
+ int function = 0x03;
|
|
|
+ int status;
|
|
|
+
|
|
|
+ if (count > MAX_READ_HOLD_REGS) {
|
|
|
+ g_print("WARNING Too many holding registers requested\n");
|
|
|
+ count = MAX_READ_HOLD_REGS;
|
|
|
+ }
|
|
|
+
|
|
|
+ status = read_registers(mb_param, slave, function,
|
|
|
+ start_addr, count, data_dest);
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Read the input registers in a slave and put the data into
|
|
|
+ an array */
|
|
|
+int read_input_registers(modbus_param_t *mb_param, int slave,
|
|
|
+ int start_addr, int count, int *data_dest)
|
|
|
+{
|
|
|
+ int function = 0x04;
|
|
|
+ int status;
|
|
|
+
|
|
|
+ if (count > MAX_READ_INPUT_REGS) {
|
|
|
+ g_print("WARNING Too many input registers requested\n");
|
|
|
+ count = MAX_READ_INPUT_REGS;
|
|
|
+ }
|
|
|
+
|
|
|
+ status = read_registers(mb_param, slave, function,
|
|
|
+ start_addr, count, data_dest);
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Reads the response data from a slave and puts the data into an
|
|
|
+ array */
|
|
|
+static int read_reg_response(modbus_param_t *mb_param, int *data_dest,
|
|
|
+ unsigned char *query)
|
|
|
+{
|
|
|
+ unsigned char response[MAX_PACKET_SIZE];
|
|
|
+ int response_ret;
|
|
|
+ int offset;
|
|
|
+ int i;
|
|
|
+
|
|
|
+ response_ret = modbus_response(mb_param, query, response);
|
|
|
+
|
|
|
+ offset = mb_param->header_length;
|
|
|
+
|
|
|
+ /* If response_ret is negative, the loop is jumped ! */
|
|
|
+ for (i = 0; i < response_ret; i++) {
|
|
|
+ /* shift reg hi_byte to temp OR with lo_byte */
|
|
|
+ data_dest[i] = response[offset + 3 + (i << 1)] << 8 |
|
|
|
+ response[offset + 4 + (i << 1)];
|
|
|
+ }
|
|
|
+
|
|
|
+ return response_ret;
|
|
|
+}
|
|
|
+
|
|
|
+/* Gets the raw data from the input stream */
|
|
|
+static int preset_response(modbus_param_t *mb_param, unsigned char *query)
|
|
|
+{
|
|
|
+ int ret;
|
|
|
+ unsigned char response[MAX_PACKET_SIZE];
|
|
|
+
|
|
|
+ ret = modbus_response(mb_param, query, response);
|
|
|
+
|
|
|
+ return ret;
|
|
|
+}
|
|
|
+
|
|
|
+/* Sends a value to a register in a slave */
|
|
|
+static int set_single(modbus_param_t *mb_param, int slave, int function,
|
|
|
+ int addr, int value)
|
|
|
+{
|
|
|
+ int status;
|
|
|
+ int query_size;
|
|
|
+ int query_ret;
|
|
|
+ unsigned char query[MAX_PACKET_SIZE];
|
|
|
+
|
|
|
+ query_size = build_request_packet(mb_param, slave, function,
|
|
|
+ addr, value, query);
|
|
|
+
|
|
|
+ query_ret = modbus_query(mb_param, query, query_size);
|
|
|
+ if (query_ret > 0)
|
|
|
+ status = preset_response(mb_param, query);
|
|
|
+ else
|
|
|
+ status = query_ret;
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/* Turn on or off a single coil on the slave device */
|
|
|
+int force_single_coil(modbus_param_t *mb_param, int slave,
|
|
|
+ int coil_addr, int state)
|
|
|
+{
|
|
|
+ int function = 0x05;
|
|
|
+ int status;
|
|
|
+
|
|
|
+ if (state)
|
|
|
+ state = 0xFF00;
|
|
|
+
|
|
|
+ status = set_single(mb_param, slave, function, coil_addr, state);
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Sets a value in one holding register in the slave device */
|
|
|
+int preset_single_register(modbus_param_t *mb_param, int slave,
|
|
|
+ int reg_addr, int value)
|
|
|
+{
|
|
|
+ int function = 0x06;
|
|
|
+ int status;
|
|
|
+
|
|
|
+ status = set_single(mb_param, slave, function, reg_addr, value);
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Takes an array of ints and sets or resets the coils on a slave
|
|
|
+ appropriatly */
|
|
|
+int force_multiple_coils(modbus_param_t *mb_param, int slave,
|
|
|
+ int start_addr, int coil_count,
|
|
|
+ int *data_src)
|
|
|
+{
|
|
|
+ int function = 0x0F;
|
|
|
+ int i;
|
|
|
+ int byte_count;
|
|
|
+ int query_size;
|
|
|
+ int coil_check = 0;
|
|
|
+ int status;
|
|
|
+ int query_ret;
|
|
|
+
|
|
|
+ unsigned char query[MAX_PACKET_SIZE];
|
|
|
+
|
|
|
+ if (coil_count > MAX_WRITE_COILS) {
|
|
|
+ g_print("WARNING Writing to too many coils\n");
|
|
|
+ coil_count = MAX_WRITE_COILS;
|
|
|
+ }
|
|
|
+
|
|
|
+ query_size = build_request_packet(mb_param, slave, function,
|
|
|
+ start_addr, coil_count, query);
|
|
|
+ byte_count = (coil_count / 8) + ((coil_count % 8) ? 1 : 0);
|
|
|
+ query[query_size++] = byte_count;
|
|
|
+
|
|
|
+ for (i = 0; i < byte_count; i++) {
|
|
|
+ int bit;
|
|
|
+ int pos = 0;
|
|
|
+
|
|
|
+ bit = 0x01;
|
|
|
+ query[query_size] = 0;
|
|
|
+
|
|
|
+ while ((bit & 0xFF) && (coil_check++ < coil_count)) {
|
|
|
+ if (data_src[pos++])
|
|
|
+ query[query_size] |= bit;
|
|
|
+ else
|
|
|
+ query[query_size] &=~ bit;
|
|
|
+
|
|
|
+ bit = bit << 1;
|
|
|
+ }
|
|
|
+ query_size++;
|
|
|
+ }
|
|
|
+
|
|
|
+ query_ret = modbus_query(mb_param, query, query_size);
|
|
|
+ if (query_ret > 0)
|
|
|
+ status = preset_response(mb_param, query);
|
|
|
+ else
|
|
|
+ status = query_ret;
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Copy the values in an array to an array on the slave */
|
|
|
+int preset_multiple_registers(modbus_param_t *mb_param, int slave,
|
|
|
+ int start_addr, int reg_count, int *data_src)
|
|
|
+{
|
|
|
+ int function = 0x10;
|
|
|
+ int i;
|
|
|
+ int query_size;
|
|
|
+ int byte_count;
|
|
|
+ int status;
|
|
|
+ int query_ret;
|
|
|
+
|
|
|
+ unsigned char query[MAX_PACKET_SIZE];
|
|
|
+
|
|
|
+ if (reg_count > MAX_WRITE_REGS) {
|
|
|
+ g_print("WARNING Trying to write to too many registers\n");
|
|
|
+ reg_count = MAX_WRITE_REGS;
|
|
|
+ }
|
|
|
+
|
|
|
+ query_size = build_request_packet(mb_param, slave, function,
|
|
|
+ start_addr, reg_count, query);
|
|
|
+ byte_count = reg_count * 2;
|
|
|
+ query[query_size++] = byte_count;
|
|
|
+
|
|
|
+ for (i = 0; i < reg_count; i++) {
|
|
|
+ query[query_size++] = data_src[i] >> 8;
|
|
|
+ query[query_size++] = data_src[i] & 0x00FF;
|
|
|
+ }
|
|
|
+
|
|
|
+ query_ret = modbus_query(mb_param, query, query_size);
|
|
|
+ if (query_ret > 0)
|
|
|
+ status = preset_response(mb_param, query);
|
|
|
+ else
|
|
|
+ status = query_ret;
|
|
|
+
|
|
|
+ return status;
|
|
|
+}
|
|
|
+
|
|
|
+/* Returns the slave id ! */
|
|
|
+int report_slave_id(modbus_param_t *mb_param, int slave,
|
|
|
+ unsigned char *data_dest)
|
|
|
+{
|
|
|
+ int function = 0x11;
|
|
|
+ int query_size;
|
|
|
+ int query_ret;
|
|
|
+ int response_ret;
|
|
|
+
|
|
|
+ unsigned char query[MIN_QUERY_SIZE];
|
|
|
+ unsigned char response[MAX_PACKET_SIZE];
|
|
|
+
|
|
|
+ query_size = build_request_packet(mb_param, slave, function,
|
|
|
+ 0, 0, query);
|
|
|
+
|
|
|
+ /* start_addr and count are not used */
|
|
|
+ query_size -= 4;
|
|
|
+
|
|
|
+ query_ret = modbus_query(mb_param, query, query_size);
|
|
|
+ if (query_ret > 0) {
|
|
|
+ int i;
|
|
|
+ int offset;
|
|
|
+ int offset_length;
|
|
|
+
|
|
|
+ /* Byte count, slave id, run indicator status,
|
|
|
+ additional data */
|
|
|
+ response_ret = modbus_response(mb_param, query, response);
|
|
|
+
|
|
|
+ offset = mb_param->header_length;
|
|
|
+ offset_length = offset + response_ret;
|
|
|
+
|
|
|
+ for (i = offset; i < offset_length; i++)
|
|
|
+ data_dest[i] = response[i];
|
|
|
+ } else {
|
|
|
+ response_ret = query_ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ return response_ret;
|
|
|
+}
|
|
|
+
|
|
|
+/* Initialises the modbus_param_t structure for RTU */
|
|
|
+void modbus_init_rtu(modbus_param_t *mb_param, char *device,
|
|
|
+ int baud_i, char *parity, int data_bit,
|
|
|
+ int stop_bit)
|
|
|
+{
|
|
|
+ memset(mb_param, 0, sizeof(modbus_param_t));
|
|
|
+ strcpy(mb_param->device, device);
|
|
|
+ mb_param->baud_i = baud_i;
|
|
|
+ strcpy(mb_param->parity, parity);
|
|
|
+ mb_param->debug = FALSE;
|
|
|
+ mb_param->data_bit = data_bit;
|
|
|
+ mb_param->stop_bit = stop_bit;
|
|
|
+ mb_param->type_com = RTU;
|
|
|
+ mb_param->header_length = HEADER_LENGTH_RTU;
|
|
|
+ mb_param->checksum_size = CHECKSUM_SIZE_RTU;
|
|
|
+}
|
|
|
+
|
|
|
+/* Initialises the modbus_param_t structure for TCP */
|
|
|
+void modbus_init_tcp(modbus_param_t *mb_param, char *ip)
|
|
|
+{
|
|
|
+ memset(mb_param, 0, sizeof(modbus_param_t));
|
|
|
+ strncpy(mb_param->ip, ip, sizeof(char)*16);
|
|
|
+ mb_param->type_com = TCP;
|
|
|
+ mb_param->header_length = HEADER_LENGTH_TCP;
|
|
|
+ mb_param->checksum_size = CHECKSUM_SIZE_TCP;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/* This function sets up a serial port for RTU communications to
|
|
|
+ modbus */
|
|
|
+static int modbus_connect_rtu(modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ struct termios tios;
|
|
|
+ speed_t baud_rate;
|
|
|
+
|
|
|
+ if (mb_param->debug) {
|
|
|
+ g_print("Opening %s at %d bauds (%s)\n",
|
|
|
+ mb_param->device, mb_param->baud_i, mb_param->parity);
|
|
|
+ }
|
|
|
+
|
|
|
+ /* The O_NOCTTY flag tells UNIX that this program doesn't want
|
|
|
+ to be the "controlling terminal" for that port. If you
|
|
|
+ don't specify this then any input (such as keyboard abort
|
|
|
+ signals and so forth) will affect your process
|
|
|
+
|
|
|
+ Timeouts are ignored in canonical input mode or when the
|
|
|
+ NDELAY option is set on the file via open or fcntl */
|
|
|
+ mb_param->fd = open(mb_param->device, O_RDWR | O_NOCTTY | O_NDELAY);
|
|
|
+ if (mb_param->fd < 0) {
|
|
|
+ perror("open");
|
|
|
+ g_print("ERROR Opening device %s (no : %d)\n",
|
|
|
+ mb_param->device, errno);
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* Save */
|
|
|
+ tcgetattr(mb_param->fd, &(mb_param->old_tios));
|
|
|
+
|
|
|
+ memset(&tios, 0, sizeof(struct termios));
|
|
|
+
|
|
|
+ /* C_ISPEED Input baud (new interface)
|
|
|
+ C_OSPEED Output baud (new interface)
|
|
|
+ */
|
|
|
+ switch (mb_param->baud_i) {
|
|
|
+ case 110:
|
|
|
+ baud_rate = B110;
|
|
|
+ break;
|
|
|
+ case 300:
|
|
|
+ baud_rate = B300;
|
|
|
+ break;
|
|
|
+ case 600:
|
|
|
+ baud_rate = B600;
|
|
|
+ break;
|
|
|
+ case 1200:
|
|
|
+ baud_rate = B1200;
|
|
|
+ break;
|
|
|
+ case 2400:
|
|
|
+ baud_rate = B2400;
|
|
|
+ break;
|
|
|
+ case 4800:
|
|
|
+ baud_rate = B4800;
|
|
|
+ break;
|
|
|
+ case 9600:
|
|
|
+ baud_rate = B9600;
|
|
|
+ break;
|
|
|
+ case 19200:
|
|
|
+ baud_rate = B19200;
|
|
|
+ break;
|
|
|
+ case 38400:
|
|
|
+ baud_rate = B38400;
|
|
|
+ break;
|
|
|
+ case 57600:
|
|
|
+ baud_rate = B57600;
|
|
|
+ break;
|
|
|
+ case 115200:
|
|
|
+ baud_rate = B115200;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ baud_rate = B9600;
|
|
|
+ g_print("WARNING Unknown baud rate %d for %s (B9600 used)\n",
|
|
|
+ mb_param->baud_i, mb_param->device);
|
|
|
+ }
|
|
|
+
|
|
|
+ /* Set the baud rate */
|
|
|
+ if ((cfsetispeed(&tios, baud_rate) < 0) ||
|
|
|
+ (cfsetospeed(&tios, baud_rate) < 0)) {
|
|
|
+ perror("cfsetispeed/cfsetospeed\n");
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* C_CFLAG Control options
|
|
|
+ CLOCAL Local line - do not change "owner" of port
|
|
|
+ CREAD Enable receiver
|
|
|
+ */
|
|
|
+ tios.c_cflag |= (CREAD | CLOCAL);
|
|
|
+ /* CSIZE, HUPCL, CRTSCTS (hardware flow control) */
|
|
|
+
|
|
|
+ /* Set data bits (5, 6, 7, 8 bits)
|
|
|
+ CSIZE Bit mask for data bits
|
|
|
+ */
|
|
|
+ tios.c_cflag &= ~CSIZE;
|
|
|
+ switch (mb_param->data_bit) {
|
|
|
+ case 5:
|
|
|
+ tios.c_cflag |= CS5;
|
|
|
+ break;
|
|
|
+ case 6:
|
|
|
+ tios.c_cflag |= CS6;
|
|
|
+ break;
|
|
|
+ case 7:
|
|
|
+ tios.c_cflag |= CS7;
|
|
|
+ break;
|
|
|
+ case 8:
|
|
|
+ default:
|
|
|
+ tios.c_cflag |= CS8;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* Stop bit (1 or 2) */
|
|
|
+ if (mb_param->stop_bit == 1)
|
|
|
+ tios.c_cflag &=~ CSTOPB;
|
|
|
+ else /* 2 */
|
|
|
+ tios.c_cflag |= CSTOPB;
|
|
|
+
|
|
|
+ /* PARENB Enable parity bit
|
|
|
+ PARODD Use odd parity instead of even */
|
|
|
+ if (strncmp(mb_param->parity, "none", 4) == 0) {
|
|
|
+ tios.c_cflag &=~ PARENB;
|
|
|
+ } else if (strncmp(mb_param->parity, "even", 4) == 0) {
|
|
|
+ tios.c_cflag |= PARENB;
|
|
|
+ tios.c_cflag &=~ PARODD;
|
|
|
+ } else {
|
|
|
+ /* odd */
|
|
|
+ tios.c_cflag |= PARENB;
|
|
|
+ tios.c_cflag |= PARODD;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* Read your man page for the meaning of all this (man
|
|
|
+ termios). Its a bit to involved to comment here :) */
|
|
|
+ tios.c_line = 0;
|
|
|
+
|
|
|
+ /* C_LFLAG Line options
|
|
|
+
|
|
|
+ ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals
|
|
|
+ ICANON Enable canonical input (else raw)
|
|
|
+ XCASE Map uppercase \lowercase (obsolete)
|
|
|
+ ECHO Enable echoing of input characters
|
|
|
+ ECHOE Echo erase character as BS-SP-BS
|
|
|
+ ECHOK Echo NL after kill character
|
|
|
+ ECHONL Echo NL
|
|
|
+ NOFLSH Disable flushing of input buffers after
|
|
|
+ interrupt or quit characters
|
|
|
+ IEXTEN Enable extended functions
|
|
|
+ ECHOCTL Echo control characters as ^char and delete as ~?
|
|
|
+ ECHOPRT Echo erased character as character erased
|
|
|
+ ECHOKE BS-SP-BS entire line on line kill
|
|
|
+ FLUSHO Output being flushed
|
|
|
+ PENDIN Retype pending input at next read or input char
|
|
|
+ TOSTOP Send SIGTTOU for background output
|
|
|
+
|
|
|
+ Canonical input is line-oriented. Input characters are put
|
|
|
+ into a buffer which can be edited interactively by the user
|
|
|
+ until a CR (carriage return) or LF (line feed) character is
|
|
|
+ received.
|
|
|
+
|
|
|
+ Raw input is unprocessed. Input characters are passed
|
|
|
+ through exactly as they are received, when they are
|
|
|
+ received. Generally you'll deselect the ICANON, ECHO,
|
|
|
+ ECHOE, and ISIG options when using raw input
|
|
|
+ */
|
|
|
+
|
|
|
+ /* Raw input */
|
|
|
+ tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
|
|
+
|
|
|
+ /* C_IFLAG Input options
|
|
|
+
|
|
|
+ Constant Description
|
|
|
+ INPCK Enable parity check
|
|
|
+ IGNPAR Ignore parity errors
|
|
|
+ PARMRK Mark parity errors
|
|
|
+ ISTRIP Strip parity bits
|
|
|
+ IXON Enable software flow control (outgoing)
|
|
|
+ IXOFF Enable software flow control (incoming)
|
|
|
+ IXANY Allow any character to start flow again
|
|
|
+ IGNBRK Ignore break condition
|
|
|
+ BRKINT Send a SIGINT when a break condition is detected
|
|
|
+ INLCR Map NL to CR
|
|
|
+ IGNCR Ignore CR
|
|
|
+ ICRNL Map CR to NL
|
|
|
+ IUCLC Map uppercase to lowercase
|
|
|
+ IMAXBEL Echo BEL on input line too long
|
|
|
+ */
|
|
|
+ if (strncmp(mb_param->parity, "none", 4) == 0) {
|
|
|
+ tios.c_iflag &= ~INPCK;
|
|
|
+ } else {
|
|
|
+ tios.c_iflag |= INPCK;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* Software flow control is disabled */
|
|
|
+ tios.c_iflag &= ~(IXON | IXOFF | IXANY);
|
|
|
+
|
|
|
+ /* C_OFLAG Output options
|
|
|
+ OPOST Postprocess output (not set = raw output)
|
|
|
+ ONLCR Map NL to CR-NL
|
|
|
+
|
|
|
+ ONCLR ant others needs OPOST to be enabled
|
|
|
+ */
|
|
|
+
|
|
|
+ /* Raw ouput */
|
|
|
+ tios.c_oflag &=~ OPOST;
|
|
|
+
|
|
|
+ /* C_CC Control characters
|
|
|
+ VMIN Minimum number of characters to read
|
|
|
+ VTIME Time to wait for data (tenths of seconds)
|
|
|
+
|
|
|
+ UNIX serial interface drivers provide the ability to
|
|
|
+ specify character and packet timeouts. Two elements of the
|
|
|
+ c_cc array are used for timeouts: VMIN and VTIME. Timeouts
|
|
|
+ are ignored in canonical input mode or when the NDELAY
|
|
|
+ option is set on the file via open or fcntl.
|
|
|
+
|
|
|
+ VMIN specifies the minimum number of characters to read. If
|
|
|
+ it is set to 0, then the VTIME value specifies the time to
|
|
|
+ wait for every character read. Note that this does not mean
|
|
|
+ that a read call for N bytes will wait for N characters to
|
|
|
+ come in. Rather, the timeout will apply to the first
|
|
|
+ character and the read call will return the number of
|
|
|
+ characters immediately available (up to the number you
|
|
|
+ request).
|
|
|
+
|
|
|
+ If VMIN is non-zero, VTIME specifies the time to wait for
|
|
|
+ the first character read. If a character is read within the
|
|
|
+ time given, any read will block (wait) until all VMIN
|
|
|
+ characters are read. That is, once the first character is
|
|
|
+ read, the serial interface driver expects to receive an
|
|
|
+ entire packet of characters (VMIN bytes total). If no
|
|
|
+ character is read within the time allowed, then the call to
|
|
|
+ read returns 0. This method allows you to tell the serial
|
|
|
+ driver you need exactly N bytes and any read call will
|
|
|
+ return 0 or N bytes. However, the timeout only applies to
|
|
|
+ the first character read, so if for some reason the driver
|
|
|
+ misses one character inside the N byte packet then the read
|
|
|
+ call could block forever waiting for additional input
|
|
|
+ characters.
|
|
|
+
|
|
|
+ VTIME specifies the amount of time to wait for incoming
|
|
|
+ characters in tenths of seconds. If VTIME is set to 0 (the
|
|
|
+ default), reads will block (wait) indefinitely unless the
|
|
|
+ NDELAY option is set on the port with open or fcntl.
|
|
|
+ */
|
|
|
+ /* Unused because we use open with the NDELAY option */
|
|
|
+ tios.c_cc[VMIN] = 0;
|
|
|
+ tios.c_cc[VTIME] = 0;
|
|
|
+
|
|
|
+ if (tcsetattr(mb_param->fd, TCSANOW, &tios) < 0) {
|
|
|
+ perror("tcsetattr\n");
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int modbus_connect_tcp(modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ int ret;
|
|
|
+ int option;
|
|
|
+ struct sockaddr_in addr;
|
|
|
+
|
|
|
+ addr.sin_family = AF_INET;
|
|
|
+ addr.sin_port = htons(MODBUS_TCP_PORT);
|
|
|
+ addr.sin_addr.s_addr = inet_addr(mb_param->ip);
|
|
|
+
|
|
|
+ mb_param->fd = socket(AF_INET, SOCK_STREAM, 0);
|
|
|
+ if (mb_param->fd < 0) {
|
|
|
+ return mb_param->fd;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* Set the TCP no delay flag */
|
|
|
+ /* SOL_TCP = IPPROTO_TCP */
|
|
|
+ option = 1;
|
|
|
+ ret = setsockopt(mb_param->fd, SOL_TCP, TCP_NODELAY,
|
|
|
+ (const void *)&option, sizeof(int));
|
|
|
+ if (ret < 0) {
|
|
|
+ perror("setsockopt");
|
|
|
+ close(mb_param->fd);
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* Set the IP low delay option */
|
|
|
+ option = IPTOS_LOWDELAY;
|
|
|
+ ret = setsockopt(mb_param->fd, SOL_TCP, IP_TOS,
|
|
|
+ (const void *)&option, sizeof(int));
|
|
|
+ if (ret < 0) {
|
|
|
+ perror("setsockopt");
|
|
|
+ close(mb_param->fd);
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (mb_param->debug) {
|
|
|
+ g_print("Connecting to %s\n", mb_param->ip);
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = connect(mb_param->fd, (struct sockaddr *)&addr,
|
|
|
+ sizeof(struct sockaddr_in));
|
|
|
+ if (ret < 0) {
|
|
|
+ perror("connect");
|
|
|
+ close(mb_param->fd);
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+int modbus_connect(modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ int ret;
|
|
|
+
|
|
|
+ if (mb_param->type_com == RTU)
|
|
|
+ ret = modbus_connect_rtu(mb_param);
|
|
|
+ else
|
|
|
+ ret = modbus_connect_tcp(mb_param);
|
|
|
+
|
|
|
+ return ret;
|
|
|
+}
|
|
|
+
|
|
|
+void modbus_listen_tcp(modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ int ret;
|
|
|
+ int new_socket;
|
|
|
+ struct sockaddr_in addr;
|
|
|
+ int addrlen;
|
|
|
+
|
|
|
+ addr.sin_family = AF_INET;
|
|
|
+ /* The modbus port is < 1024
|
|
|
+ This program must be made setuid root. */
|
|
|
+ addr.sin_port = htons(MODBUS_TCP_PORT);
|
|
|
+ addr.sin_addr.s_addr = INADDR_ANY;
|
|
|
+ memset(&(addr.sin_zero), '\0', 8);
|
|
|
+
|
|
|
+ new_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
|
|
|
+ if (new_socket < 0) {
|
|
|
+ perror("socket");
|
|
|
+ exit(1);
|
|
|
+ } else {
|
|
|
+ g_print("Socket OK\n");
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = bind(new_socket, (struct sockaddr *)&addr,
|
|
|
+ sizeof(struct sockaddr_in));
|
|
|
+ if (ret < 0) {
|
|
|
+ perror("bind");
|
|
|
+ close(new_socket);
|
|
|
+ exit(1);
|
|
|
+ } else {
|
|
|
+ g_print("Bind OK\n");
|
|
|
+ }
|
|
|
+
|
|
|
+ ret = listen(new_socket, 1);
|
|
|
+ if (ret != 0) {
|
|
|
+ perror("listen");
|
|
|
+ close(new_socket);
|
|
|
+ exit(1);
|
|
|
+ } else {
|
|
|
+ g_print("Listen OK\n");
|
|
|
+ }
|
|
|
+
|
|
|
+ addrlen = sizeof(struct sockaddr_in);
|
|
|
+ mb_param->fd = accept(new_socket, (struct sockaddr *)&addr, &addrlen);
|
|
|
+ if (ret < 0) {
|
|
|
+ perror("accept");
|
|
|
+ close(new_socket);
|
|
|
+ exit(1);
|
|
|
+ } else {
|
|
|
+ unsigned char response[MAX_PACKET_SIZE];
|
|
|
+ int response_size;
|
|
|
+
|
|
|
+ g_print("The client %s is connected\n",
|
|
|
+ inet_ntoa(addr.sin_addr));
|
|
|
+
|
|
|
+ receive_response(mb_param, MAX_PACKET_SIZE,
|
|
|
+ response, &response_size);
|
|
|
+ }
|
|
|
+
|
|
|
+ close(new_socket);
|
|
|
+}
|
|
|
+
|
|
|
+/* Close the file descriptor */
|
|
|
+static void modbus_close_rtu(modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ if (tcsetattr(mb_param->fd, TCSANOW, &(mb_param->old_tios)) < 0)
|
|
|
+ perror("tcsetattr");
|
|
|
+
|
|
|
+ close(mb_param->fd);
|
|
|
+}
|
|
|
+
|
|
|
+static void modbus_close_tcp(modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ shutdown(mb_param->fd, SHUT_RDWR);
|
|
|
+ close(mb_param->fd);
|
|
|
+}
|
|
|
+
|
|
|
+void modbus_close(modbus_param_t *mb_param)
|
|
|
+{
|
|
|
+ if (mb_param->type_com == RTU)
|
|
|
+ modbus_close_rtu(mb_param);
|
|
|
+ else
|
|
|
+ modbus_close_tcp(mb_param);
|
|
|
+}
|
|
|
+
|
|
|
+void modbus_set_debug(modbus_param_t *mb_param, int boolean)
|
|
|
+{
|
|
|
+ mb_param->debug = boolean;
|
|
|
+}
|
Stephane Raimbault
19 tahun lalu