source: trunk/Simulator/modbus.c@ 14

Last change on this file since 14 was 5, checked in by mansoux, 15 years ago

Ajout librairie Modbus base MacOSX
File size: 70.4 KB
Line 
1/*
2 * Copyright © 2001-2008 Stéphane Raimbault <stephane.raimbault@gmail.com>
3 *
4 * This program is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU Lesser Public License as published by
6 * the Free Software Foundation; either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU Lesser Public License for more details.
13 *
14 * You should have received a copy of the GNU Lesser Public License
15 * along with this program. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18/*
19 The library is designed to send and receive data from a device that
20 communicate via the Modbus protocol.
21
22 The function names used are inspired by the Modicon Modbus Protocol
23 Reference Guide which can be obtained from Schneider at
24 www.schneiderautomation.com.
25
26 Documentation:
27 http://www.easysw.com/~mike/serial/serial.html
28 http://copyleft.free.fr/wordpress/index.php/libmodbus/
29*/
30
31#include <stdio.h>
32#include <string.h>
33#include <stdlib.h>
34#include <stdint.h>
35#include <termios.h>
36#include <sys/time.h>
37#include <unistd.h>
38#include <errno.h>
39#include <limits.h>
40#include <fcntl.h>
41
42/* TCP */
43#include <sys/types.h>
44#include <sys/socket.h>
45#include <sys/ioctl.h>
46#include <netinet/in.h>
47#include <netinet/ip.h>
48#include <netinet/tcp.h>
49#include <arpa/inet.h>
50
51#include "modbus.h"
52
53#define UNKNOWN_ERROR_MSG "Not defined in modbus specification"
54
55/* This structure reduces the number of params in functions and so
56 * optimizes the speed of execution (~ 37%). */
57typedef struct {
58 int slave;
59 int function;
60 int t_id;
61} sft_t;
62
63static const uint8_t NB_TAB_ERROR_MSG = 12;
64static const char *TAB_ERROR_MSG[] = {
65 /* 0x00 */ UNKNOWN_ERROR_MSG,
66 /* 0x01 */ "Illegal function code",
67 /* 0x02 */ "Illegal data address",
68 /* 0x03 */ "Illegal data value",
69 /* 0x04 */ "Slave device or server failure",
70 /* 0x05 */ "Acknowledge",
71 /* 0x06 */ "Slave device or server busy",
72 /* 0x07 */ "Negative acknowledge",
73 /* 0x08 */ "Memory parity error",
74 /* 0x09 */ UNKNOWN_ERROR_MSG,
75 /* 0x0A */ "Gateway path unavailable",
76 /* 0x0B */ "Target device failed to respond"
77};
78
79/* Table of CRC values for high-order byte */
80static uint8_t table_crc_hi[] = {
81 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
82 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
83 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
84 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
85 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
86 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
87 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
88 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
89 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
90 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
91 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
92 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
93 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
94 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
95 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
96 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
97 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
98 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
99 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
100 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
101 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
102 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
103 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
104 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
105 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
106 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
107};
108
109/* Table of CRC values for low-order byte */
110static uint8_t table_crc_lo[] = {
111 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
112 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
113 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
114 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
115 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
116 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
117 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
118 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
119 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
120 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
121 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
122 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
123 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
124 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
125 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
126 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
127 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
128 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
129 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
130 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
131 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
132 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
133 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
134 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
135 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
136 0x43, 0x83, 0x41, 0x81, 0x80, 0x40
137};
138
139/* Treats errors and flush or close connection if necessary */
140static void error_treat(modbus_param_t *mb_param, int code, const char *string)
141{
142 printf("\nERROR %s (%d)\n", string, code);
143
144 if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) {
145 switch (code) {
146 case ILLEGAL_DATA_VALUE:
147 case ILLEGAL_DATA_ADDRESS:
148 case ILLEGAL_FUNCTION:
149 break;
150 default:
151 if (mb_param->type_com == RTU) {
152 tcflush(mb_param->fd, TCIOFLUSH);
153 } else {
154 modbus_close(mb_param);
155 modbus_connect(mb_param);
156 }
157 }
158 }
159}
160
161/* Computes the length of the expected response */
162static unsigned int compute_response_length(modbus_param_t *mb_param,
163 uint8_t *query)
164{
165 int resp_length;
166 int offset;
167
168 offset = mb_param->header_length;
169
170 switch (query[offset + 1]) {
171 case FC_READ_COIL_STATUS:
172 case FC_READ_INPUT_STATUS: {
173 /* Header + nb values (code from force_multiple_coils) */
174 int nb = (query[offset + 4] << 8) | query[offset + 5];
175 resp_length = 3 + (nb / 8) + ((nb % 8) ? 1 : 0);
176 }
177 break;
178 case FC_READ_HOLDING_REGISTERS:
179 case FC_READ_INPUT_REGISTERS:
180 /* Header + 2 * nb values */
181 resp_length = 3 + 2 * (query[offset + 4] << 8 |
182 query[offset + 5]);
183 break;
184 case FC_READ_EXCEPTION_STATUS:
185 resp_length = 4;
186 break;
187 default:
188 resp_length = 6;
189 }
190
191 resp_length += offset + mb_param->checksum_length;
192
193 return resp_length;
194}
195
196/* Builds a RTU query header */
197static int build_query_basis_rtu(int slave, int function,
198 int start_addr, int nb,
199 uint8_t *query)
200{
201 query[0] = slave;
202 query[1] = function;
203 query[2] = start_addr >> 8;
204 query[3] = start_addr & 0x00ff;
205 query[4] = nb >> 8;
206 query[5] = nb & 0x00ff;
207
208 return PRESET_QUERY_LENGTH_RTU;
209}
210
211/* Builds a TCP query header */
212static int build_query_basis_tcp(int slave, int function,
213 int start_addr, int nb,
214 uint8_t *query)
215{
216
217 /* Extract from MODBUS Messaging on TCP/IP Implementation
218 Guide V1.0b (page 23/46):
219 The transaction identifier is used to associate the future
220 response with the request. So, at a time, on a TCP
221 connection, this identifier must be unique.
222 */
223 static uint16_t t_id = 0;
224
225 /* Transaction ID */
226 if (t_id < UINT16_MAX)
227 t_id++;
228 else
229 t_id = 0;
230 query[0] = t_id >> 8;
231 query[1] = t_id & 0x00ff;
232
233 /* Protocol Modbus */
234 query[2] = 0;
235 query[3] = 0;
236
237 /* Length to fix later with set_query_length_tcp (4 and 5) */
238
239 query[6] = slave;
240 query[7] = function;
241 query[8] = start_addr >> 8;
242 query[9] = start_addr & 0x00ff;
243 query[10] = nb >> 8;
244 query[11] = nb & 0x00ff;
245
246 return PRESET_QUERY_LENGTH_TCP;
247}
248
249static int build_query_basis(modbus_param_t *mb_param, int slave,
250 int function, int start_addr,
251 int nb, uint8_t *query)
252{
253 if (mb_param->type_com == RTU)
254 return build_query_basis_rtu(slave, function, start_addr,
255 nb, query);
256 else
257 return build_query_basis_tcp(slave, function, start_addr,
258 nb, query);
259}
260
261/* Builds a RTU response header */
262static int build_response_basis_rtu(sft_t *sft, uint8_t *response)
263{
264 response[0] = sft->slave;
265 response[1] = sft->function;
266
267 return PRESET_RESPONSE_LENGTH_RTU;
268}
269
270/* Builds a TCP response header */
271static int build_response_basis_tcp(sft_t *sft, uint8_t *response)
272{
273 /* Extract from MODBUS Messaging on TCP/IP Implementation
274 Guide V1.0b (page 23/46):
275 The transaction identifier is used to associate the future
276 response with the request. */
277 response[0] = sft->t_id >> 8;
278 response[1] = sft->t_id & 0x00ff;
279
280 /* Protocol Modbus */
281 response[2] = 0;
282 response[3] = 0;
283
284 /* Length to fix later with set_message_length_tcp (4 and 5) */
285
286 response[6] = sft->slave;
287 response[7] = sft->function;
288
289 return PRESET_RESPONSE_LENGTH_TCP;
290}
291
292static int build_response_basis(modbus_param_t *mb_param, sft_t *sft,
293 uint8_t *response)
294{
295 if (mb_param->type_com == RTU)
296 return build_response_basis_rtu(sft, response);
297 else
298 return build_response_basis_tcp(sft, response);
299}
300
301/* Sets the length of TCP message in the message (query and response) */
302void set_message_length_tcp(uint8_t *msg, int msg_length)
303{
304 /* Substract the header length to the message length */
305 int mbap_length = msg_length - 6;
306
307 msg[4] = mbap_length >> 8;
308 msg[5] = mbap_length & 0x00FF;
309}
310
311/* Fast CRC */
312static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length)
313{
314 uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
315 uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
316 unsigned int i; /* will index into CRC lookup */
317
318 /* pass through message buffer */
319 while (buffer_length--) {
320 i = crc_hi ^ *buffer++; /* calculate the CRC */
321 crc_hi = crc_lo ^ table_crc_hi[i];
322 crc_lo = table_crc_lo[i];
323 }
324
325 return (crc_hi << 8 | crc_lo);
326}
327
328/* If CRC is correct returns 0 else returns INVALID_CRC */
329static int check_crc16(modbus_param_t *mb_param,
330 uint8_t *msg,
331 const int msg_length)
332{
333 int ret;
334 uint16_t crc_calc;
335 uint16_t crc_received;
336
337 crc_calc = crc16(msg, msg_length - 2);
338 crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
339
340 /* Check CRC of msg */
341 if (crc_calc == crc_received) {
342 ret = 0;
343 } else {
344 char s_error[64];
345 sprintf(s_error,
346 "invalid crc received %0X - crc_calc %0X",
347 crc_received, crc_calc);
348 ret = INVALID_CRC;
349 error_treat(mb_param, ret, s_error);
350 }
351
352 return ret;
353}
354
355/* Sends a query/response over a serial or a TCP communication */
356static int modbus_send(modbus_param_t *mb_param, uint8_t *query,
357 int query_length)
358{
359 int ret;
360 uint16_t s_crc;
361 int i;
362
363 if (mb_param->type_com == RTU) {
364 s_crc = crc16(query, query_length);
365 query[query_length++] = s_crc >> 8;
366 query[query_length++] = s_crc & 0x00FF;
367 } else {
368 set_message_length_tcp(query, query_length);
369 }
370
371 if (mb_param->debug) {
372 for (i = 0; i < query_length; i++)
373 printf("[%.2X]", query[i]);
374 printf("\n");
375 }
376
377 if (mb_param->saveRawData)
378 {
379 for (i = 0; i < query_length; i++)
380 mb_param->rawQuery[i]=query[i];
381 mb_param->rawQueryLength=query_length;
382 }
383
384 if (mb_param->type_com == RTU)
385 ret = write(mb_param->fd, query, query_length);
386 else
387 ret = send(mb_param->fd, query, query_length, 0);
388
389 /* Return the number of bytes written (0 to n)
390 or PORT_SOCKET_FAILURE on error */
391 if ((ret == -1) || (ret != query_length)) {
392 ret = PORT_SOCKET_FAILURE;
393 error_treat(mb_param, ret, "Write port/socket failure");
394 }
395
396 return ret;
397}
398
399/* Computes the length of the header following the function code */
400static uint8_t compute_query_length_header(int function)
401{
402 int length;
403
404 if (function <= FC_FORCE_SINGLE_COIL ||
405 function == FC_PRESET_SINGLE_REGISTER)
406 /* Read and single write */
407 length = 4;
408 else if (function == FC_FORCE_MULTIPLE_COILS ||
409 function == FC_PRESET_MULTIPLE_REGISTERS)
410 /* Multiple write */
411 length = 5;
412 else
413 length = 0;
414
415 return length;
416}
417
418/* Computes the length of the data to write in the query */
419static int compute_query_length_data(modbus_param_t *mb_param, uint8_t *msg)
420{
421 int function = msg[mb_param->header_length + 1];
422 int length;
423
424 if (function == FC_FORCE_MULTIPLE_COILS ||
425 function == FC_PRESET_MULTIPLE_REGISTERS)
426 length = msg[mb_param->header_length + 6];
427 else
428 length = 0;
429
430 length += mb_param->checksum_length;
431
432 return length;
433}
434
435#define WAIT_DATA() \
436{ \
437 while ((select_ret = select(mb_param->fd+1, &rfds, NULL, NULL, &tv)) == -1) { \
438 if (errno == EINTR) { \
439 printf("A non blocked signal was caught\n"); \
440 /* Necessary after an error */ \
441 FD_ZERO(&rfds); \
442 FD_SET(mb_param->fd, &rfds); \
443 } else { \
444 error_treat(mb_param, SELECT_FAILURE, "Select failure"); \
445 return SELECT_FAILURE; \
446 } \
447 } \
448 \
449 if (select_ret == 0) { \
450 /* Call to error_treat is done later to manage exceptions */ \
451 return COMM_TIME_OUT; \
452 } \
453}
454
455/* Waits a reply from a modbus slave or a query from a modbus master.
456 This function blocks for timeout seconds if there is no reply.
457
458 In
459 - msg_length_computed must be set to MSG_LENGTH_UNDEFINED if undefined
460
461 Out
462 - msg is an array of uint8_t to receive the message
463 - p_msg_length, the variable is assigned to the number of
464 characters received. This value won't be greater than
465 msg_length_computed.
466
467 Returns 0 in success or a negative value if an error occured.
468*/
469static int receive_msg(modbus_param_t *mb_param,
470 int msg_length_computed,
471 uint8_t *msg, int *p_msg_length)
472{
473 int select_ret;
474 int read_ret;
475 fd_set rfds;
476 struct timeval tv;
477 int length_to_read;
478 uint8_t *p_msg;
479 enum { FUNCTION, BYTE, COMPLETE };
480 int state;
481
482 if (mb_param->debug) {
483 if (msg_length_computed == MSG_LENGTH_UNDEFINED)
484 printf("Waiting for a message...\n");
485 else
486 printf("Waiting for a message (%d bytes)...\n",
487 msg_length_computed);
488 }
489
490 /* Add a file descriptor to the set */
491 FD_ZERO(&rfds);
492 FD_SET(mb_param->fd, &rfds);
493
494 if (msg_length_computed == MSG_LENGTH_UNDEFINED) {
495 /* Wait for a message */
496 tv.tv_sec = 60;
497 tv.tv_usec = 0;
498
499 /* The message length is undefined (query receiving) so
500 * we need to analyse the message step by step.
501 * At the first step, we want to reach the function
502 * code because all packets have that information. */
503 msg_length_computed = mb_param->header_length + 2;
504 state = FUNCTION;
505 } else {
506 tv.tv_sec = 0;
507 tv.tv_usec = TIME_OUT_BEGIN_OF_TRAME;
508 state = COMPLETE;
509 }
510
511 length_to_read = msg_length_computed;
512
513 select_ret = 0;
514 WAIT_DATA();
515
516 /* Initialize the readin the message */
517 (*p_msg_length) = 0;
518 p_msg = msg;
519
520 while (select_ret) {
521 if (mb_param->type_com == RTU)
522 read_ret = read(mb_param->fd, p_msg, length_to_read);
523 else
524 read_ret = recv(mb_param->fd, p_msg, length_to_read, 0);
525
526 if (read_ret == 0) {
527 printf("Connection closed\n");
528 return CONNECTION_CLOSED;
529 } else if (read_ret < 0) {
530 /* The only negative possible value is -1 */
531 error_treat(mb_param, PORT_SOCKET_FAILURE,
532 "Read port/socket failure");
533 return PORT_SOCKET_FAILURE;
534 }
535
536 /* Sums bytes received */
537 (*p_msg_length) += read_ret;
538
539 /* Display the hex code of each character received */
540 if (mb_param->debug) {
541 int i;
542 for (i=0; i < read_ret; i++)
543 printf("<%.2X>", p_msg[i]);
544 }
545
546 if ((*p_msg_length) < msg_length_computed) {
547 /* Message incomplete */
548 length_to_read = msg_length_computed - (*p_msg_length);
549 } else {
550 switch (state) {
551 case FUNCTION:
552 /* Function code position */
553 length_to_read = compute_query_length_header(msg[mb_param->header_length + 1]);
554 msg_length_computed += length_to_read;
555 /* It's useless to check
556 p_msg_length_computed value in this
557 case (only defined values are used). */
558 state = BYTE;
559 break;
560 case BYTE:
561 length_to_read = compute_query_length_data(mb_param, msg);
562 msg_length_computed += length_to_read;
563 if (msg_length_computed > MAX_MESSAGE_LENGTH) {
564 error_treat(mb_param, TOO_MANY_DATA, "Too many data");
565 return TOO_MANY_DATA;
566 }
567 state = COMPLETE;
568 break;
569 case COMPLETE:
570 length_to_read = 0;
571 break;
572 }
573 }
574
575 /* Moves the pointer to receive other data */
576 p_msg = &(p_msg[read_ret]);
577
578 if (length_to_read > 0) {
579 /* If no character at the buffer wait
580 TIME_OUT_END_OF_TRAME before to generate an error. */
581 tv.tv_sec = 0;
582 tv.tv_usec = TIME_OUT_END_OF_TRAME;
583
584 WAIT_DATA();
585 } else {
586 /* All chars are received */
587 select_ret = FALSE;
588 }
589 }
590
591 if (mb_param->debug)
592 printf("\n");
593
594 if (mb_param->type_com == RTU) {
595 check_crc16(mb_param, msg, (*p_msg_length));
596 }
597
598 /* OK */
599 return 0;
600}
601
602
603/* Receives the response and checks values (and checksum in RTU).
604
605 Returns:
606 - the number of values (bits or word) if success or the response
607 length if no value is returned
608 - less than 0 for exception errors
609
610 Note: all functions used to send or receive data with modbus return
611 these values. */
612static int modbus_receive(modbus_param_t *mb_param,
613 uint8_t *query,
614 uint8_t *response)
615{
616 int ret, i;
617 int response_length;
618 int response_length_computed;
619 int offset = mb_param->header_length;
620
621 response_length_computed = compute_response_length(mb_param, query);
622 ret = receive_msg(mb_param, response_length_computed,
623 response, &response_length);
624 if (ret == 0) {
625 /* GOOD RESPONSE */
626 int query_nb_value;
627 int response_nb_value;
628
629 /* The number of values is returned if it's corresponding
630 * to the query */
631 switch (response[offset + 1]) {
632 case FC_READ_COIL_STATUS:
633 case FC_READ_INPUT_STATUS:
634 /* Read functions, 8 values in a byte (nb
635 * of values in the query and byte count in
636 * the response. */
637 query_nb_value = (query[offset+4] << 8) + query[offset+5];
638 query_nb_value = (query_nb_value / 8) + ((query_nb_value % 8) ? 1 : 0);
639 response_nb_value = response[offset + 2];
640 break;
641 case FC_READ_HOLDING_REGISTERS:
642 case FC_READ_INPUT_REGISTERS:
643 /* Read functions 1 value = 2 bytes */
644 query_nb_value = (query[offset+4] << 8) + query[offset+5];
645 response_nb_value = (response[offset + 2] / 2);
646 break;
647 case FC_FORCE_MULTIPLE_COILS:
648 case FC_PRESET_MULTIPLE_REGISTERS:
649 /* N Write functions */
650 query_nb_value = (query[offset+4] << 8) + query[offset+5];
651 response_nb_value = (response[offset + 4] << 8) | response[offset + 5];
652 break;
653 case FC_REPORT_SLAVE_ID:
654 /* Report slave ID (bytes received) */
655 query_nb_value = response_nb_value = response_length;
656 break;
657 default:
658 /* 1 Write functions & others */
659 query_nb_value = response_nb_value = 1;
660 }
661
662 if (query_nb_value == response_nb_value)
663 {
664 ret = response_nb_value;
665 if (mb_param->saveRawData)
666 {
667 for (i = 0; i < response_length_computed; i++)
668 mb_param->rawResponse[i]=response[i];
669 mb_param->rawResponseLength=response_length_computed;
670 }
671
672 } else {
673 char *s_error = malloc(64 * sizeof(char));
674 sprintf(s_error, "Quantity (%d) not corresponding to the query (%d)",
675 response_nb_value, query_nb_value);
676 ret = ILLEGAL_DATA_VALUE;
677 error_treat(mb_param, ILLEGAL_DATA_VALUE, s_error);
678 free(s_error);
679 }
680 } else if (ret == COMM_TIME_OUT) {
681
682 if (response_length == (offset + 3 + mb_param->checksum_length)) {
683 /* EXCEPTION CODE RECEIVED */
684
685 /* Optimization allowed because exception response is
686 the smallest trame in modbus protocol (3) so always
687 raise a timeout error */
688
689 /* CRC must be checked here (not done in receive_msg) */
690 if (mb_param->type_com == RTU) {
691 ret = check_crc16(mb_param, response, response_length);
692 if (ret != 0)
693 return ret;
694 }
695
696 /* Check for exception response.
697 0x80 + function is stored in the exception
698 response. */
699 if (0x80 + query[offset + 1] == response[offset + 1]) {
700
701 int exception_code = response[offset + 2];
702 // FIXME check test
703 if (exception_code < NB_TAB_ERROR_MSG) {
704 error_treat(mb_param, -exception_code,
705 TAB_ERROR_MSG[response[offset + 2]]);
706 /* RETURN THE EXCEPTION CODE */
707 /* Modbus error code is negative */
708 return -exception_code;
709 } else {
710 /* The chances are low to hit this
711 case but it can avoid a vicious
712 segfault */
713 char *s_error = malloc(64 * sizeof(char));
714 sprintf(s_error,
715 "Invalid exception code %d",
716 response[offset + 2]);
717 error_treat(mb_param, INVALID_EXCEPTION_CODE,
718 s_error);
719 free(s_error);
720 return INVALID_EXCEPTION_CODE;
721 }
722 }
723 /* If doesn't return previously, return as
724 TIME OUT here */
725 }
726
727 /* COMMUNICATION TIME OUT */
728 error_treat(mb_param, ret, "Communication time out");
729 return ret;
730 }
731
732 return ret;
733}
734
735static int response_io_status(int address, int nb,
736 uint8_t *tab_io_status,
737 uint8_t *response, int offset)
738{
739 int shift = 0;
740 int byte = 0;
741 int i;
742
743 for (i = address; i < address+nb; i++) {
744 byte |= tab_io_status[i] << shift;
745 if (shift == 7) {
746 /* Byte is full */
747 response[offset++] = byte;
748 byte = shift = 0;
749 } else {
750 shift++;
751 }
752 }
753
754 if (shift != 0)
755 response[offset++] = byte;
756
757 return offset;
758}
759
760/* Build the exception response */
761static int response_exception(modbus_param_t *mb_param, sft_t *sft,
762 int exception_code, uint8_t *response)
763{
764 int response_length;
765
766 sft->function = sft->function + 0x80;
767 response_length = build_response_basis(mb_param, sft, response);
768
769 /* Positive exception code */
770 response[response_length++] = -exception_code;
771
772 return response_length;
773}
774
775/* Manages the received query.
776 Analyses the query and constructs a response.
777 If an error occurs, this function construct the response
778 accordingly.
779*/
780void modbus_manage_query(modbus_param_t *mb_param, const uint8_t *query,
781 int query_length, modbus_mapping_t *mb_mapping)
782{
783 int offset = mb_param->header_length;
784 int slave = query[offset];
785 int function = query[offset+1];
786 uint16_t address = (query[offset+2] << 8) + query[offset+3];
787 uint8_t response[MAX_MESSAGE_LENGTH];
788 int resp_length = 0;
789 sft_t sft;
790
791 sft.slave = slave;
792 sft.function = function;
793 if (mb_param->type_com == TCP)
794 sft.t_id = (query[0] << 8) + query[1];
795 else
796 sft.t_id = 0;
797
798 switch (function) {
799 case FC_READ_COIL_STATUS: {
800 int nb = (query[offset+4] << 8) + query[offset+5];
801
802 if ((address + nb) > mb_mapping->nb_coil_status) {
803 printf("Illegal data address %0X in read_coil_status\n",
804 address + nb);
805 resp_length = response_exception(mb_param, &sft,
806 ILLEGAL_DATA_ADDRESS, response);
807 } else {
808 resp_length = build_response_basis(mb_param, &sft, response);
809 response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
810 resp_length = response_io_status(address, nb,
811 mb_mapping->tab_coil_status,
812 response, resp_length);
813 }
814 }
815 break;
816 case FC_READ_INPUT_STATUS: {
817 /* Similar to coil status (but too much arguments to use a
818 * function) */
819 int nb = (query[offset+4] << 8) + query[offset+5];
820
821 if ((address + nb) > mb_mapping->nb_input_status) {
822 printf("Illegal data address %0X in read_input_status\n",
823 address + nb);
824 resp_length = response_exception(mb_param, &sft,
825 ILLEGAL_DATA_ADDRESS, response);
826 } else {
827 resp_length = build_response_basis(mb_param, &sft, response);
828 response[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
829 resp_length = response_io_status(address, nb,
830 mb_mapping->tab_input_status,
831 response, resp_length);
832 }
833 }
834 break;
835 case FC_READ_HOLDING_REGISTERS: {
836 int nb = (query[offset+4] << 8) + query[offset+5];
837
838 if ((address + nb) > mb_mapping->nb_holding_registers) {
839 printf("Illegal data address %0X in read_holding_registers\n",
840 address + nb);
841 resp_length = response_exception(mb_param, &sft,
842 ILLEGAL_DATA_ADDRESS, response);
843 } else {
844 int i;
845
846 resp_length = build_response_basis(mb_param, &sft, response);
847 response[resp_length++] = nb << 1;
848 for (i = address; i < address + nb; i++) {
849 response[resp_length++] = mb_mapping->tab_holding_registers[i] >> 8;
850 response[resp_length++] = mb_mapping->tab_holding_registers[i] & 0xFF;
851 }
852 }
853 }
854 break;
855 case FC_READ_INPUT_REGISTERS: {
856 /* Similar to holding registers (but too much arguments to use a
857 * function) */
858 int nb = (query[offset+4] << 8) + query[offset+5];
859
860 if ((address + nb) > mb_mapping->nb_input_registers) {
861 printf("Illegal data address %0X in read_input_registers\n",
862 address + nb);
863 resp_length = response_exception(mb_param, &sft,
864 ILLEGAL_DATA_ADDRESS, response);
865 } else {
866 int i;
867
868 resp_length = build_response_basis(mb_param, &sft, response);
869 response[resp_length++] = nb << 1;
870 for (i = address; i < address + nb; i++) {
871 response[resp_length++] = mb_mapping->tab_input_registers[i] >> 8;
872 response[resp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
873 }
874 }
875 }
876 break;
877 case FC_FORCE_SINGLE_COIL:
878 if (address >= mb_mapping->nb_coil_status) {
879 printf("Illegal data address %0X in force_singe_coil\n", address);
880 resp_length = response_exception(mb_param, &sft,
881 ILLEGAL_DATA_ADDRESS, response);
882 } else {
883 int data = (query[offset+4] << 8) + query[offset+5];
884
885 if (data == 0xFF00 || data == 0x0) {
886 mb_mapping->tab_coil_status[address] = (data) ? ON : OFF;
887
888 /* In RTU mode, the CRC is computed
889 and added to the query by modbus_send */
890 memcpy(response, query, query_length - mb_param->checksum_length);
891 resp_length = query_length;
892 } else {
893 printf("Illegal data value %0X in force_single_coil request at address %0X\n",
894 data, address);
895 resp_length = response_exception(mb_param, &sft,
896 ILLEGAL_DATA_VALUE, response);
897 }
898 }
899 break;
900 case FC_PRESET_SINGLE_REGISTER:
901 if (address >= mb_mapping->nb_holding_registers) {
902 printf("Illegal data address %0X in preset_holding_register\n", address);
903 resp_length = response_exception(mb_param, &sft,
904 ILLEGAL_DATA_ADDRESS, response);
905 } else {
906 int data = (query[offset+4] << 8) + query[offset+5];
907
908 mb_mapping->tab_holding_registers[address] = data;
909 memcpy(response, query, query_length - mb_param->checksum_length);
910 resp_length = query_length;
911 }
912 break;
913 case FC_FORCE_MULTIPLE_COILS: {
914 int nb = (query[offset+4] << 8) + query[offset+5];
915
916 if ((address + nb) > mb_mapping->nb_coil_status) {
917 printf("Illegal data address %0X in force_multiple_coils\n",
918 address + nb);
919 resp_length = response_exception(mb_param, &sft,
920 ILLEGAL_DATA_ADDRESS, response);
921 } else {
922 /* 6 = byte count, 7 = first byte of data */
923 set_bits_from_bytes(mb_mapping->tab_coil_status, address, nb, &query[offset + 7]);
924
925 resp_length = build_response_basis(mb_param, &sft, response);
926 /* 4 to copy the coil address (2) and the quantity of coils */
927 memcpy(response + resp_length, query + resp_length, 4);
928 resp_length += 4;
929 }
930 }
931 break;
932 case FC_PRESET_MULTIPLE_REGISTERS: {
933 int nb = (query[offset+4] << 8) + query[offset+5];
934
935 if ((address + nb) > mb_mapping->nb_holding_registers) {
936 printf("Illegal data address %0X in preset_multiple_registers\n",
937 address + nb);
938 resp_length = response_exception(mb_param, &sft,
939 ILLEGAL_DATA_ADDRESS, response);
940 } else {
941 int i, j;
942 for (i = address, j = 0; i < address + nb; i++, j += 2) {
943 /* 6 = byte count, 7 and 8 = first value */
944 mb_mapping->tab_holding_registers[i] =
945 (query[offset + 7 + j] << 8) + query[offset + 8 + j];
946 }
947
948 resp_length = build_response_basis(mb_param, &sft, response);
949 /* 4 to copy the address (2) and the no. of registers */
950 memcpy(response + resp_length, query + resp_length, 4);
951 resp_length += 4;
952 }
953 }
954 break;
955 case FC_READ_EXCEPTION_STATUS:
956 case FC_REPORT_SLAVE_ID:
957 printf("Not implemented\n");
958 break;
959 }
960
961 modbus_send(mb_param, response, resp_length);
962}
963
964/* Listens any message on a socket or file descriptor.
965 Returns:
966 - 0 if OK, or a negative error number if the request fails
967 - query, message received
968 - query_length, length in bytes of the message */
969int modbus_listen(modbus_param_t *mb_param, uint8_t *query, int *query_length)
970{
971 int ret;
972
973 /* The length of the query to receive isn't known. */
974 ret = receive_msg(mb_param, MSG_LENGTH_UNDEFINED, query, query_length);
975
976 return ret;
977}
978
979/* Reads IO status */
980static int read_io_status(modbus_param_t *mb_param, int slave, int function,
981 int start_addr, int nb, uint8_t *data_dest)
982{
983 int ret;
984 int query_length;
985
986 uint8_t query[MIN_QUERY_LENGTH];
987 uint8_t response[MAX_MESSAGE_LENGTH];
988
989 query_length = build_query_basis(mb_param, slave, function,
990 start_addr, nb, query);
991
992 ret = modbus_send(mb_param, query, query_length);
993 if (ret > 0) {
994 int i, temp, bit;
995 int pos = 0;
996 int processed = 0;
997 int offset;
998 int offset_length;
999
1000 ret = modbus_receive(mb_param, query, response);
1001 if (ret < 0)
1002 return ret;
1003
1004 offset = mb_param->header_length;
1005
1006 offset_length = offset + ret;
1007 mb_param->response_length=offset_length+2;
1008 for (i = offset; i < offset_length; i++) {
1009 /* Shift reg hi_byte to temp */
1010 temp = response[3 + i];
1011
1012 for (bit = 0x01; (bit & 0xff) && (processed < nb);) {
1013 data_dest[pos++] = (temp & bit) ? TRUE : FALSE;
1014 processed++;
1015 bit = bit << 1;
1016 }
1017
1018 }
1019 }
1020
1021 return ret;
1022}
1023
1024/* Reads the boolean status of coils and sets the array elements
1025 in the destination to TRUE or FALSE. */
1026int read_coil_status(modbus_param_t *mb_param, int slave, int start_addr,
1027 int nb, uint8_t *data_dest)
1028{
1029 int status;
1030
1031 if (nb > MAX_STATUS) {
1032 printf("ERROR Too many coils status requested (%d > %d)\n",
1033 nb, MAX_STATUS);
1034 return TOO_MANY_DATA;
1035 }
1036
1037 status = read_io_status(mb_param, slave, FC_READ_COIL_STATUS,
1038 start_addr, nb, data_dest);
1039
1040 if (status > 0)
1041 status = nb;
1042
1043 return status;
1044}
1045
1046
1047/* Same as read_coil_status but reads the slaves input table */
1048int read_input_status(modbus_param_t *mb_param, int slave, int start_addr,
1049 int nb, uint8_t *data_dest)
1050{
1051 int status;
1052
1053 if (nb > MAX_STATUS) {
1054 printf("ERROR Too many input status requested (%d > %d)\n",
1055 nb, MAX_STATUS);
1056 return TOO_MANY_DATA;
1057 }
1058
1059 status = read_io_status(mb_param, slave, FC_READ_INPUT_STATUS,
1060 start_addr, nb, data_dest);
1061
1062 if (status > 0)
1063 status = nb;
1064
1065 return status;
1066}
1067
1068/* Reads the data from a modbus slave and put that data into an array */
1069static int read_registers(modbus_param_t *mb_param, int slave, int function,
1070 int start_addr, int nb, uint16_t *data_dest)
1071{
1072 int ret;
1073 int query_length;
1074 uint8_t query[MIN_QUERY_LENGTH];
1075 uint8_t response[MAX_MESSAGE_LENGTH];
1076
1077 if (nb > MAX_REGISTERS) {
1078 printf("EROOR Too many holding registers requested (%d > %d)\n",
1079 nb, MAX_REGISTERS);
1080 return TOO_MANY_DATA;
1081 }
1082
1083 query_length = build_query_basis(mb_param, slave, function,
1084 start_addr, nb, query);
1085
1086 ret = modbus_send(mb_param, query, query_length);
1087 if (ret > 0) {
1088 int offset;
1089 int i;
1090
1091 ret = modbus_receive(mb_param, query, response);
1092
1093 offset = mb_param->header_length;
1094 mb_param->response_length=offset+ret+3;
1095
1096 /* If ret is negative, the loop is jumped ! */
1097 for (i = 0; i < ret; i++) {
1098 /* shift reg hi_byte to temp OR with lo_byte */
1099 data_dest[i] = (response[offset + 3 + (i << 1)] << 8) |
1100 response[offset + 4 + (i << 1)];
1101 }
1102 }
1103
1104 return ret;
1105}
1106
1107/* Reads the holding registers in a slave and put the data into an
1108 array */
1109int read_holding_registers(modbus_param_t *mb_param, int slave,
1110 int start_addr, int nb, uint16_t *data_dest)
1111{
1112 int status;
1113
1114 if (nb > MAX_REGISTERS) {
1115 printf("ERROR Too many holding registers requested (%d > %d)\n",
1116 nb, MAX_REGISTERS);
1117 return TOO_MANY_DATA;
1118 }
1119
1120 status = read_registers(mb_param, slave, FC_READ_HOLDING_REGISTERS,
1121 start_addr, nb, data_dest);
1122 return status;
1123}
1124
1125/* Reads the input registers in a slave and put the data into
1126 an array */
1127int read_input_registers(modbus_param_t *mb_param, int slave,
1128 int start_addr, int nb, uint16_t *data_dest)
1129{
1130 int status;
1131
1132 if (nb > MAX_REGISTERS) {
1133 printf("ERROR Too many input registers requested (%d > %d)\n",
1134 nb, MAX_REGISTERS);
1135 return TOO_MANY_DATA;
1136 }
1137
1138 status = read_registers(mb_param, slave, FC_READ_INPUT_REGISTERS,
1139 start_addr, nb, data_dest);
1140
1141 return status;
1142}
1143
1144/* Sends a value to a register in a slave.
1145 Used by force_single_coil and preset_single_register */
1146static int set_single(modbus_param_t *mb_param, int slave, int function,
1147 int addr, int value)
1148{
1149 int ret;
1150 int query_length;
1151 uint8_t query[MIN_QUERY_LENGTH];
1152
1153 query_length = build_query_basis(mb_param, slave, function,
1154 addr, value, query);
1155
1156 ret = modbus_send(mb_param, query, query_length);
1157 if (ret > 0) {
1158 /* Used by force_single_coil and
1159 * preset_single_register */
1160 uint8_t response[MIN_QUERY_LENGTH];
1161 ret = modbus_receive(mb_param, query, response);
1162 }
1163
1164 return ret;
1165}
1166
1167/* Turns ON or OFF a single coil in the slave device */
1168int force_single_coil(modbus_param_t *mb_param, int slave,
1169 int coil_addr, int state)
1170{
1171 int status;
1172
1173 if (state)
1174 state = 0xFF00;
1175
1176 status = set_single(mb_param, slave, FC_FORCE_SINGLE_COIL,
1177 coil_addr, state);
1178
1179 return status;
1180}
1181
1182/* Sets a value in one holding register in the slave device */
1183int preset_single_register(modbus_param_t *mb_param, int slave,
1184 int reg_addr, int value)
1185{
1186 int status;
1187
1188 status = set_single(mb_param, slave, FC_PRESET_SINGLE_REGISTER,
1189 reg_addr, value);
1190
1191 return status;
1192}
1193
1194/* Sets/resets the coils in the slave from an array in argument */
1195int force_multiple_coils(modbus_param_t *mb_param, int slave,
1196 int start_addr, int nb,
1197 const uint8_t *data_src)
1198{
1199 int ret;
1200 int i;
1201 int byte_count;
1202 int query_length;
1203 int coil_check = 0;
1204 int pos = 0;
1205
1206 uint8_t query[MAX_MESSAGE_LENGTH];
1207
1208 if (nb > MAX_STATUS) {
1209 printf("ERROR Writing to too many coils (%d > %d)\n",
1210 nb, MAX_STATUS);
1211 return TOO_MANY_DATA;
1212 }
1213
1214 query_length = build_query_basis(mb_param, slave,
1215 FC_FORCE_MULTIPLE_COILS,
1216 start_addr, nb, query);
1217 byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
1218 query[query_length++] = byte_count;
1219
1220 for (i = 0; i < byte_count; i++) {
1221 int bit;
1222
1223 bit = 0x01;
1224 query[query_length] = 0;
1225
1226 while ((bit & 0xFF) && (coil_check++ < nb)) {
1227 if (data_src[pos++])
1228 query[query_length] |= bit;
1229 else
1230 query[query_length] &=~ bit;
1231
1232 bit = bit << 1;
1233 }
1234 query_length++;
1235 }
1236
1237 ret = modbus_send(mb_param, query, query_length);
1238 if (ret > 0) {
1239 uint8_t response[MAX_MESSAGE_LENGTH];
1240 ret = modbus_receive(mb_param, query, response);
1241 }
1242
1243
1244 return ret;
1245}
1246
1247/* Copies the values in the slave from the array given in argument */
1248int preset_multiple_registers(modbus_param_t *mb_param, int slave,
1249 int start_addr, int nb,
1250 const uint16_t *data_src)
1251{
1252 int ret;
1253 int i;
1254 int query_length;
1255 int byte_count;
1256
1257 uint8_t query[MAX_MESSAGE_LENGTH];
1258
1259 if (nb > MAX_REGISTERS) {
1260 printf("ERROR Trying to write to too many registers (%d > %d)\n",
1261 nb, MAX_REGISTERS);
1262 return TOO_MANY_DATA;
1263 }
1264
1265 query_length = build_query_basis(mb_param, slave,
1266 FC_PRESET_MULTIPLE_REGISTERS,
1267 start_addr, nb, query);
1268 byte_count = nb * 2;
1269 query[query_length++] = byte_count;
1270
1271 for (i = 0; i < nb; i++) {
1272 query[query_length++] = data_src[i] >> 8;
1273 query[query_length++] = data_src[i] & 0x00FF;
1274 }
1275
1276 ret = modbus_send(mb_param, query, query_length);
1277 if (ret > 0) {
1278 uint8_t response[MAX_MESSAGE_LENGTH];
1279 ret = modbus_receive(mb_param, query, response);
1280 }
1281
1282 return ret;
1283}
1284
1285/* Returns the slave id! */
1286int report_slave_id(modbus_param_t *mb_param, int slave,
1287 uint8_t *data_dest)
1288{
1289 int ret;
1290 int query_length;
1291 uint8_t query[MIN_QUERY_LENGTH];
1292
1293 query_length = build_query_basis(mb_param, slave, FC_REPORT_SLAVE_ID,
1294 0, 0, query);
1295
1296 /* HACKISH, start_addr and count are not used */
1297 query_length -= 4;
1298
1299 ret = modbus_send(mb_param, query, query_length);
1300 if (ret > 0) {
1301 int i;
1302 int offset;
1303 int offset_length;
1304 uint8_t response[MAX_MESSAGE_LENGTH];
1305
1306 /* Byte count, slave id, run indicator status,
1307 additional data */
1308 ret = modbus_receive(mb_param, query, response);
1309 if (ret < 0)
1310 return ret;
1311
1312 offset = mb_param->header_length;
1313 offset_length = offset + ret;
1314
1315 for (i = offset; i < offset_length; i++)
1316 data_dest[i] = response[i];
1317 }
1318
1319 return ret;
1320}
1321
1322/* Initializes the modbus_param_t structure for RTU
1323 - device: "/dev/ttyS0"
1324 - baud: 9600, 19200, 57600, 115200, etc
1325 - parity: "even", "odd" or "none"
1326 - data_bits: 5, 6, 7, 8
1327 - stop_bits: 1, 2
1328*/
1329void modbus_init_rtu(modbus_param_t *mb_param, const char *device,
1330 int baud, const char *parity, int data_bit,
1331 int stop_bit)
1332{
1333 memset(mb_param, 0, sizeof(modbus_param_t));
1334 strcpy(mb_param->device, device);
1335 mb_param->baud = baud;
1336 strcpy(mb_param->parity, parity);
1337 mb_param->debug = FALSE;
1338 mb_param->data_bit = data_bit;
1339 mb_param->stop_bit = stop_bit;
1340 mb_param->type_com = RTU;
1341 mb_param->header_length = HEADER_LENGTH_RTU;
1342 mb_param->checksum_length = CHECKSUM_LENGTH_RTU;
1343 mb_param->saveRawData=FALSE;
1344}
1345
1346/* Initializes the modbus_param_t structure for TCP.
1347 - ip : "192.168.0.5"
1348 - port : 1099
1349
1350 Set the port to MODBUS_TCP_DEFAULT_PORT to use the default one
1351 (502). It's convenient to use a port number greater than or equal
1352 to 1024 because it's not necessary to be root to use this port
1353 number.
1354*/
1355void modbus_init_tcp(modbus_param_t *mb_param, const char *ip, int port)
1356{
1357 memset(mb_param, 0, sizeof(modbus_param_t));
1358 strncpy(mb_param->ip, ip, sizeof(char)*16);
1359 mb_param->port = port;
1360 mb_param->type_com = TCP;
1361 mb_param->header_length = HEADER_LENGTH_TCP;
1362 mb_param->checksum_length = CHECKSUM_LENGTH_TCP;
1363 mb_param->error_handling = FLUSH_OR_RECONNECT_ON_ERROR;
1364 mb_param->saveRawData=FALSE;
1365}
1366
1367/* By default, the error handling mode used is FLUSH_OR_RECONNECT_ON_ERROR.
1368
1369 With FLUSH_OR_RECONNECT_ON_ERROR, the library will flush to I/O
1370 port in RTU mode or attempt an immediate reconnection which may
1371 hang for several seconds if the network to the remote target unit
1372 is down in TCP mode.
1373
1374 With NOP_ON_ERROR, it is expected that the application will
1375 check for error returns and deal with them as necessary.
1376*/
1377void modbus_set_error_handling(modbus_param_t *mb_param,
1378 error_handling_t error_handling)
1379{
1380 if (error_handling == FLUSH_OR_RECONNECT_ON_ERROR ||
1381 error_handling == NOP_ON_ERROR) {
1382 mb_param->error_handling = error_handling;
1383 } else {
1384 printf("Invalid setting for error handling (not changed)\n");
1385 }
1386}
1387
1388
1389/* Sets up a serial port for RTU communications */
1390static int modbus_connect_rtu(modbus_param_t *mb_param)
1391{
1392 struct termios tios;
1393 speed_t speed;
1394
1395 if (mb_param->debug) {
1396 printf("Opening %s at %d bauds (%s)\n",
1397 mb_param->device, mb_param->baud, mb_param->parity);
1398 }
1399
1400 /* The O_NOCTTY flag tells UNIX that this program doesn't want
1401 to be the "controlling terminal" for that port. If you
1402 don't specify this then any input (such as keyboard abort
1403 signals and so forth) will affect your process
1404
1405 Timeouts are ignored in canonical input mode or when the
1406 NDELAY option is set on the file via open or fcntl */
1407 mb_param->fd = open(mb_param->device, O_RDWR | O_NOCTTY | O_NDELAY);
1408 if (mb_param->fd < 0) {
1409 perror("open");
1410 printf("ERROR Can't open the device %s (errno %d)\n",
1411 mb_param->device, errno);
1412 return -1;
1413 }
1414
1415 /* Save */
1416 tcgetattr(mb_param->fd, &(mb_param->old_tios));
1417
1418 memset(&tios, 0, sizeof(struct termios));
1419
1420 /* C_ISPEED Input baud (new interface)
1421 C_OSPEED Output baud (new interface)
1422 */
1423 switch (mb_param->baud) {
1424 case 110:
1425 speed = B110;
1426 break;
1427 case 300:
1428 speed = B300;
1429 break;
1430 case 600:
1431 speed = B600;
1432 break;
1433 case 1200:
1434 speed = B1200;
1435 break;
1436 case 2400:
1437 speed = B2400;
1438 break;
1439 case 4800:
1440 speed = B4800;
1441 break;
1442 case 9600:
1443 speed = B9600;
1444 break;
1445 case 19200:
1446 speed = B19200;
1447 break;
1448 case 38400:
1449 speed = B38400;
1450 break;
1451 case 57600:
1452 speed = B57600;
1453 break;
1454 case 115200:
1455 speed = B115200;
1456 break;
1457 default:
1458 speed = B9600;
1459 printf("WARNING Unknown baud rate %d for %s (B9600 used)\n",
1460 mb_param->baud, mb_param->device);
1461 }
1462
1463 /* Set the baud rate */
1464 if ((cfsetispeed(&tios, speed) < 0) ||
1465 (cfsetospeed(&tios, speed) < 0)) {
1466 perror("cfsetispeed/cfsetospeed\n");
1467 return -1;
1468 }
1469
1470 /* C_CFLAG Control options
1471 CLOCAL Local line - do not change "owner" of port
1472 CREAD Enable receiver
1473 */
1474 tios.c_cflag |= (CREAD | CLOCAL);
1475 /* CSIZE, HUPCL, CRTSCTS (hardware flow control) */
1476
1477 /* Set data bits (5, 6, 7, 8 bits)
1478 CSIZE Bit mask for data bits
1479 */
1480 tios.c_cflag &= ~CSIZE;
1481 switch (mb_param->data_bit) {
1482 case 5:
1483 tios.c_cflag |= CS5;
1484 break;
1485 case 6:
1486 tios.c_cflag |= CS6;
1487 break;
1488 case 7:
1489 tios.c_cflag |= CS7;
1490 break;
1491 case 8:
1492 default:
1493 tios.c_cflag |= CS8;
1494 break;
1495 }
1496
1497 /* Stop bit (1 or 2) */
1498 if (mb_param->stop_bit == 1)
1499 tios.c_cflag &=~ CSTOPB;
1500 else /* 2 */
1501 tios.c_cflag |= CSTOPB;
1502
1503 /* PARENB Enable parity bit
1504 PARODD Use odd parity instead of even */
1505 if (strncmp(mb_param->parity, "none", 4) == 0) {
1506 tios.c_cflag &=~ PARENB;
1507 } else if (strncmp(mb_param->parity, "even", 4) == 0) {
1508 tios.c_cflag |= PARENB;
1509 tios.c_cflag &=~ PARODD;
1510 } else {
1511 /* odd */
1512 tios.c_cflag |= PARENB;
1513 tios.c_cflag |= PARODD;
1514 }
1515
1516 /* Read the man page of termios if you need more information. */
1517
1518 /* This field isn't used on POSIX systems
1519 tios.c_line = 0;
1520 */
1521
1522 /* C_LFLAG Line options
1523
1524 ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals
1525 ICANON Enable canonical input (else raw)
1526 XCASE Map uppercase \lowercase (obsolete)
1527 ECHO Enable echoing of input characters
1528 ECHOE Echo erase character as BS-SP-BS
1529 ECHOK Echo NL after kill character
1530 ECHONL Echo NL
1531 NOFLSH Disable flushing of input buffers after
1532 interrupt or quit characters
1533 IEXTEN Enable extended functions
1534 ECHOCTL Echo control characters as ^char and delete as ~?
1535 ECHOPRT Echo erased character as character erased
1536 ECHOKE BS-SP-BS entire line on line kill
1537 FLUSHO Output being flushed
1538 PENDIN Retype pending input at next read or input char
1539 TOSTOP Send SIGTTOU for background output
1540
1541 Canonical input is line-oriented. Input characters are put
1542 into a buffer which can be edited interactively by the user
1543 until a CR (carriage return) or LF (line feed) character is
1544 received.
1545
1546 Raw input is unprocessed. Input characters are passed
1547 through exactly as they are received, when they are
1548 received. Generally you'll deselect the ICANON, ECHO,
1549 ECHOE, and ISIG options when using raw input
1550 */
1551
1552 /* Raw input */
1553 tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
1554
1555 /* C_IFLAG Input options
1556
1557 Constant Description
1558 INPCK Enable parity check
1559 IGNPAR Ignore parity errors
1560 PARMRK Mark parity errors
1561 ISTRIP Strip parity bits
1562 IXON Enable software flow control (outgoing)
1563 IXOFF Enable software flow control (incoming)
1564 IXANY Allow any character to start flow again
1565 IGNBRK Ignore break condition
1566 BRKINT Send a SIGINT when a break condition is detected
1567 INLCR Map NL to CR
1568 IGNCR Ignore CR
1569 ICRNL Map CR to NL
1570 IUCLC Map uppercase to lowercase
1571 IMAXBEL Echo BEL on input line too long
1572 */
1573 if (strncmp(mb_param->parity, "none", 4) == 0) {
1574 tios.c_iflag &= ~INPCK;
1575 } else {
1576 tios.c_iflag |= INPCK;
1577 }
1578
1579 /* Software flow control is disabled */
1580 tios.c_iflag &= ~(IXON | IXOFF | IXANY);
1581
1582 /* C_OFLAG Output options
1583 OPOST Postprocess output (not set = raw output)
1584 ONLCR Map NL to CR-NL
1585
1586 ONCLR ant others needs OPOST to be enabled
1587 */
1588
1589 /* Raw ouput */
1590 tios.c_oflag &=~ OPOST;
1591
1592 /* C_CC Control characters
1593 VMIN Minimum number of characters to read
1594 VTIME Time to wait for data (tenths of seconds)
1595
1596 UNIX serial interface drivers provide the ability to
1597 specify character and packet timeouts. Two elements of the
1598 c_cc array are used for timeouts: VMIN and VTIME. Timeouts
1599 are ignored in canonical input mode or when the NDELAY
1600 option is set on the file via open or fcntl.
1601
1602 VMIN specifies the minimum number of characters to read. If
1603 it is set to 0, then the VTIME value specifies the time to
1604 wait for every character read. Note that this does not mean
1605 that a read call for N bytes will wait for N characters to
1606 come in. Rather, the timeout will apply to the first
1607 character and the read call will return the number of
1608 characters immediately available (up to the number you
1609 request).
1610
1611 If VMIN is non-zero, VTIME specifies the time to wait for
1612 the first character read. If a character is read within the
1613 time given, any read will block (wait) until all VMIN
1614 characters are read. That is, once the first character is
1615 read, the serial interface driver expects to receive an
1616 entire packet of characters (VMIN bytes total). If no
1617 character is read within the time allowed, then the call to
1618 read returns 0. This method allows you to tell the serial
1619 driver you need exactly N bytes and any read call will
1620 return 0 or N bytes. However, the timeout only applies to
1621 the first character read, so if for some reason the driver
1622 misses one character inside the N byte packet then the read
1623 call could block forever waiting for additional input
1624 characters.
1625
1626 VTIME specifies the amount of time to wait for incoming
1627 characters in tenths of seconds. If VTIME is set to 0 (the
1628 default), reads will block (wait) indefinitely unless the
1629 NDELAY option is set on the port with open or fcntl.
1630 */
1631 /* Unused because we use open with the NDELAY option */
1632 tios.c_cc[VMIN] = 0;
1633 tios.c_cc[VTIME] = 0;
1634
1635 if (tcsetattr(mb_param->fd, TCSANOW, &tios) < 0) {
1636 perror("tcsetattr\n");
1637 return -1;
1638 }
1639
1640 return 0;
1641}
1642
1643/* Establishes a modbus TCP connection with a modbus slave */
1644static int modbus_connect_tcp(modbus_param_t *mb_param)
1645{
1646 int ret;
1647 int option;
1648 struct sockaddr_in addr;
1649
1650 addr.sin_family = AF_INET;
1651 addr.sin_port = htons(mb_param->port);
1652 addr.sin_addr.s_addr = inet_addr(mb_param->ip);
1653
1654 mb_param->fd = socket(AF_INET, SOCK_STREAM, 0);
1655 if (mb_param->fd < 0) {
1656 return mb_param->fd;
1657 }
1658
1659 /* Set the TCP no delay flag */
1660 /* SOL_TCP = IPPROTO_TCP */
1661 option = 1;
1662 ret = setsockopt(mb_param->fd, IPPROTO_TCP, TCP_NODELAY,
1663 (const void *)&option, sizeof(int));
1664 if (ret < 0) {
1665 perror("setsockopt");
1666 close(mb_param->fd);
1667 return ret;
1668 }
1669
1670#ifndef __APPLE__
1671 /* Set the IP low delay option- Not Supported on Mac OS X MJB 02/16/09 */
1672 option = IPTOS_LOWDELAY;
1673 ret = setsockopt(mb_param->fd, IPPROTO_TCP, IP_TOS,
1674 (const void *)&option, sizeof(int));
1675 if (ret < 0) {
1676 perror("setsockopt");
1677 close(mb_param->fd);
1678 return ret;
1679 }
1680#endif
1681
1682 if (mb_param->debug) {
1683 printf("Connecting to %s\n", mb_param->ip);
1684 }
1685
1686 ret = connect(mb_param->fd, (struct sockaddr *)&addr,
1687 sizeof(struct sockaddr_in));
1688 if (ret < 0) {
1689 perror("connect");
1690 close(mb_param->fd);
1691 return ret;
1692 }
1693
1694 return 0;
1695}
1696
1697/* Establishes a modbus connexion.
1698 Returns -1 if an error occured. */
1699int modbus_connect(modbus_param_t *mb_param)
1700{
1701 int ret;
1702
1703 if (mb_param->type_com == RTU)
1704 ret = modbus_connect_rtu(mb_param);
1705 else
1706 ret = modbus_connect_tcp(mb_param);
1707
1708 return ret;
1709}
1710
1711/* Closes the file descriptor in RTU mode */
1712static void modbus_close_rtu(modbus_param_t *mb_param)
1713{
1714 if (tcsetattr(mb_param->fd, TCSANOW, &(mb_param->old_tios)) < 0)
1715 perror("tcsetattr");
1716
1717 close(mb_param->fd);
1718}
1719
1720/* Closes the network connection and socket in TCP mode */
1721static void modbus_close_tcp(modbus_param_t *mb_param)
1722{
1723 shutdown(mb_param->fd, SHUT_RDWR);
1724 close(mb_param->fd);
1725}
1726
1727/* Closes a modbus connection */
1728void modbus_close(modbus_param_t *mb_param)
1729{
1730 if (mb_param->type_com == RTU)
1731 modbus_close_rtu(mb_param);
1732 else
1733 modbus_close_tcp(mb_param);
1734}
1735
1736/* Activates the debug messages */
1737void modbus_set_debug(modbus_param_t *mb_param, int boolean)
1738{
1739 mb_param->debug = boolean;
1740}
1741
1742
1743// Activates saving raw query and responses
1744void modbus_set_raw_data_save(modbus_param_t *mb_param, int boolean)
1745{
1746 mb_param->saveRawData=boolean;
1747}
1748
1749// Returns a raw query
1750int modbus_last_raw_query(modbus_param_t *mb_param, uint8_t *rawQuery)
1751{
1752 int i;
1753 if(mb_param->rawQuery!=NULL)
1754 {
1755 for (i = 0; i < MIN_QUERY_LENGTH; i++)
1756 rawQuery[i]=mb_param->rawQuery[i];
1757 return TRUE;
1758 }
1759 return FALSE;
1760}
1761
1762// Returns a raw response
1763int modbus_last_raw_response(modbus_param_t *mb_param, uint8_t *rawResponse)
1764{
1765 int i;
1766 if(mb_param->rawResponse!=NULL)
1767 {
1768 for (i = 0; i < mb_param->rawResponseLength; i++)
1769 rawResponse[i]=mb_param->rawResponse[i];
1770 return TRUE;
1771 }
1772 return FALSE;
1773}
1774
1775int modbus_last_raw_query_length(modbus_param_t *mb_param)
1776{
1777 return(mb_param->rawQueryLength);
1778}
1779
1780int modbus_last_raw_response_length(modbus_param_t *mb_param)
1781{
1782 return(mb_param->rawResponseLength);
1783}
1784
1785
1786/* Allocates 4 arrays to store coils, input status, input registers and
1787 holding registers. The pointers are stored in modbus_mapping structure.
1788
1789 Returns: TRUE if ok, FALSE on failure
1790*/
1791int modbus_mapping_new(modbus_mapping_t *mb_mapping,
1792 int nb_coil_status, int nb_input_status,
1793 int nb_holding_registers, int nb_input_registers)
1794{
1795 /* 0X */
1796 mb_mapping->nb_coil_status = nb_coil_status;
1797 mb_mapping->tab_coil_status =
1798 (uint8_t *) malloc(nb_coil_status * sizeof(uint8_t));
1799 memset(mb_mapping->tab_coil_status, 0,
1800 nb_coil_status * sizeof(uint8_t));
1801 if (mb_mapping->tab_coil_status == NULL)
1802 return FALSE;
1803
1804 /* 1X */
1805 mb_mapping->nb_input_status = nb_input_status;
1806 mb_mapping->tab_input_status =
1807 (uint8_t *) malloc(nb_input_status * sizeof(uint8_t));
1808 memset(mb_mapping->tab_input_status, 0,
1809 nb_input_status * sizeof(uint8_t));
1810 if (mb_mapping->tab_input_status == NULL) {
1811 free(mb_mapping->tab_coil_status);
1812 return FALSE;
1813 }
1814
1815 /* 4X */
1816 mb_mapping->nb_holding_registers = nb_holding_registers;
1817 mb_mapping->tab_holding_registers =
1818 (uint16_t *) malloc(nb_holding_registers * sizeof(uint16_t));
1819 memset(mb_mapping->tab_holding_registers, 0,
1820 nb_holding_registers * sizeof(uint16_t));
1821 if (mb_mapping->tab_holding_registers == NULL) {
1822 free(mb_mapping->tab_coil_status);
1823 free(mb_mapping->tab_input_status);
1824 return FALSE;
1825 }
1826
1827 /* 3X */
1828 mb_mapping->nb_input_registers = nb_input_registers;
1829 mb_mapping->tab_input_registers =
1830 (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t));
1831 memset(mb_mapping->tab_input_registers, 0,
1832 nb_input_registers * sizeof(uint16_t));
1833 if (mb_mapping->tab_input_registers == NULL) {
1834 free(mb_mapping->tab_coil_status);
1835 free(mb_mapping->tab_input_status);
1836 free(mb_mapping->tab_holding_registers);
1837 return FALSE;
1838 }
1839
1840 return TRUE;
1841}
1842
1843/* Frees the 4 arrays */
1844void modbus_mapping_free(modbus_mapping_t *mb_mapping)
1845{
1846 free(mb_mapping->tab_coil_status);
1847 free(mb_mapping->tab_input_status);
1848 free(mb_mapping->tab_holding_registers);
1849 free(mb_mapping->tab_input_registers);
1850}
1851
1852/* Listens for any query from a modbus master in TCP */
1853int modbus_init_listen_tcp(modbus_param_t *mb_param)
1854{
1855 int ret;
1856 int new_socket;
1857 struct sockaddr_in addr;
1858 socklen_t addrlen;
1859
1860 addr.sin_family = AF_INET;
1861 /* If the modbus port is < to 1024, we need the setuid root. */
1862 addr.sin_port = htons(mb_param->port);
1863 addr.sin_addr.s_addr = INADDR_ANY;
1864 memset(&(addr.sin_zero), '\0', 8);
1865
1866 new_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
1867 if (new_socket < 0) {
1868 perror("socket");
1869 exit(1);
1870 } else {
1871 printf("Socket OK\n");
1872 }
1873
1874 ret = bind(new_socket, (struct sockaddr *)&addr,
1875 sizeof(struct sockaddr_in));
1876 if (ret < 0) {
1877 perror("bind");
1878 close(new_socket);
1879 exit(1);
1880 } else {
1881 printf("Bind OK\n");
1882 }
1883
1884 ret = listen(new_socket, 1);
1885 if (ret != 0) {
1886 perror("listen");
1887 close(new_socket);
1888 exit(1);
1889 } else {
1890 printf("Listen OK\n");
1891 }
1892
1893 addrlen = sizeof(struct sockaddr_in);
1894 mb_param->fd = accept(new_socket, (struct sockaddr *)&addr, &addrlen);
1895 if (ret < 0) {
1896 perror("accept");
1897 close(new_socket);
1898 new_socket = 0;
1899 exit(1);
1900 } else {
1901 printf("The client %s is connected\n",
1902 inet_ntoa(addr.sin_addr));
1903 }
1904
1905 return new_socket;
1906}
1907
1908/** Utils **/
1909
1910/* Sets many input/coil status from a single byte value (all 8 bits of
1911 the byte value are setted) */
1912void set_bits_from_byte(uint8_t *dest, int address, const uint8_t value)
1913{
1914 int i;
1915
1916 for (i=0; i<8; i++) {
1917 dest[address+i] = (value & (1 << i)) ? ON : OFF;
1918 }
1919}
1920
1921/* Sets many input/coil status from a table of bytes (only the bits
1922 between address and address + nb_bits are setted) */
1923void set_bits_from_bytes(uint8_t *dest, int address, int nb_bits,
1924 const uint8_t tab_byte[])
1925{
1926 int i;
1927 int shift = 0;
1928
1929 for (i = address; i < address + nb_bits; i++) {
1930 dest[i] = tab_byte[(i - address) / 8] & (1 << shift) ? ON : OFF;
1931 /* gcc doesn't like: shift = (++shift) % 8; */
1932 shift++;
1933 shift %= 8;
1934 }
1935}
1936
1937/* Gets the byte value from many input/coil status.
1938 To obtain a full byte, set nb_bits to 8. */
1939uint8_t get_byte_from_bits(const uint8_t *src, int address, int nb_bits)
1940{
1941 int i;
1942 uint8_t value = 0;
1943
1944 if (nb_bits > 8) {
1945 printf("Error: nb_bits is too big\n");
1946 nb_bits = 8;
1947 }
1948
1949 for (i=0; i < nb_bits; i++) {
1950 value |= (src[address+i] << i);
1951 }
1952
1953 return value;
1954}
Note: See TracBrowser for help on using the repository browser.