| author | Edouard Tisserant <edouard.tisserant@gmail.com> |
| Mon, 07 Jun 2021 11:21:26 +0200 | |
| changeset 17 | e319814f1c17 |
| parent 15 | cadd89d14ca5 |
| permissions | -rw-r--r-- |
| 0 | 1 |
/* |
2 |
* Copyright (c) 2001,2016 Mario de Sousa (msousa@fe.up.pt) |
|
3 |
* |
|
4 |
* This file is part of the Modbus library for Beremiz and matiec. |
|
5 |
* |
|
6 |
* This Modbus library is free software: you can redistribute it and/or modify |
|
7 |
* it under the terms of the GNU Lesser General Public License as published by |
|
8 |
* the Free Software Foundation, either version 3 of the License, or |
|
9 |
* (at your option) any later version. |
|
10 |
* |
|
11 |
* This program is distributed in the hope that it will be useful, but |
|
12 |
* WITHOUT ANY WARRANTY; without even the implied warranty of |
|
13 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser |
|
14 |
* General Public License for more details. |
|
15 |
* |
|
16 |
* You should have received a copy of the GNU Lesser General Public License |
|
17 |
* along with this Modbus library. If not, see <http://www.gnu.org/licenses/>. |
|
18 |
* |
|
19 |
* This code is made available on the understanding that it will not be |
|
20 |
* used in safety-critical situations without a full and competent review. |
|
21 |
*/ |
|
22 |
||
23 |
||
24 |
||
25 |
#include <fcntl.h> /* File control definitions */ |
|
26 |
#include <stdio.h> /* Standard input/output */ |
|
27 |
#include <string.h> |
|
28 |
#include <stdlib.h> |
|
29 |
#include <termio.h> /* POSIX terminal control definitions */ |
|
30 |
#include <sys/time.h> /* Time structures for select() */ |
|
31 |
#include <unistd.h> /* POSIX Symbolic Constants */ |
|
32 |
#include <assert.h> |
|
33 |
#include <errno.h> /* Error definitions */ |
|
34 |
#include <time.h> /* clock_gettime() */ |
|
35 |
#include <limits.h> /* required for INT_MAX */ |
|
36 |
||
37 |
#include <netinet/in.h> /* required for htons() and ntohs() */ |
|
38 |
||
39 |
#include "mb_layer1.h" /* The public interface this file implements... */ |
|
40 |
#include "mb_rtu_private.h" |
|
41 |
||
42 |
||
43 |
#define ERRMSG |
|
44 |
#define ERRMSG_HEAD "ModbusRTU: " |
|
45 |
||
46 |
// #define DEBUG /* uncomment to see the data sent and received */ |
|
47 |
||
48 |
#ifdef DEBUG |
|
49 |
#ifndef ERRMSG |
|
50 |
#define ERRMSG |
|
51 |
#endif |
|
52 |
#endif |
|
53 |
||
54 |
||
55 |
#define SAFETY_MARGIN 10 |
|
56 |
||
57 |
/************************************/ |
|
58 |
/** **/ |
|
59 |
/** Include common code... **/ |
|
60 |
/** **/ |
|
61 |
/************************************/ |
|
62 |
||
63 |
#include "mb_ds_util.h" /* data structures... */ |
|
64 |
#include "mb_time_util.h" /* time conversion routines... */ |
|
65 |
||
66 |
||
67 |
||
68 |
/**************************************************************/ |
|
69 |
/**************************************************************/ |
|
70 |
/**** ****/ |
|
71 |
/**** ****/ |
|
72 |
/**** Forward Declarations ****/ |
|
73 |
/**** and Defaults ****/ |
|
74 |
/**** ****/ |
|
75 |
/**************************************************************/ |
|
76 |
/**************************************************************/ |
|
77 |
||
78 |
/* CRC funtions... */ |
|
79 |
typedef u16 (*crc_func_t)(u8 *buf, int cnt); |
|
80 |
static u16 crc_slow(u8 *buf, int cnt); |
|
81 |
static u16 crc_fast(u8 *buf, int cnt); |
|
82 |
||
83 |
/* slow version does not need to be initialised, so we use it as default. */ |
|
84 |
#define DEF_CRC_FUNCTION crc_slow |
|
85 |
||
86 |
||
87 |
/**************************************************************/ |
|
88 |
/**************************************************************/ |
|
89 |
/**** ****/ |
|
90 |
/**** ****/ |
|
91 |
/**** Local Utility functions... ****/ |
|
92 |
/**** ****/ |
|
93 |
/**** ****/ |
|
94 |
/**************************************************************/ |
|
95 |
/**************************************************************/ |
|
96 |
||
97 |
/************************************/ |
|
98 |
/** **/ |
|
99 |
/** Miscelaneous Utility functions **/ |
|
100 |
/** **/ |
|
101 |
/************************************/ |
|
102 |
||
103 |
/* |
|
104 |
* Functions to convert u16 variables |
|
105 |
* between network and host byte order |
|
106 |
* |
|
107 |
* NOTE: Modbus uses MSByte first, just like |
|
108 |
* tcp/ip, so we use the htons() and |
|
109 |
* ntoh() functions to guarantee |
|
110 |
* code portability. |
|
111 |
*/ |
|
112 |
static inline u16 mb_hton(u16 h_value) |
|
113 |
{return htons(h_value);} /* return h_value; */
|
|
114 |
||
115 |
static inline u16 mb_ntoh(u16 m_value) |
|
116 |
{return ntohs(m_value);} /* return m_value; */
|
|
117 |
||
118 |
/* return Most Significant Byte of value; */ |
|
119 |
static inline u8 msb(u16 value) |
|
120 |
{return (value >> 8) & 0xFF;}
|
|
121 |
||
122 |
/* return Least Significant Byte of value; */ |
|
123 |
static inline u8 lsb(u16 value) |
|
124 |
{return value & 0xFF;}
|
|
125 |
||
126 |
#define u16_v(char_ptr) (*((u16 *)(&(char_ptr)))) |
|
127 |
||
128 |
||
129 |
||
130 |
/**************************************/ |
|
131 |
/** **/ |
|
132 |
/** Initialise a termios struct **/ |
|
133 |
/** **/ |
|
134 |
/**************************************/ |
|
135 |
static int termios_init(struct termios *tios, |
|
136 |
int baud, |
|
137 |
int parity, |
|
138 |
int data_bits, |
|
139 |
int stop_bits) {
|
|
140 |
speed_t baud_rate; |
|
141 |
||
142 |
if (tios == NULL) |
|
143 |
return -1; |
|
144 |
||
145 |
/* reset all the values... */ |
|
146 |
/* NOTE: the following are initialised later on... |
|
147 |
tios->c_iflag = 0; |
|
148 |
tios->c_oflag = 0; |
|
149 |
tios->c_cflag = 0; |
|
150 |
tios->c_lflag = 0; |
|
151 |
*/ |
|
152 |
tios->c_line = 0; |
|
153 |
||
154 |
/* The minimum number of characters that should be received |
|
155 |
* to satisfy a call to read(). |
|
156 |
*/ |
|
157 |
tios->c_cc[VMIN ] = 0; |
|
158 |
||
159 |
/* The maximum inter-arrival interval between two characters, |
|
160 |
* in deciseconds. |
|
161 |
* |
|
162 |
* NOTE: we could use this to detect the end of RTU frames, |
|
163 |
* but we prefer to use select() that has higher resolution, |
|
164 |
* even though this higher resolution is most probably not |
|
165 |
* supported, and the effective resolution is 10ms, |
|
166 |
* one tenth of a decisecond. |
|
167 |
*/ |
|
168 |
tios->c_cc[VTIME] = 0; |
|
169 |
||
170 |
/* configure the input modes... */ |
|
171 |
tios->c_iflag = IGNBRK | /* ignore BREAK condition on input */ |
|
172 |
IGNPAR | /* ignore framing errors and parity errors */ |
|
173 |
IXANY; /* enable any character to restart output */ |
|
174 |
/* BRKINT Only active if IGNBRK is not set. |
|
175 |
* generate SIGINT on BREAK condition, |
|
176 |
* otherwise read BREAK as character \0. |
|
177 |
* PARMRK Only active if IGNPAR is not set. |
|
178 |
* replace bytes with parity errors with |
|
179 |
* \377 \0, instead of \0. |
|
180 |
* INPCK enable input parity checking |
|
181 |
* ISTRIP strip off eighth bit |
|
182 |
* IGNCR ignore carriage return on input |
|
183 |
* INLCR only active if IGNCR is not set. |
|
184 |
* translate newline to carriage return on input |
|
185 |
* ICRNL only active if IGNCR is not set. |
|
186 |
* translate carriage return to newline on input |
|
187 |
* IUCLC map uppercase characters to lowercase on input |
|
188 |
* IXON enable XON/XOFF flow control on output |
|
189 |
* IXOFF enable XON/XOFF flow control on input |
|
190 |
* IMAXBEL ring bell when input queue is full |
|
191 |
*/ |
|
192 |
||
193 |
/* configure the output modes... */ |
|
194 |
tios->c_oflag = OPOST; /* enable implementation-defined output processing */ |
|
195 |
/* ONOCR don't output CR at column 0 |
|
196 |
* OLCUC map lowercase characters to uppercase on output |
|
197 |
* ONLCR map NL to CR-NL on output |
|
198 |
* OCRNL map CR to NL on output |
|
199 |
* OFILL send fill characters for a delay, rather than |
|
200 |
* using a timed delay |
|
201 |
* OFDEL fill character is ASCII DEL. If unset, fill |
|
202 |
* character is ASCII NUL |
|
203 |
* ONLRET don't output CR |
|
204 |
* NLDLY NL delay mask. Values are NL0 and NL1. |
|
205 |
* CRDLY CR delay mask. Values are CR0, CR1, CR2, or CR3. |
|
206 |
* TABDLY horizontal tab delay mask. Values are TAB0, TAB1, |
|
207 |
* TAB2, TAB3, or XTABS. A value of XTABS expands |
|
208 |
* tabs to spaces (with tab stops every eight columns). |
|
209 |
* BSDLY backspace delay mask. Values are BS0 or BS1. |
|
210 |
* VTDLY vertical tab delay mask. Values are VT0 or VT1. |
|
211 |
* FFDLY form feed delay mask. Values are FF0 or FF1. |
|
212 |
*/ |
|
213 |
||
214 |
/* configure the control modes... */ |
|
215 |
tios->c_cflag = CREAD | /* enable receiver. */ |
|
216 |
CLOCAL; /* ignore modem control lines */ |
|
217 |
/* HUPCL lower modem control lines after last process |
|
218 |
* closes the device (hang up). |
|
219 |
* CRTSCTS flow control (Request/Clear To Send). |
|
220 |
*/ |
|
221 |
if (data_bits == 5) tios->c_cflag |= CS5; |
|
222 |
else if (data_bits == 6) tios->c_cflag |= CS6; |
|
223 |
else if (data_bits == 7) tios->c_cflag |= CS7; |
|
224 |
else if (data_bits == 8) tios->c_cflag |= CS8; |
|
225 |
else return -1; |
|
226 |
||
227 |
if (stop_bits == 1) tios->c_cflag &=~ CSTOPB; |
|
228 |
else if (stop_bits == 2) tios->c_cflag |= CSTOPB; |
|
229 |
else return -1; |
|
230 |
||
231 |
if(parity == 0) { /* none */
|
|
232 |
tios->c_cflag &=~ PARENB; |
|
233 |
tios->c_cflag &=~ PARODD; |
|
234 |
} else if(parity == 2) { /* even */
|
|
235 |
tios->c_cflag |= PARENB; |
|
236 |
tios->c_cflag &=~ PARODD; |
|
237 |
} else if(parity == 1) { /* odd */
|
|
238 |
tios->c_cflag |= PARENB; |
|
239 |
tios->c_cflag |= PARODD; |
|
240 |
} else return -1; |
|
241 |
||
242 |
||
243 |
/* configure the local modes... */ |
|
244 |
tios->c_lflag = IEXTEN; /* enable implementation-defined input processing */ |
|
245 |
/* ISIG when any of the characters INTR, QUIT, SUSP, or DSUSP |
|
246 |
* are received, generate the corresponding signal. |
|
247 |
* ICANON enable canonical mode. This enables the special |
|
248 |
* characters EOF, EOL, EOL2, ERASE, KILL, REPRINT, |
|
249 |
* STATUS, and WERASE, and buffers by lines. |
|
250 |
* ECHO echo input characters. |
|
251 |
*/ |
|
252 |
||
253 |
/* Set the baud rate */ |
|
254 |
/* Must be done before reseting all the values to 0! */ |
|
255 |
switch(baud) {
|
|
256 |
case 110: baud_rate = B110; break; |
|
257 |
case 300: baud_rate = B300; break; |
|
258 |
case 600: baud_rate = B600; break; |
|
259 |
case 1200: baud_rate = B1200; break; |
|
260 |
case 2400: baud_rate = B2400; break; |
|
261 |
case 4800: baud_rate = B4800; break; |
|
262 |
case 9600: baud_rate = B9600; break; |
|
263 |
case 19200: baud_rate = B19200; break; |
|
264 |
case 38400: baud_rate = B38400; break; |
|
265 |
case 57600: baud_rate = B57600; break; |
|
266 |
case 115200: baud_rate = B115200; break; |
|
267 |
default: return -1; |
|
268 |
} /* switch() */ |
|
269 |
||
270 |
if ((cfsetispeed(tios, baud_rate) < 0) || |
|
271 |
(cfsetospeed(tios, baud_rate) < 0)) |
|
272 |
return -1;; |
|
273 |
||
274 |
return 0; |
|
275 |
} |
|
276 |
||
277 |
||
278 |
/************************************/ |
|
279 |
/** **/ |
|
280 |
/** A data structure - recv buffer **/ |
|
281 |
/** **/ |
|
282 |
/************************************/ |
|
283 |
||
284 |
/* A data structutre used for the receive buffer, i.e. the buffer |
|
285 |
* that stores the bytes we receive from the bus. |
|
286 |
* |
|
287 |
* What we realy needed here is an unbounded buffer. This may be |
|
288 |
* implemented by: |
|
289 |
* - a circular buffer the size of the maximum frame length |
|
290 |
* - a linear buffer somewhat larger than the maximum frame length |
|
291 |
* |
|
292 |
* Due to the fact that this library's API hands over the frame data |
|
293 |
* in a linear buffer, and also reads the data (i,e, calls to read()) |
|
294 |
* into a linear buffer: |
|
295 |
* - the circular buffer would be more efficient in aborted frame |
|
296 |
* situations |
|
297 |
* - the linear is more efficient when no aborted frames are recieved. |
|
298 |
* |
|
299 |
* I have decided to optimize for the most often encountered situation, |
|
300 |
* i.e. when no aborted frames are received. |
|
301 |
* |
|
302 |
* The linear buffer has a size larger than the maximum |
|
303 |
* number of bytes we intend to store in it. We simply start ignoring |
|
304 |
* the first bytes in the buffer in which we are not interested in, and |
|
305 |
* continue with the extra bytes of the buffer. When we reach the limit |
|
306 |
* of these extra bytes, we shift the data down so it once again |
|
307 |
* uses the first bytes of the buffer. The more number of extra bytes, |
|
308 |
* the more efficient it will be. |
|
309 |
* |
|
310 |
* Note that if we don't receive any aborted frames, it will work as a |
|
311 |
* simple linear buffer, and no memory shifts will be required! |
|
312 |
*/ |
|
313 |
||
314 |
typedef struct {
|
|
315 |
lb_buf_t data_buf; |
|
316 |
/* Flag: |
|
317 |
* 1 => We have detected a frame boundary using 3.5 character silence |
|
318 |
* 0 => We have not yet detected any frame boundary |
|
319 |
*/ |
|
320 |
int found_frame_boundary; /* ==1 => valid data ends at a frame boundary. */ |
|
321 |
/* Flag: |
|
322 |
* Used in the call to search_for_frame() as the history parameter! |
|
323 |
*/ |
|
324 |
int frame_search_history; |
|
325 |
} recv_buf_t; |
|
326 |
||
327 |
/* A small auxiliary function... */ |
|
328 |
static inline u8 *recv_buf_init(recv_buf_t *buf, int size, int max_data_start) {
|
|
329 |
buf->found_frame_boundary = 0; |
|
330 |
buf->frame_search_history = 0; |
|
331 |
return lb_init(&buf->data_buf, size, max_data_start); |
|
332 |
} |
|
333 |
||
334 |
||
335 |
/* A small auxiliary function... */ |
|
336 |
static inline void recv_buf_done(recv_buf_t *buf) {
|
|
337 |
buf->found_frame_boundary = 0; |
|
338 |
buf->frame_search_history = 0; |
|
339 |
lb_done(&buf->data_buf); |
|
340 |
} |
|
341 |
||
342 |
||
343 |
/* A small auxiliary function... */ |
|
344 |
static inline void recv_buf_reset(recv_buf_t *buf) {
|
|
345 |
buf->found_frame_boundary = 0; |
|
346 |
buf->frame_search_history = 0; |
|
347 |
lb_data_purge_all(&buf->data_buf); |
|
348 |
} |
|
349 |
||
350 |
||
351 |
/************************************/ |
|
352 |
/** **/ |
|
353 |
/** A data structure - nd entry **/ |
|
354 |
/** **/ |
|
355 |
/************************************/ |
|
356 |
||
357 |
/* NOTE: nd = node descriptor */ |
|
358 |
||
359 |
typedef struct {
|
|
360 |
/* The file descriptor associated with this node */ |
|
361 |
/* NOTE: if the node is not yet in use, i.e. if the node is free, |
|
362 |
* then fd will be set to -1 |
|
363 |
*/ |
|
364 |
int fd; |
|
365 |
||
366 |
/* the time it takes to transmit 1.5 characters at the current baud rate */ |
|
367 |
struct timeval time_15_char_; |
|
368 |
/* the time it takes to transmit 3.5 characters at the current baud rate */ |
|
369 |
struct timeval time_35_char_; |
|
370 |
||
371 |
/* Due to the algorithm used to work around aborted frames, the modbus_read() |
|
372 |
* function might read beyond the current modbus frame. The extra bytes |
|
373 |
* must be stored for the subsequent call to modbus_read(). |
|
374 |
*/ |
|
375 |
recv_buf_t recv_buf_; |
|
376 |
||
377 |
/* The old settings of the serial port, to be reset when the library is closed... */ |
|
378 |
struct termios old_tty_settings_; |
|
379 |
||
380 |
/* ignore echo flag. |
|
381 |
* If set to 1, then it means that we will be reading every byte we |
|
382 |
* ourselves write out to the bus, so we must ignore those bytes read |
|
383 |
* before we really read the data sent by remote nodes. |
|
384 |
* |
|
385 |
* This comes in useful when using a RS232-RS485 converter that does |
|
386 |
* not correctly control the RTS-CTS lines... |
|
387 |
*/ |
|
388 |
int ignore_echo; |
|
389 |
} nd_entry_t; |
|
390 |
||
391 |
||
392 |
static inline void nd_entry_init(nd_entry_t *nde) {
|
|
393 |
nde->fd = -1; /* The node is free... */ |
|
394 |
} |
|
395 |
||
396 |
||
397 |
||
398 |
static int nd_entry_connect(nd_entry_t *nde, |
|
399 |
node_addr_t *node_addr, |
|
400 |
optimization_t opt) {
|
|
401 |
||
402 |
int parity_bits, start_bits, char_bits; |
|
403 |
struct termios settings; |
|
404 |
int buf_size; |
|
405 |
||
406 |
/* |
|
407 |
if (nde == NULL) |
|
408 |
goto error_exit_0; |
|
409 |
*/ |
|
410 |
if (nde->fd >= 0) |
|
411 |
goto error_exit_0; |
|
412 |
||
413 |
/* initialise the termios data structure */ |
|
414 |
if (termios_init(&settings, |
|
415 |
node_addr->addr.rtu.baud, |
|
416 |
node_addr->addr.rtu.parity, |
|
417 |
node_addr->addr.rtu.data_bits, |
|
418 |
node_addr->addr.rtu.stop_bits) |
|
419 |
< 0) {
|
|
420 |
#ifdef ERRMSG |
|
421 |
fprintf(stderr, ERRMSG_HEAD "Invalid serial line settings" |
|
422 |
"(baud=%d, parity=%d, data_bits=%d, stop_bits=%d)\n", |
|
423 |
node_addr->addr.rtu.baud, |
|
424 |
node_addr->addr.rtu.parity, |
|
425 |
node_addr->addr.rtu.data_bits, |
|
426 |
node_addr->addr.rtu.stop_bits); |
|
427 |
#endif |
|
428 |
goto error_exit_1; |
|
429 |
} |
|
430 |
||
431 |
/* set the ignore_echo flag */ |
|
432 |
nde->ignore_echo = node_addr->addr.rtu.ignore_echo; |
|
433 |
||
434 |
/* initialise recv buffer */ |
|
435 |
buf_size = (opt == optimize_size)?RECV_BUFFER_SIZE_SMALL: |
|
436 |
RECV_BUFFER_SIZE_LARGE; |
|
437 |
if (recv_buf_init(&nde->recv_buf_, buf_size, buf_size - MAX_RTU_FRAME_LENGTH) |
|
438 |
== NULL) {
|
|
439 |
#ifdef ERRMSG |
|
440 |
fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing receive buffer\n"); |
|
441 |
#endif |
|
442 |
goto error_exit_2; |
|
443 |
} |
|
444 |
||
445 |
/* open the serial port */ |
|
446 |
if((nde->fd = open(node_addr->addr.rtu.device, O_RDWR | O_NOCTTY | O_NDELAY)) |
|
447 |
< 0) {
|
|
448 |
#ifdef ERRMSG |
|
449 |
perror("open()");
|
|
450 |
fprintf(stderr, ERRMSG_HEAD "Error opening device %s\n", |
|
451 |
node_addr->addr.rtu.device); |
|
452 |
#endif |
|
453 |
goto error_exit_3; |
|
454 |
} |
|
455 |
||
456 |
if(tcgetattr(nde->fd, &nde->old_tty_settings_) < 0) {
|
|
457 |
#ifdef ERRMSG |
|
458 |
perror("tcgetattr()");
|
|
459 |
fprintf(stderr, ERRMSG_HEAD "Error reading device's %s original settings.\n", |
|
460 |
node_addr->addr.rtu.device); |
|
461 |
#endif |
|
462 |
goto error_exit_4; |
|
463 |
} |
|
464 |
||
465 |
if(tcsetattr(nde->fd, TCSANOW, &settings) < 0) {
|
|
466 |
#ifdef ERRMSG |
|
467 |
perror("tcsetattr()");
|
|
468 |
fprintf(stderr, ERRMSG_HEAD "Error configuring device %s " |
|
469 |
"(baud=%d, parity=%d, data_bits=%d, stop_bits=%d)\n", |
|
470 |
node_addr->addr.rtu.device, |
|
471 |
node_addr->addr.rtu.baud, |
|
472 |
node_addr->addr.rtu.parity, |
|
473 |
node_addr->addr.rtu.data_bits, |
|
474 |
node_addr->addr.rtu.stop_bits); |
|
475 |
#endif |
|
476 |
goto error_exit_4; |
|
477 |
} |
|
478 |
||
479 |
parity_bits = (node_addr->addr.rtu.parity == 0)?0:1; |
|
480 |
start_bits = 1; |
|
481 |
char_bits = start_bits + node_addr->addr.rtu.data_bits + |
|
482 |
parity_bits + node_addr->addr.rtu.stop_bits; |
|
483 |
nde->time_15_char_ = d_to_timeval(SAFETY_MARGIN*1.5*char_bits/node_addr->addr.rtu.baud); |
|
484 |
nde->time_35_char_ = d_to_timeval(SAFETY_MARGIN*3.5*char_bits/node_addr->addr.rtu.baud); |
|
485 |
||
486 |
#ifdef DEBUG |
|
487 |
fprintf(stderr, "nd_entry_connect(): %s ope{.node=NULL, .node_count=0};n\n", node_addr->addr.rtu.device );
|
|
488 |
fprintf(stderr, "nd_entry_connect(): returning fd=%d\n", nde->fd); |
|
489 |
#endif |
|
490 |
return nde->fd; |
|
491 |
||
492 |
error_exit_4: |
|
493 |
close(nde->fd); |
|
494 |
error_exit_3: |
|
495 |
recv_buf_done(&nde->recv_buf_); |
|
496 |
error_exit_2: |
|
497 |
error_exit_1: |
|
498 |
nde->fd = -1; /* set the node as free... */ |
|
499 |
error_exit_0: |
|
500 |
return -1; |
|
501 |
} |
|
502 |
||
503 |
||
504 |
||
505 |
static int nd_entry_free(nd_entry_t *nde) {
|
|
506 |
if (nde->fd < 0) |
|
507 |
/* already free */ |
|
508 |
return -1; |
|
509 |
||
510 |
/* reset the tty device old settings... */ |
|
511 |
#ifdef ERRMSG |
|
512 |
int res = |
|
513 |
#endif |
|
514 |
tcsetattr(nde->fd, TCSANOW, &nde->old_tty_settings_); |
|
515 |
#ifdef ERRMSG |
|
516 |
if(res < 0) |
|
517 |
fprintf(stderr, ERRMSG_HEAD "Error reconfiguring serial port to it's original settings.\n"); |
|
518 |
#endif |
|
519 |
||
520 |
recv_buf_done(&nde->recv_buf_); |
|
521 |
close(nde->fd); |
|
522 |
nde->fd = -1; |
|
523 |
||
524 |
return 0; |
|
525 |
} |
|
526 |
||
527 |
||
528 |
||
529 |
||
530 |
static inline int nd_entry_is_free(nd_entry_t *nde) {
|
|
531 |
return (nde->fd < 0); |
|
532 |
} |
|
533 |
||
534 |
||
535 |
||
536 |
||
537 |
/************************************/ |
|
538 |
/** **/ |
|
539 |
/** A data structure - nd table **/ |
|
540 |
/** **/ |
|
541 |
/************************************/ |
|
542 |
||
543 |
typedef struct {
|
|
544 |
/* the array of node descriptors, and current size... */ |
|
545 |
nd_entry_t *node; |
|
546 |
int node_count; /* total number of nodes in the node[] array */ |
|
547 |
} nd_table_t; |
|
548 |
||
549 |
||
550 |
#if 1 |
|
551 |
/* nd_table_init() |
|
552 |
* Version 1 of the nd_table_init() function. |
|
553 |
* If called more than once, 2nd and any subsequent calls will |
|
554 |
* be interpreted as a request to confirm that it was already correctly |
|
555 |
* initialized with the requested number of nodes. |
|
556 |
*/ |
|
557 |
static int nd_table_init(nd_table_t *ndt, int nd_count) {
|
|
558 |
int count; |
|
559 |
||
560 |
if (ndt->node != NULL) {
|
|
561 |
/* this function has already been called, and the node table is already initialised */ |
|
562 |
return (ndt->node_count == nd_count)?0:-1; |
|
563 |
} |
|
564 |
||
565 |
/* initialise the node descriptor metadata array... */ |
|
566 |
ndt->node = malloc(sizeof(nd_entry_t) * nd_count); |
|
567 |
if (ndt->node == NULL) {
|
|
568 |
#ifdef ERRMSG |
|
569 |
fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing node address buffer\n"); |
|
570 |
#endif |
|
571 |
return -1; |
|
572 |
} |
|
573 |
ndt->node_count = nd_count; |
|
574 |
||
575 |
/* initialise the state of each node in the array... */ |
|
576 |
for (count = 0; count < ndt->node_count; count++) {
|
|
577 |
nd_entry_init(&ndt->node[count]); |
|
578 |
} /* for() */ |
|
579 |
||
580 |
return nd_count; /* number of succesfully created nodes! */ |
|
581 |
} |
|
582 |
#else |
|
583 |
/* nd_table_init() |
|
584 |
* Version 2 of the nd_table_init() function. |
|
585 |
* If called more than once, 2nd and any subsequent calls will |
|
586 |
* be interpreted as a request to reserve an extra new_nd_count |
|
587 |
* number of nodes. This will be done using realloc(). |
|
588 |
*/ |
|
589 |
static int nd_table_init(nd_table_t *ndt, int new_nd_count) {
|
|
590 |
int count; |
|
591 |
||
592 |
/* initialise the node descriptor metadata array... */ |
|
593 |
ndt->node = realloc(ndt->node, sizeof(nd_entry_t) * (ndt->node_count + new_nd_count)); |
|
594 |
if (ndt->node == NULL) {
|
|
595 |
#ifdef ERRMSG |
|
596 |
fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing node address buffer\n"); |
|
597 |
#endif |
|
598 |
return -1; |
|
599 |
} |
|
600 |
||
601 |
/* initialise the state of each newly added node in the array... */ |
|
602 |
for (count = ndt->node_count; count < ndt->node_count + new_nd_count; count++) {
|
|
603 |
nd_entry_init(&ndt->node[count]); |
|
604 |
} /* for() */ |
|
605 |
ndt->node_count += new_nd_count; |
|
606 |
||
607 |
return new_nd_count; /* number of succesfully created nodes! */ |
|
608 |
} |
|
609 |
#endif |
|
610 |
||
611 |
||
612 |
static inline nd_entry_t *nd_table_get_nd(nd_table_t *ndt, int nd) {
|
|
613 |
if ((nd < 0) || (nd >= ndt->node_count)) |
|
614 |
return NULL; |
|
615 |
||
616 |
return &ndt->node[nd]; |
|
617 |
} |
|
618 |
||
619 |
||
620 |
static inline void nd_table_done(nd_table_t *ndt) {
|
|
621 |
int i; |
|
622 |
||
623 |
if (ndt->node == NULL) |
|
624 |
return; |
|
625 |
||
626 |
/* close all the connections... */ |
|
627 |
for (i = 0; i < ndt->node_count; i++) |
|
628 |
nd_entry_free(&ndt->node[i]); |
|
629 |
||
630 |
/* Free memory... */ |
|
631 |
free(ndt->node); |
|
632 |
*ndt = (nd_table_t){.node=NULL, .node_count=0};
|
|
633 |
} |
|
634 |
||
635 |
||
636 |
||
637 |
static inline int nd_table_get_free_nd(nd_table_t *ndt) {
|
|
638 |
int count; |
|
639 |
||
640 |
for (count = 0; count < ndt->node_count; count++) {
|
|
641 |
if (nd_entry_is_free(&ndt->node[count])) |
|
642 |
return count; |
|
643 |
} |
|
644 |
||
645 |
/* none found... */ |
|
646 |
return -1; |
|
647 |
} |
|
648 |
||
649 |
||
650 |
static inline int nd_table_free_nd(nd_table_t *ndt, int nd) {
|
|
651 |
if ((nd < 0) || (nd >= ndt->node_count)) |
|
652 |
return -1; |
|
653 |
||
654 |
return nd_entry_free(&ndt->node[nd]); |
|
655 |
} |
|
656 |
||
657 |
||
658 |
||
659 |
/**************************************************************/ |
|
660 |
/**************************************************************/ |
|
661 |
/**** ****/ |
|
662 |
/**** ****/ |
|
663 |
/**** Global Library State ****/ |
|
664 |
/**** ****/ |
|
665 |
/**** ****/ |
|
666 |
/**************************************************************/ |
|
667 |
/**************************************************************/ |
|
668 |
||
669 |
/* The node descriptor table... */ |
|
670 |
/* NOTE: This variable must be correctly initialised here!! */ |
|
671 |
static nd_table_t nd_table_ = {.node=NULL, .node_count=0};
|
|
672 |
||
673 |
/* The optimization choice... */ |
|
674 |
static optimization_t optimization_; |
|
675 |
||
676 |
/* the crc function currently in use... */ |
|
677 |
/* This will depend on the optimisation choice... */ |
|
678 |
crc_func_t crc_calc = DEF_CRC_FUNCTION; |
|
679 |
||
680 |
||
681 |
||
682 |
/**************************************************************/ |
|
683 |
/**************************************************************/ |
|
684 |
/**** ****/ |
|
685 |
/**** ****/ |
|
686 |
/**** CRC functions ****/ |
|
687 |
/**** ****/ |
|
688 |
/**** ****/ |
|
689 |
/**************************************************************/ |
|
690 |
/**************************************************************/ |
|
691 |
||
692 |
#if RTU_FRAME_CRC_LENGTH < 2 |
|
693 |
#error The CRC on modbus RTU frames requires at least 2 bytes in the frame length. |
|
694 |
#endif |
|
695 |
||
696 |
||
697 |
/************************************/ |
|
698 |
/** **/ |
|
699 |
/** Read the CRC of a frame **/ |
|
700 |
/** **/ |
|
701 |
/************************************/ |
|
702 |
||
703 |
/* NOTE: cnt is number of bytes in the frame _excluding_ CRC! */ |
|
704 |
static inline u16 crc_read(u8 *buf, int cnt) {
|
|
705 |
/* For some strange reason, the crc is transmited |
|
706 |
* LSB first, unlike all other values... |
|
707 |
*/ |
|
708 |
return (buf[cnt + 1] << 8) | buf[cnt]; |
|
709 |
} |
|
710 |
||
711 |
||
712 |
/************************************/ |
|
713 |
/** **/ |
|
714 |
/** Write the CRC of a frame **/ |
|
715 |
/** **/ |
|
716 |
/************************************/ |
|
717 |
||
718 |
/* NOTE: cnt is number of bytes in the frame _excluding_ CRC! */ |
|
719 |
static inline void crc_write(u8 *buf, int cnt) {
|
|
720 |
/* For some strange reason, the crc is transmited |
|
721 |
* LSB first, unlike all other values... |
|
722 |
* |
|
723 |
* u16_v(query[string_length]) = mb_hton(temp_crc); -> This is wrong !! |
|
724 |
*/ |
|
725 |
/* NOTE: We have already checked above that RTU_FRAME_CRC_LENGTH is >= 2 */ |
|
726 |
u16 crc = crc_calc(buf, cnt); |
|
727 |
buf[cnt] = lsb(crc); |
|
728 |
buf[cnt+1] = msb(crc); |
|
729 |
} |
|
730 |
||
731 |
||
732 |
||
733 |
/************************************/ |
|
734 |
/** **/ |
|
735 |
/** A slow version of the **/ |
|
736 |
/** CRC function **/ |
|
737 |
/** **/ |
|
738 |
/************************************/ |
|
739 |
||
740 |
/* crc optimized for smallest memory footprint */ |
|
741 |
static u16 crc_slow(u8 *buf, int cnt) |
|
742 |
{
|
|
743 |
int bit; |
|
744 |
u16 temp,flag; |
|
745 |
||
746 |
temp=0xFFFF; |
|
747 |
||
748 |
while (cnt-- != 0) {
|
|
749 |
temp=temp ^ *buf++; |
|
750 |
for (bit=1; bit<=8; bit++) {
|
|
751 |
flag = temp & 0x0001; |
|
752 |
/* NOTE: |
|
753 |
* - since temp is unsigned, we are guaranteed a zero in MSbit; |
|
754 |
* - if it were signed, the value placed in the MSbit would be |
|
755 |
* compiler dependent! |
|
756 |
*/ |
|
757 |
temp >>= 1; |
|
758 |
if (flag) |
|
759 |
temp=temp ^ 0xA001; |
|
760 |
} |
|
761 |
} |
|
762 |
return(temp); |
|
763 |
} |
|
764 |
||
765 |
||
766 |
||
767 |
||
768 |
/************************************/ |
|
769 |
/** **/ |
|
770 |
/** A fast version of the **/ |
|
771 |
/** CRC function **/ |
|
772 |
/** **/ |
|
773 |
/************************************/ |
|
774 |
static u8 *crc_fast_buf = NULL; |
|
775 |
||
776 |
/* crc optimized for speed */ |
|
777 |
static u16 crc_fast(u8 *buf, int cnt) |
|
778 |
{
|
|
779 |
/* NOTE: The following arrays have been replaced by an equivalent |
|
780 |
* array (crc_fast_buf[]) initialised at run-time. |
|
781 |
*/ |
|
782 |
/* |
|
783 |
static u8 buf_lsb[] = {0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
|
|
784 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
785 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
786 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
787 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
788 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
789 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
790 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
791 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
792 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
793 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
794 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
795 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
796 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
797 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
798 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
799 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
800 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
801 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
802 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
803 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
804 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
805 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
806 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
807 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
808 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
809 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
810 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
811 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, |
|
812 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
813 |
0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, |
|
814 |
0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40 |
|
815 |
}; |
|
816 |
||
817 |
static u8 buf_msb[] = {0x00, 0xc0, 0xc1, 0x01, 0xc3, 0x03, 0x02, 0xc2,
|
|
818 |
0xc6, 0x06, 0x07, 0xc7, 0x05, 0xc5, 0xc4, 0x04, |
|
819 |
0xcc, 0x0c, 0x0d, 0xcd, 0x0f, 0xcf, 0xce, 0x0e, |
|
820 |
0x0a, 0xca, 0xcb, 0x0b, 0xc9, 0x09, 0x08, 0xc8, |
|
821 |
0xd8, 0x18, 0x19, 0xd9, 0x1b, 0xdb, 0xda, 0x1a, |
|
822 |
0x1e, 0xde, 0xdf, 0x1f, 0xdd, 0x1d, 0x1c, 0xdc, |
|
823 |
0x14, 0xd4, 0xd5, 0x15, 0xd7, 0x17, 0x16, 0xd6, |
|
824 |
0xd2, 0x12, 0x13, 0xd3, 0x11, 0xd1, 0xd0, 0x10, |
|
825 |
0xf0, 0x30, 0x31, 0xf1, 0x33, 0xf3, 0xf2, 0x32, |
|
826 |
0x36, 0xf6, 0xf7, 0x37, 0xf5, 0x35, 0x34, 0xf4, |
|
827 |
0x3c, 0xfc, 0xfd, 0x3d, 0xff, 0x3f, 0x3e, 0xfe, |
|
828 |
0xfa, 0x3a, 0x3b, 0xfb, 0x39, 0xf9, 0xf8, 0x38, |
|
829 |
0x28, 0xe8, 0xe9, 0x29, 0xeb, 0x2b, 0x2a, 0xea, |
|
830 |
0xee, 0x2e, 0x2f, 0xef, 0x2d, 0xed, 0xec, 0x2c, |
|
831 |
0xe4, 0x24, 0x25, 0xe5, 0x27, 0xe7, 0xe6, 0x26, |
|
832 |
0x22, 0xe2, 0xe3, 0x23, 0xe1, 0x21, 0x20, 0xe0, |
|
833 |
0xa0, 0x60, 0x61, 0xa1, 0x63, 0xa3, 0xa2, 0x62, |
|
834 |
0x66, 0xa6, 0xa7, 0x67, 0xa5, 0x65, 0x64, 0xa4, |
|
835 |
0x6c, 0xac, 0xad, 0x6d, 0xaf, 0x6f, 0x6e, 0xae, |
|
836 |
0xaa, 0x6a, 0x6b, 0xab, 0x69, 0xa9, 0xa8, 0x68, |
|
837 |
0x78, 0xb8, 0xb9, 0x79, 0xbb, 0x7b, 0x7a, 0xba, |
|
838 |
0xbe, 0x7e, 0x7f, 0xbf, 0x7d, 0xbd, 0xbc, 0x7c, |
|
839 |
0xb4, 0x74, 0x75, 0xb5, 0x77, 0xb7, 0xb6, 0x76, |
|
840 |
0x72, 0xb2, 0xb3, 0x73, 0xb1, 0x71, 0x70, 0xb0, |
|
841 |
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, |
|
842 |
0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, |
|
843 |
0x9c, 0x5c, 0x5d, 0x9d, 0x5f, 0x9f, 0x9e, 0x5e, |
|
844 |
0x5a, 0x9a, 0x9b, 0x5b, 0x99, 0x59, 0x58, 0x98, |
|
845 |
0x88, 0x48, 0x49, 0x89, 0x4b, 0x8b, 0x8a, 0x4a, |
|
846 |
0x4e, 0x8e, 0x8f, 0x4f, 0x8d, 0x4d, 0x4c, 0x8c, |
|
847 |
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, |
|
848 |
0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 |
|
849 |
}; |
|
850 |
*/ |
|
851 |
u8 crc_msb = 0xFF; |
|
852 |
u8 crc_lsb = 0xFF; |
|
853 |
int index; |
|
854 |
||
855 |
if (cnt <= 0) {
|
|
856 |
fprintf(stderr, "\nInternal program error in file %s at line %d\n\n\n", __FILE__, __LINE__); |
|
857 |
exit(EXIT_FAILURE); |
|
858 |
} |
|
859 |
||
860 |
while (cnt-- != 0) {
|
|
861 |
index = 2 * (crc_lsb ^ *buf++); |
|
862 |
crc_lsb = crc_msb ^ crc_fast_buf[index]/* buf_lsb[index/2] */; |
|
863 |
crc_msb = crc_fast_buf[index + 1] /* buf_msb[index/2] */; |
|
864 |
} |
|
865 |
||
866 |
return crc_msb*0x0100 + crc_lsb; |
|
867 |
} |
|
868 |
||
869 |
||
870 |
/************************************/ |
|
871 |
/** **/ |
|
872 |
/** init() and done() functions **/ |
|
873 |
/** of fast CRC version **/ |
|
874 |
/** **/ |
|
875 |
/************************************/ |
|
876 |
||
877 |
static inline int crc_fast_init(void) {
|
|
878 |
int i; |
|
879 |
u8 data[2]; |
|
880 |
u16 tmp_crc; |
|
881 |
||
882 |
if ((crc_fast_buf = (u8 *)malloc(256 * 2)) == NULL) |
|
883 |
return -1; |
|
884 |
||
885 |
for (i = 0x00; i < 0x100; i++) {
|
|
886 |
data[0] = 0xFF; |
|
887 |
data[1] = i; |
|
888 |
data[1] = ~data[1]; |
|
889 |
tmp_crc = crc_slow(data, 2); |
|
890 |
crc_fast_buf[2*i ] = lsb(tmp_crc); |
|
891 |
crc_fast_buf[2*i + 1] = msb(tmp_crc); |
|
892 |
} |
|
893 |
||
894 |
return 0; |
|
895 |
} |
|
896 |
||
897 |
||
898 |
static inline void crc_fast_done(void) {
|
|
899 |
free(crc_fast_buf); |
|
900 |
} |
|
901 |
||
902 |
||
903 |
/************************************/ |
|
904 |
/** **/ |
|
905 |
/** init() and done() functions **/ |
|
906 |
/** of generic CRC **/ |
|
907 |
/** **/ |
|
908 |
/************************************/ |
|
909 |
||
910 |
static inline int crc_init(optimization_t opt) {
|
|
911 |
switch (opt) {
|
|
912 |
case optimize_speed: |
|
913 |
if (crc_fast_init() < 0) |
|
914 |
return -1; |
|
915 |
crc_calc = crc_fast; |
|
916 |
return 0; |
|
917 |
case optimize_size : |
|
918 |
crc_calc = crc_slow; |
|
919 |
return 0; |
|
920 |
default: |
|
921 |
return -1; |
|
922 |
} |
|
923 |
||
924 |
/* humour the compiler */ |
|
925 |
return -1; |
|
926 |
} |
|
927 |
||
928 |
||
929 |
static inline int crc_done(void) {
|
|
930 |
if (crc_calc == crc_fast) |
|
931 |
crc_fast_done(); |
|
932 |
||
933 |
crc_calc = DEF_CRC_FUNCTION; |
|
934 |
return 0; |
|
935 |
} |
|
936 |
||
937 |
||
938 |
||
939 |
/**************************************************************/ |
|
940 |
/**************************************************************/ |
|
941 |
/**** ****/ |
|
942 |
/**** ****/ |
|
943 |
/**** Sending of Modbus RTU Frames ****/ |
|
944 |
/**** ****/ |
|
945 |
/**** ****/ |
|
946 |
/**************************************************************/ |
|
947 |
/**************************************************************/ |
|
948 |
||
949 |
/* W A R N I N G |
|
950 |
* ============= |
|
951 |
* The modbus_rtu_write() function assumes that the caller |
|
952 |
* has allocated a few bytes extra for the buffer containing |
|
953 |
* the data. These bytes will be used to write the crc. |
|
954 |
* |
|
955 |
* The caller of this function MUST make sure that the data |
|
956 |
* buffer, although only containing data_length bytes, has |
|
957 |
* been allocated with a size equal to or larger than |
|
958 |
* data_length + RTU_FRAME_CRC_LENGTH bytes |
|
959 |
* |
|
960 |
* I know, this is a very ugly hack, but we don't have much |
|
961 |
* choice (please read other comments further on for more |
|
962 |
* explanations) |
|
963 |
* |
|
964 |
* We will nevertheless try and make this explicit by having the |
|
965 |
* library initialisation function (modbus_rtu_init() ) return a |
|
966 |
* value specifying how many extra bytes this buffer should have. |
|
967 |
* Maybe this way this very ugly hack won't go unnoticed, and we |
|
968 |
* won't be having any segmentation faults...! |
|
969 |
* |
|
970 |
* NOTE: for now the transmit_timeout is silently ignored in RTU version! |
|
971 |
*/ |
|
972 |
int modbus_rtu_write(int nd, |
|
973 |
u8 *data, |
|
974 |
size_t data_length, |
|
975 |
u16 transaction_id, |
|
976 |
const struct timespec *transmit_timeout |
|
977 |
) |
|
978 |
{
|
|
979 |
fd_set rfds; |
|
980 |
struct timeval timeout; |
|
981 |
int res, send_retries; |
|
982 |
nd_entry_t *nd_entry; |
|
983 |
||
984 |
#ifdef DEBUG |
|
985 |
fprintf(stderr, "modbus_rtu_write(fd=%d) called...\n", nd); |
|
986 |
#endif |
|
987 |
/* check if nd is correct... */ |
|
988 |
if ((nd_entry = nd_table_get_nd(&nd_table_, nd)) == NULL) |
|
989 |
return -1; |
|
990 |
||
991 |
/* check if nd is initialzed... */ |
|
992 |
if (nd_entry->fd < 0) |
|
993 |
return -1; |
|
994 |
||
995 |
/************************** |
|
996 |
* append crc to frame... * |
|
997 |
**************************/ |
|
998 |
/* WARNING: |
|
999 |
* The crc_write() function assumes that we have an extra |
|
1000 |
* RTU_FRAME_CRC_LENGTH free bytes at the end of the *data |
|
1001 |
* buffer. |
|
1002 |
* The caller of this function had better make sure he has |
|
1003 |
* allocated those extra bytes, or a segmentation fault will |
|
1004 |
* occur. |
|
1005 |
* Please read on why we leave this as it is... |
|
1006 |
* |
|
1007 |
* REASONS: |
|
1008 |
* We want to write the data and the crc in a single call to |
|
1009 |
* the OS. This is the only way we can minimally try to gurantee |
|
1010 |
* that we will not be introducing a silence of more than 1.5 |
|
1011 |
* character transmission times between any two characters. |
|
1012 |
* |
|
1013 |
* We could do the above using one of two methods: |
|
1014 |
* (a) use a special writev() call in which the data |
|
1015 |
* to be sent is stored in two buffers (one for the |
|
1016 |
* data and the other for the crc). |
|
1017 |
* (b) place all the data in a single linear buffer and |
|
1018 |
* use the normal write() function. |
|
1019 |
* |
|
1020 |
* We cannot use (a) since the writev(2) function does not seem |
|
1021 |
* to be POSIX compliant... |
|
1022 |
* (b) has the drawback that we would need to allocate a new buffer, |
|
1023 |
* and copy all the data into that buffer. We have enough copying of |
|
1024 |
* data between buffers as it is, so we won't be doing it here |
|
1025 |
* yet again! |
|
1026 |
* |
|
1027 |
* The only option that seems left over is to have the caller |
|
1028 |
* of this function allocate a few extra bytes. Let's hope he |
|
1029 |
* does not forget! |
|
1030 |
*/ |
|
1031 |
crc_write(data, data_length); |
|
1032 |
data_length += RTU_FRAME_CRC_LENGTH; |
|
1033 |
||
1034 |
#ifdef DEBUG |
|
1035 |
/* Print the hex value of each character that is about to be |
|
1036 |
* sent over the bus. |
|
1037 |
*/ |
|
1038 |
{ int i;
|
|
1039 |
for(i = 0; i < data_length; i++) |
|
1040 |
fprintf(stderr, "[0x%2X]", data[i]); |
|
1041 |
fprintf(stderr, "\n"); |
|
1042 |
} |
|
1043 |
#endif |
|
1044 |
/* THE MAIN LOOP!!! */ |
|
1045 |
/* NOTE: The modbus standard specifies that the message must |
|
1046 |
* be sent continuosly over the wire with maximum |
|
1047 |
* inter-character delays of 1.5 character intervals. |
|
1048 |
* |
|
1049 |
* If the write() call is interrupted by a signal, then |
|
1050 |
* this delay will most probably be exceeded. We should then |
|
1051 |
* re-start writing the query from the begining. |
|
1052 |
* |
|
1053 |
* BUT, can we really expect the write() call to return |
|
1054 |
* query_length on every platform when no error occurs? |
|
1055 |
* The write call would still be correct if it only wrote |
|
1056 |
* 1 byte at a time! |
|
1057 |
* |
|
1058 |
* To protect ourselves getting into an infinte loop in the |
|
1059 |
* above cases, we specify a maximum number of retries, and |
|
1060 |
* hope for the best...! The worst will now be we simply do |
|
1061 |
* not get to send out a whole frame, and will therefore always |
|
1062 |
* fail on writing a modbus frame! |
|
1063 |
*/ |
|
1064 |
send_retries = RTU_FRAME_SEND_RETRY + 1; /* must try at least once... */ |
|
1065 |
while (send_retries > 0) {
|
|
1066 |
||
1067 |
/******************************* |
|
1068 |
* synchronise with the bus... * |
|
1069 |
*******************************/ |
|
1070 |
/* Remember that a RS485 bus is half-duplex, so we have to wait until |
|
1071 |
* nobody is transmitting over the bus for our turn to transmit. |
|
1072 |
* This will never happen on a modbus network if the master and |
|
1073 |
* slave state machines never get out of synch (granted, it probably |
|
1074 |
* only has two states, but a state machine nonetheless), but we want |
|
1075 |
* to make sure we can re-synchronise if they ever do get out of synch. |
|
1076 |
* |
|
1077 |
* The following lines will guarantee that we will re-synchronise our |
|
1078 |
* state machine with the current state of the bus. |
|
1079 |
* |
|
1080 |
* We first wait until the bus has been silent for at least |
|
1081 |
* char_interval_timeout (i.e. 3.5 character interval). We then flush |
|
1082 |
* any input and output that might be on the cache. |
|
1083 |
*/ |
|
1084 |
/* NOTES: |
|
1085 |
* - we do not need to reset the rfds with FD_SET(ttyfd, &rfds) |
|
1086 |
* before every call to select! We only wait on one file descriptor, |
|
1087 |
* so if select returns succesfully, it must have that same file |
|
1088 |
* decriptor set in the rdfs! |
|
1089 |
* If select returns with a timeout, then we do not get to call |
|
1090 |
* select again! |
|
1091 |
* - On Linux, timeout (i.e. timeout) is modified by select() to |
|
1092 |
* reflect the amount of time not slept; most other implementations |
|
1093 |
* do not do this. In the cases in which timeout is not modified, |
|
1094 |
* we will simply have to wait for longer periods if select is |
|
1095 |
* interrupted by a signal. |
|
1096 |
*/ |
|
1097 |
FD_ZERO(&rfds); |
|
1098 |
FD_SET(nd_entry->fd, &rfds); |
|
1099 |
timeout = nd_entry->time_35_char_; |
|
1100 |
while ((res = select(nd_entry->fd+1, &rfds, NULL, NULL, &timeout)) != 0) {
|
|
1101 |
if (res > 0) {
|
|
1102 |
/* we are receiving data over the serial port! */ |
|
1103 |
/* Throw the data away! */ |
|
1104 |
tcflush(nd_entry->fd, TCIFLUSH); /* flush the input stream */ |
|
1105 |
/* reset the timeout value! */ |
|
1106 |
timeout = nd_entry->time_35_char_; |
|
1107 |
/* We do not need to reset the FD SET here! */ |
|
1108 |
} else {
|
|
1109 |
/* some kind of error ocurred */ |
|
1110 |
if (errno != EINTR) |
|
1111 |
/* we were not interrupted by a signal */ |
|
1112 |
return -1; |
|
1113 |
/* We will be calling select() again. |
|
1114 |
* We need to reset the FD SET ! |
|
1115 |
*/ |
|
1116 |
FD_ZERO(&rfds); |
|
1117 |
FD_SET(nd_entry->fd, &rfds); |
|
1118 |
} |
|
1119 |
} /* while (select()) */ |
|
1120 |
||
1121 |
/* Flush both input and output streams... */ |
|
1122 |
/* NOTE: Due to the nature of the modbus protocol, |
|
1123 |
* when a frame is sent all previous |
|
1124 |
* frames that may have arrived at the sending node become |
|
1125 |
* irrelevant. |
|
1126 |
*/ |
|
1127 |
tcflush(nd_entry->fd, TCIOFLUSH); /* flush the input & output streams */ |
|
1128 |
recv_buf_reset(&nd_entry->recv_buf_); /* reset the recv buffer */ |
|
1129 |
||
1130 |
/********************** |
|
1131 |
* write to output... * |
|
1132 |
**********************/ |
|
1133 |
/* Please see the comment just above the main loop!! */ |
|
1134 |
if ((res = write(nd_entry->fd, data, data_length)) != data_length) {
|
|
1135 |
if ((res < 0) && (errno != EAGAIN ) && (errno != EINTR )) |
|
1136 |
return -1; |
|
1137 |
} else {
|
|
1138 |
/* query succesfully sent! */ |
|
1139 |
/* res == query_length */ |
|
1140 |
||
1141 |
/* NOTE: We do not flush the input stream after sending the frame! |
|
1142 |
* If the process gets swapped out between the end of writing |
|
1143 |
* to the serial port, and the call to flush the input of the |
|
1144 |
* same serial port, the response to the modbus query may be |
|
1145 |
* sent over between those two calls. This would result in the |
|
1146 |
* tcflush(ttyfd, TCIFLUSH) call flushing out the response |
|
1147 |
* to the query we have just sent! |
|
1148 |
* Not a good thing at all... ;-) |
|
1149 |
*/ |
|
1150 |
return data_length - RTU_FRAME_CRC_LENGTH; |
|
1151 |
} |
|
1152 |
/* NOTE: The maximum inter-character delay of 1.5 character times |
|
1153 |
* has most probably been exceeded, so we abort the frame and |
|
1154 |
* retry again... |
|
1155 |
*/ |
|
1156 |
send_retries--; |
|
1157 |
} /* while() MAIN LOOP */ |
|
1158 |
||
1159 |
/* maximum retries exceeded */ |
|
1160 |
return -1; |
|
1161 |
} |
|
1162 |
||
1163 |
||
1164 |
||
1165 |
/**************************************************************/ |
|
1166 |
/**************************************************************/ |
|
1167 |
/**** ****/ |
|
1168 |
/**** ****/ |
|
1169 |
/**** Receiving Modbus RTU Frames ****/ |
|
1170 |
/**** ****/ |
|
1171 |
/**** ****/ |
|
1172 |
/**************************************************************/ |
|
1173 |
/**************************************************************/ |
|
1174 |
||
1175 |
#if MIN_FRAME_LENGTH < 2 |
|
1176 |
#error Modbus RTU frames have a minimum length larger than MIN_FRAME_LENGTH. |
|
1177 |
#endif |
|
1178 |
||
1179 |
/************************************/ |
|
1180 |
/** **/ |
|
1181 |
/** Guess length of frame **/ |
|
1182 |
/** being read. **/ |
|
1183 |
/** **/ |
|
1184 |
/************************************/ |
|
1185 |
||
1186 |
/* Auxiliary function to the search_for_frame() function. |
|
1187 |
* |
|
1188 |
* NOTE: data_byte_count must be >=2 for correct operation, therefore |
|
1189 |
* the #error condition above. |
|
1190 |
* |
|
1191 |
* Function to determine the length of the frame currently being read, |
|
1192 |
* assuming it is a query/response frame. |
|
1193 |
* |
|
1194 |
* The guess is obtained by analysing the bytes that have already been |
|
1195 |
* read. Sometimes we cannot be sure what is the frame length, because |
|
1196 |
* not enough bytes of the frame have been read yet (for example, frames |
|
1197 |
* that have a byte_count value which has not yet been read). In these |
|
1198 |
* cases we return not the frame length, but an error (-1). |
|
1199 |
* |
|
1200 |
* If we find the data does not make any sense (i.e. it cannot be a valid |
|
1201 |
* modbus frame), we return -1. |
|
1202 |
*/ |
|
1203 |
static int frame_length(u8 *frame_data, |
|
1204 |
int frame_data_length, |
|
1205 |
/* The array containing the lengths of frames. */ |
|
1206 |
/* - query_frame_length[] |
|
1207 |
* - response_frame_length[] |
|
1208 |
*/ |
|
1209 |
i8 *frame_length_array) {
|
|
1210 |
||
1211 |
u8 function_code; |
|
1212 |
int res; |
|
1213 |
||
1214 |
/* check consistency of input parameters... */ |
|
1215 |
/* |
|
1216 |
if ((frame_data == NULL) || (frame_length_array == NULL) || (frame_data_length < 2)) |
|
1217 |
return -1; |
|
1218 |
*/ |
|
1219 |
||
1220 |
function_code = frame_data[L2_FRAME_FUNCTION_OFS]; |
|
1221 |
||
1222 |
/* hard code the length of response to diagnostic function 8 (0x08), with |
|
1223 |
* subfunction 21 (0x15), and sub-sub-function (a.k.a. operation) 3 (0x03), |
|
1224 |
* which contains a byte count... |
|
1225 |
*/ |
|
1226 |
if ((function_code == 0x08) && (frame_length_array == response_frame_lengths)) {
|
|
1227 |
if (frame_data_length < 4) {
|
|
1228 |
/* not enough info to determine the sub-function... */ |
|
1229 |
return -1; |
|
1230 |
} else {
|
|
1231 |
if ((frame_data[2] == 0x00) && (frame_data[3] == 0x15)) {
|
|
1232 |
/* we need a couple more bytes to figure out the sub-sub-function... */ |
|
1233 |
if (frame_data_length < 6) {
|
|
1234 |
/* not enough info to determine the sub-sub-function... */ |
|
1235 |
return -1; |
|
1236 |
} else {
|
|
1237 |
if ((frame_data[4] == 0x00) && (frame_data[5] == 0x03)) {
|
|
1238 |
/* We have found a response frame to diagnostic sub-function ... */ |
|
1239 |
if (frame_data_length < 8) {
|
|
1240 |
/* not enough info to determine the frame length */ |
|
1241 |
return -1; |
|
1242 |
} else {
|
|
1243 |
return /*HEADER*/ 6 + mb_ntoh(u16_v(frame_data[6])) + RTU_FRAME_CRC_LENGTH; |
|
1244 |
} |
|
1245 |
} |
|
1246 |
} |
|
1247 |
} |
|
1248 |
} |
|
1249 |
} |
|
1250 |
||
1251 |
res = frame_length_array[function_code]; |
|
1252 |
||
1253 |
switch(res) {
|
|
1254 |
case BYTE_COUNT_3 : |
|
1255 |
if (frame_data_length >= 3) |
|
1256 |
return BYTE_COUNT_3_HEADER + frame_data[2] + RTU_FRAME_CRC_LENGTH; |
|
1257 |
break; |
|
1258 |
case BYTE_COUNT_34: |
|
1259 |
if (frame_data_length >= 4) |
|
1260 |
return BYTE_COUNT_34_HEADER + mb_ntoh(u16_v(frame_data[2])) + RTU_FRAME_CRC_LENGTH; |
|
1261 |
break; |
|
1262 |
case BYTE_COUNT_7 : |
|
1263 |
if (frame_data_length >= 7) |
|
1264 |
return BYTE_COUNT_7_HEADER + frame_data[6] + RTU_FRAME_CRC_LENGTH; |
|
1265 |
break; |
|
1266 |
case BYTE_COUNT_11: |
|
1267 |
if (frame_data_length >= 11) |
|
1268 |
return BYTE_COUNT_11_HEADER + frame_data[10] + RTU_FRAME_CRC_LENGTH; |
|
1269 |
break; |
|
1270 |
case BYTE_COUNT_U : |
|
1271 |
return -1; |
|
1272 |
default: |
|
1273 |
return res + RTU_FRAME_CRC_LENGTH; |
|
1274 |
} /* switch() */ |
|
1275 |
||
1276 |
/* unknown frame length */ |
|
1277 |
return -1; |
|
1278 |
} |
|
1279 |
||
1280 |
||
1281 |
||
1282 |
/************************************/ |
|
1283 |
/** **/ |
|
1284 |
/** Search for a frame **/ |
|
1285 |
/** **/ |
|
1286 |
/************************************/ |
|
1287 |
||
1288 |
/* Search for a valid frame in the current data. |
|
1289 |
* If no valid frame is found, then we return -1. |
|
1290 |
* |
|
1291 |
* NOTE: Since frame verification is done by calculating the CRC, which is rather |
|
1292 |
* CPU intensive, and this function may be called several times with the same, |
|
1293 |
* data, we keep state regarding the result of previous invocations... |
|
1294 |
* That is the reason for the *search_history parameter! |
|
1295 |
*/ |
|
1296 |
static int search_for_frame(u8 *frame_data, |
|
1297 |
int frame_data_length, |
|
1298 |
int *search_history) {
|
|
1299 |
int query_length, resp_length; |
|
1300 |
u8 function_code; |
|
1301 |
/* *search_history flag will have or'ed of following values... */ |
|
1302 |
#define SFF_HIST_NO_QUERY_FRAME 0x01 |
|
1303 |
#define SFF_HIST_NO_RESPONSE_FRAME 0x02 |
|
1304 |
#define SFF_HIST_NO_FRAME (SFF_HIST_NO_RESPONSE_FRAME + SFF_HIST_NO_QUERY_FRAME) |
|
1305 |
||
1306 |
if ((*search_history == SFF_HIST_NO_FRAME) || |
|
1307 |
(frame_data_length < MIN_FRAME_LENGTH) || |
|
1308 |
(frame_data_length > MAX_RTU_FRAME_LENGTH)) |
|
1309 |
return -1; |
|
1310 |
||
1311 |
function_code = frame_data[L2_FRAME_FUNCTION_OFS]; |
|
1312 |
||
1313 |
/* check for exception frame... */ |
|
1314 |
if ((function_code && 0x80) == 0x80) {
|
|
1315 |
if (frame_data_length >= EXCEPTION_FRAME_LENGTH + RTU_FRAME_CRC_LENGTH) {
|
|
1316 |
/* let's check CRC for valid frame. */ |
|
1317 |
if ( crc_calc(frame_data, EXCEPTION_FRAME_LENGTH) |
|
1318 |
== crc_read(frame_data, EXCEPTION_FRAME_LENGTH)) |
|
1319 |
return EXCEPTION_FRAME_LENGTH + RTU_FRAME_CRC_LENGTH; |
|
1320 |
else |
|
1321 |
/* We have checked the CRC, and it is not a valid frame! */ |
|
1322 |
*search_history |= SFF_HIST_NO_FRAME; |
|
1323 |
} |
|
1324 |
return -1; |
|
1325 |
} |
|
1326 |
||
1327 |
/* check for valid function code */ |
|
1328 |
if ((function_code > MAX_FUNCTION_CODE) || (function_code < 1)) {
|
|
1329 |
/* This is an invalid frame!!! */ |
|
1330 |
*search_history |= SFF_HIST_NO_FRAME; |
|
1331 |
return -1; |
|
1332 |
} |
|
1333 |
||
1334 |
/* let's guess the frame length */ |
|
1335 |
query_length = resp_length = -1; |
|
1336 |
if ((*search_history & SFF_HIST_NO_QUERY_FRAME) == 0) |
|
1337 |
query_length = frame_length(frame_data, frame_data_length, query_frame_lengths); |
|
1338 |
if ((*search_history & SFF_HIST_NO_RESPONSE_FRAME) == 0) |
|
1339 |
resp_length = frame_length(frame_data, frame_data_length, response_frame_lengths); |
|
1340 |
||
1341 |
/* let's check whether any of the lengths are valid...*/ |
|
1342 |
/* If any of the guesses coincides with the available data length |
|
1343 |
* we check that length first... |
|
1344 |
*/ |
|
1345 |
if ((frame_data_length == query_length) || (frame_data_length == resp_length)) {
|
|
1346 |
if ( crc_calc(frame_data, frame_data_length - RTU_FRAME_CRC_LENGTH) |
|
1347 |
== crc_read(frame_data, frame_data_length - RTU_FRAME_CRC_LENGTH)) |
|
1348 |
return frame_data_length; |
|
1349 |
/* nope, wrong guess...*/ |
|
1350 |
if (frame_data_length == query_length) |
|
1351 |
*search_history |= SFF_HIST_NO_QUERY_FRAME; |
|
1352 |
if (frame_data_length == resp_length) |
|
1353 |
*search_history |= SFF_HIST_NO_RESPONSE_FRAME; |
|
1354 |
} |
|
1355 |
||
1356 |
/* let's shoot for a query frame */ |
|
1357 |
if ((*search_history & SFF_HIST_NO_QUERY_FRAME) == 0) {
|
|
1358 |
if (query_length >= 0) {
|
|
1359 |
if (frame_data_length >= query_length) {
|
|
1360 |
/* let's check if we have a valid frame */ |
|
1361 |
if ( crc_calc(frame_data, query_length - RTU_FRAME_CRC_LENGTH) |
|
1362 |
== crc_read(frame_data, query_length - RTU_FRAME_CRC_LENGTH)) |
|
1363 |
return query_length; |
|
1364 |
else |
|
1365 |
/* We have checked the CRC, and it is not a valid frame! */ |
|
1366 |
*search_history |= SFF_HIST_NO_QUERY_FRAME; |
|
1367 |
} |
|
1368 |
} |
|
1369 |
} |
|
1370 |
||
1371 |
/* let's shoot for a response frame */ |
|
1372 |
if ((*search_history & SFF_HIST_NO_RESPONSE_FRAME) == 0) {
|
|
1373 |
if (resp_length >= 0) {
|
|
1374 |
if (frame_data_length >= resp_length) {
|
|
1375 |
/* let's check if we have a valid frame */ |
|
1376 |
if ( crc_calc(frame_data, resp_length - RTU_FRAME_CRC_LENGTH) |
|
1377 |
== crc_read(frame_data, resp_length - RTU_FRAME_CRC_LENGTH)) |
|
1378 |
return resp_length; |
|
1379 |
else |
|
1380 |
*search_history |= SFF_HIST_NO_RESPONSE_FRAME; |
|
1381 |
} |
|
1382 |
} |
|
1383 |
} |
|
1384 |
||
1385 |
/* Could not find valid frame... */ |
|
1386 |
return -1; |
|
1387 |
} |
|
1388 |
||
1389 |
||
1390 |
||
1391 |
/************************************/ |
|
1392 |
/** **/ |
|
1393 |
/** Read a frame **/ |
|
1394 |
/** **/ |
|
1395 |
/************************************/ |
|
1396 |
||
1397 |
/* A small auxiliary function, just to make the code easier to read... */ |
|
1398 |
static inline void next_frame_offset(recv_buf_t *buf, u8 *slave_id) {
|
|
1399 |
buf->frame_search_history = 0; |
|
1400 |
lb_data_purge(&(buf->data_buf), 1 /* skip one byte */); |
|
1401 |
||
1402 |
if (slave_id == NULL) |
|
1403 |
return; |
|
1404 |
||
1405 |
/* keep ignoring bytes, until we find one == *slave_id, |
|
1406 |
* or no more bytes... |
|
1407 |
*/ |
|
1408 |
while (lb_data_count(&(buf->data_buf)) != 0) {
|
|
1409 |
if (*lb_data(&(buf->data_buf)) == *slave_id) |
|
1410 |
return; |
|
1411 |
lb_data_purge(&(buf->data_buf), 1 /* skip one byte */); |
|
1412 |
} |
|
1413 |
} |
|
1414 |
||
1415 |
/* A small auxiliary function, just to make the code easier to read... */ |
|
1416 |
static inline int return_frame(recv_buf_t *buf, |
|
1417 |
int frame_length, |
|
1418 |
u8 **recv_data_ptr) {
|
|
1419 |
#ifdef DEBUG |
|
1420 |
fprintf(stderr, "\n" ); |
|
1421 |
fprintf(stderr, "returning valid frame of %d bytes.\n", frame_length); |
|
1422 |
#endif |
|
1423 |
/* set the data pointer */ |
|
1424 |
*recv_data_ptr = lb_data(&(buf->data_buf)); |
|
|
15
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1425 |
/* Mark the frame bytes to be deleted off the buffer by the next call to lb_data_purge() */ |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1426 |
/* Notice that we cannot delete the frame bytes right away, as they will still be accessed by whoever |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1427 |
* called read_frame(). We can only delete this data on the next call to read_frame() |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1428 |
*/ |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1429 |
lb_data_mark_for_purge(&(buf->data_buf), frame_length); |
| 0 | 1430 |
/* reset the search_history flag */ |
1431 |
buf->frame_search_history = 0; |
|
1432 |
/* if the buffer becomes empty, then reset boundary flag */ |
|
1433 |
if (lb_data_count(&(buf->data_buf)) <= 0) |
|
1434 |
buf->found_frame_boundary = 0; |
|
1435 |
/* return the frame length, excluding CRC */ |
|
1436 |
return frame_length - RTU_FRAME_CRC_LENGTH; |
|
1437 |
} |
|
1438 |
||
1439 |
/* A function to read a valid frame off the rtu bus. |
|
1440 |
* |
|
1441 |
* NOTES: |
|
1442 |
* - The returned frame is guaranteed to be a valid frame. |
|
1443 |
* - The returned length does *not* include the CRC. |
|
1444 |
* - The returned frame is not guaranteed to have the same |
|
1445 |
* slave id as that stored in (*slave_id). This value is used |
|
1446 |
* merely in optimizing the search for wanted valid frames |
|
1447 |
* after reading an aborted frame. Only in this situation do |
|
1448 |
* we limit our search for frames with a slvae id == (*slave_id). |
|
1449 |
* Under normal circumstances, the value in (*slave_id) is |
|
1450 |
* simply ignored... |
|
1451 |
* If any valid frame is desired, then slave_id should be NULL. |
|
1452 |
* |
|
1453 |
*/ |
|
1454 |
||
1455 |
/* NOTE: We cannot relly on the 3.5 character interval between frames to detect |
|
1456 |
* end of frame. We are reading the bytes from a user process, so in |
|
1457 |
* essence the bytes we are reading are coming off a cache. |
|
1458 |
* Any inter-character delays between the arrival of the bytes are |
|
1459 |
* lost as soon as they were placed in the cache. |
|
1460 |
* |
|
1461 |
* Our only recourse is to analyse the frame we are reading in real-time, |
|
1462 |
* and check if it is a valid frame by checking it's CRC. |
|
1463 |
* To optimise this, we must be able to figure out the length |
|
1464 |
* of the frame currently being received by analysing the first bytes |
|
1465 |
* of that frame. Unfortunately, we have three problems with this: |
|
1466 |
* 1) The spec does not specify the format of every possible modbus |
|
1467 |
* frame. For ex.functions 9, 10, 13, 14, 18 and 19(?). |
|
1468 |
* 2) It is not possible to figure out whether a frame is a query |
|
1469 |
* or a response by just analysing the frame, and query and response |
|
1470 |
* frames have different sizes... |
|
1471 |
* 3) A frame may be aborted in the middle! We have no easy way of telling |
|
1472 |
* if what we are reading is a partial (aborted) frame, followed by a |
|
1473 |
* correct frame. |
|
1474 |
* Possible solutions to: |
|
1475 |
* 1) We could try to reverse engineer, but at the moment I have no |
|
1476 |
* PLCs that will generate the required frames. |
|
1477 |
* The chosen method is to verify the CRC if we are lucky enough to |
|
1478 |
* detect the 3.5 frame boundary imediately following one of these |
|
1479 |
* frames of unknown length. |
|
1480 |
* If we do not detect any frame boundary, then our only option |
|
1481 |
* is to consider it an aborted frame. |
|
1482 |
* 2) We aim for the query frame (usually the shortest), and check |
|
1483 |
* it's CRC. If it matches, we accept, the frame, otherwise we try |
|
1484 |
* a response frame. |
|
1485 |
* 3) The only way is to consider a frame boundary after each byte, |
|
1486 |
* (i.e. ignore one bye at a time) and verify if the following bytes |
|
1487 |
* constitue a valid frame (by checking the CRC). |
|
1488 |
* |
|
1489 |
* When reading an aborted frame followed by two or more valid frames, if |
|
1490 |
* we are unlucky and do not detetect any frame boundary using the 3.5 |
|
1491 |
* character interval, then we will most likely be reading in bytes |
|
1492 |
* beyond the first valid frame. This means we will have to store the extra |
|
1493 |
* bytes we have already read, so they may be handled the next time the |
|
1494 |
* read_frame() function is called. |
|
1495 |
*/ |
|
1496 |
/* |
|
1497 |
* NOTE: The modbus RTU spec is inconsistent on how to handle |
|
1498 |
* inter-character delays larger than 1.5 characters. |
|
1499 |
* - On one paragraph it is stated that any delay larger than |
|
1500 |
* 1.5 character times aborts the current frame, and a new |
|
1501 |
* frame is started. |
|
1502 |
* - On another paragraph it is stated that a frame must begin |
|
1503 |
* with a silence of 3.5 character times. |
|
1504 |
* |
|
1505 |
* We will therefore consider that any delay larger than 1.5 character |
|
1506 |
* times terminates a valid frame. All the above references to the 3.5 character |
|
1507 |
* interval should therefore be read as a 1.5 character interval. |
|
1508 |
*/ |
|
1509 |
/* NOTE: This function is only called from one place in the rest of the code, |
|
1510 |
* so we might just as well make it inline... |
|
1511 |
*/ |
|
1512 |
/* RETURNS: number of bytes in received frame |
|
1513 |
* -1 on read file error |
|
1514 |
* -2 on timeout |
|
1515 |
*/ |
|
1516 |
static inline int read_frame(nd_entry_t *nd_entry, |
|
1517 |
u8 **recv_data_ptr, |
|
1518 |
struct timespec *end_time, |
|
1519 |
u8 *slave_id) |
|
1520 |
{
|
|
1521 |
/* temporary variables... */ |
|
1522 |
fd_set rfds; |
|
1523 |
struct timeval timeout; |
|
1524 |
int res, read_stat; |
|
1525 |
int frame_length; |
|
1526 |
recv_buf_t *recv_buf = &nd_entry->recv_buf_; |
|
1527 |
||
1528 |
/* Flag: |
|
1529 |
* 1 => we are reading in an aborted frame, so we must |
|
1530 |
* start ignoring bytes... |
|
1531 |
*/ |
|
1532 |
int found_aborted_frame; |
|
1533 |
||
1534 |
/* assume error... */ |
|
1535 |
*recv_data_ptr = NULL; |
|
1536 |
||
1537 |
/*===================================* |
|
|
15
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1538 |
* Delete any previously received data that has already been returned as a valid frame in * |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1539 |
*===================================*/ |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1540 |
/* Delete any previously received data that has already been returned as a valid frame in |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1541 |
* the previous invocation of read_frame(). |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1542 |
* Notice that the data that will be deleted hass ben marked for deletion by calling |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1543 |
* lb_data_mark_for_purge() in return_frame() |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1544 |
*/ |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1545 |
lb_data_purge(&(recv_buf->data_buf), 0); |
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1546 |
|
|
cadd89d14ca5
fix bug: frame data read from network was potentially being moved inside buffer (lb_normalize()) before it was processed by caller to read_frame()
mjsousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
1547 |
/*===================================* |
| 0 | 1548 |
* Check for frame in left over data * |
1549 |
*===================================*/ |
|
1550 |
/* If we have any data left over from previous call to read_frame() |
|
1551 |
* (i.e. this very same function), then we try to interpret that |
|
1552 |
* data, and do not wait for any extra bytes... |
|
1553 |
*/ |
|
1554 |
frame_length = search_for_frame(lb_data(&recv_buf->data_buf), |
|
1555 |
lb_data_count(&recv_buf->data_buf), |
|
1556 |
&recv_buf->frame_search_history); |
|
1557 |
if (frame_length > 0) |
|
1558 |
/* We found a valid frame! */ |
|
1559 |
return return_frame(recv_buf, frame_length, recv_data_ptr); |
|
1560 |
||
1561 |
/* If the left over data finished at a frame boundary, and since it |
|
1562 |
* doesn't contain any valid frame, we discard those bytes... |
|
1563 |
*/ |
|
1564 |
if (recv_buf->found_frame_boundary == 1) |
|
1565 |
recv_buf_reset(recv_buf); |
|
1566 |
||
1567 |
/*============================* |
|
1568 |
* wait for data availability * |
|
1569 |
*============================*/ |
|
1570 |
/* if we can't find a valid frame in the existing data, or no data |
|
1571 |
* was left over, then we need to read more bytes! |
|
1572 |
*/ |
|
1573 |
FD_ZERO(&rfds); |
|
1574 |
FD_SET(nd_entry->fd, &rfds); |
|
1575 |
{int sel_res = my_select(nd_entry->fd + 1, &rfds, NULL, end_time);
|
|
1576 |
if (sel_res < 0) |
|
1577 |
return -1; |
|
1578 |
if (sel_res == 0) |
|
1579 |
return -2; |
|
1580 |
} |
|
1581 |
||
1582 |
/*==============* |
|
1583 |
* read a frame * |
|
1584 |
*==============*/ |
|
1585 |
/* The main loop that reads one frame */ |
|
1586 |
/* (multiple calls to read() ) */ |
|
1587 |
/* and jumps out as soon as it finds a valid frame. */ |
|
1588 |
||
1589 |
found_aborted_frame = 0; |
|
1590 |
FD_ZERO(&rfds); |
|
1591 |
FD_SET(nd_entry->fd, &rfds); |
|
1592 |
while (1) {
|
|
1593 |
||
1594 |
/*------------------* |
|
1595 |
* read frame bytes * |
|
1596 |
*------------------*/ |
|
1597 |
/* Read in as many bytes as possible... |
|
1598 |
* But only if we have not found a frame boundary. Once we find |
|
1599 |
* a frame boundary, we do not want to read in any more bytes |
|
1600 |
* and mix them up with the current frame's bytes. |
|
1601 |
*/ |
|
1602 |
if (recv_buf->found_frame_boundary == 0) {
|
|
1603 |
read_stat = read(nd_entry->fd, |
|
1604 |
lb_free(&recv_buf->data_buf), |
|
1605 |
lb_free_count(&recv_buf->data_buf)); |
|
1606 |
if (read_stat < 0) {
|
|
1607 |
if (errno != EINTR) |
|
1608 |
return -1; |
|
1609 |
else |
|
1610 |
read_stat = 0; |
|
1611 |
} |
|
1612 |
#ifdef DEBUG |
|
1613 |
{/* display the hex code of each character received */
|
|
1614 |
int i; |
|
1615 |
fprintf(stderr, "-"); |
|
1616 |
for (i=0; i < read_stat; i++) |
|
1617 |
fprintf(stderr, "<0x%2X>", *(lb_free(&recv_buf->data_buf) + i)); |
|
1618 |
} |
|
1619 |
#endif |
|
1620 |
lb_data_add(&recv_buf->data_buf, read_stat); |
|
1621 |
} |
|
1622 |
||
1623 |
/*-----------------------* |
|
1624 |
* check for valid frame * |
|
1625 |
*-----------------------*/ |
|
1626 |
frame_length = search_for_frame(lb_data(&recv_buf->data_buf), |
|
1627 |
lb_data_count(&recv_buf->data_buf), |
|
1628 |
&recv_buf->frame_search_history); |
|
1629 |
if (frame_length > 0) |
|
1630 |
/* We found a valid frame! */ |
|
1631 |
return return_frame(recv_buf, frame_length, recv_data_ptr); |
|
1632 |
||
1633 |
/* if we reach this point, we are sure we do not have valid frame |
|
1634 |
* of known length in the current data with the current offset... |
|
1635 |
*/ |
|
1636 |
||
1637 |
/*---------------------------------* |
|
1638 |
* Have we found an aborted frame? * |
|
1639 |
*---------------------------------*/ |
|
1640 |
if (lb_data_count(&recv_buf->data_buf) >= MAX_RTU_FRAME_LENGTH) |
|
1641 |
found_aborted_frame = 1; |
|
1642 |
||
1643 |
/*---------------------------------* |
|
1644 |
* Must we try a new frame_offset? * |
|
1645 |
*---------------------------------*/ |
|
1646 |
if (found_aborted_frame == 1) {
|
|
1647 |
/* Note that the found_aborted_frame flag is only set if: |
|
1648 |
* 1 - we have previously detected a frame_boundary, |
|
1649 |
* (i.e. found_frame_boundary is == 1 !!) so we won't be |
|
1650 |
* reading in more bytes; |
|
1651 |
* 2 - we have read more bytes than the maximum frame length |
|
1652 |
* |
|
1653 |
* Considering we have just failed finding a valid frame, and the above |
|
1654 |
* points (1) and (2), then there is no way we are still going to |
|
1655 |
* find a valid frame in the current data. |
|
1656 |
* We must therefore try a new first byte for the frame... |
|
1657 |
*/ |
|
1658 |
next_frame_offset(recv_buf, slave_id); |
|
1659 |
} |
|
1660 |
||
1661 |
/*-----------------------------* |
|
1662 |
* check for data availability * |
|
1663 |
*-----------------------------*/ |
|
1664 |
if (recv_buf->found_frame_boundary == 0) {
|
|
1665 |
/* We need more bytes!! */ |
|
1666 |
/* |
|
1667 |
* if no character at the buffer, then we wait time_15_char_ |
|
1668 |
* before accepting end of frame |
|
1669 |
*/ |
|
1670 |
/* NOTES: |
|
1671 |
* - On Linux, timeout is modified by select() to reflect |
|
1672 |
* the amount of time not slept; most other implementations do |
|
1673 |
* not do this. On those platforms we will simply have to wait |
|
1674 |
* longer than we wished if select() is by any chance interrupted |
|
1675 |
* by a signal... |
|
1676 |
*/ |
|
1677 |
timeout = nd_entry->time_15_char_; |
|
1678 |
while ((res = select(nd_entry->fd+1, &rfds, NULL, NULL, &timeout)) < 0) {
|
|
1679 |
if (errno != EINTR) |
|
1680 |
return -1; |
|
1681 |
/* We will be calling select() again. |
|
1682 |
* We need to reset the FD SET ! |
|
1683 |
*/ |
|
1684 |
FD_ZERO(&rfds); |
|
1685 |
FD_SET(nd_entry->fd, &rfds); |
|
1686 |
} |
|
1687 |
||
1688 |
if (res == 0) {
|
|
1689 |
int frame_length = lb_data_count(&recv_buf->data_buf); |
|
1690 |
/* We have detected an end of frame using timing boundaries... */ |
|
1691 |
recv_buf->found_frame_boundary = 1; /* => stop trying to read any more bytes! */ |
|
1692 |
||
1693 |
/* Let's check if we happen to have a correct frame... */ |
|
1694 |
if ((frame_length <= MAX_RTU_FRAME_LENGTH) && |
|
1695 |
(frame_length - RTU_FRAME_CRC_LENGTH > 0)) {
|
|
1696 |
if ( crc_calc(lb_data(&recv_buf->data_buf), frame_length - RTU_FRAME_CRC_LENGTH) |
|
1697 |
== crc_read(lb_data(&recv_buf->data_buf), frame_length - RTU_FRAME_CRC_LENGTH)) {
|
|
1698 |
/* We have found a valid frame. Let's get out of here! */ |
|
1699 |
return return_frame(recv_buf, frame_length, recv_data_ptr); |
|
1700 |
} |
|
1701 |
} |
|
1702 |
||
1703 |
/* We have detected a frame boundary, but the frame we read |
|
1704 |
* is not valid... |
|
1705 |
* |
|
1706 |
* One of the following reasons must be the cause: |
|
1707 |
* 1 - we are reading a single aborted frame. |
|
1708 |
* 2 - we are reading more than one frame. The first frame, |
|
1709 |
* followed by any number of valid and/or aborted frames, |
|
1710 |
* may be one of: |
|
1711 |
* a - a valid frame whose length is unknown to us, |
|
1712 |
* i.e. it is not specified in the public Modbus spec. |
|
1713 |
* b - an aborted frame. |
|
1714 |
* |
|
1715 |
* Due to the complexity of reading 2a as a correct frame, we will |
|
1716 |
* consider it as an aborted frame. (NOTE: it is possible, but |
|
1717 |
* we will ignore it until the need arises... hopefully, never!) |
|
1718 |
* |
|
1719 |
* To put it succintly, what wee now have is an 'aborted' frame |
|
1720 |
* followed by one or more aborted and/or valid frames. To get to |
|
1721 |
* any valid frames, and since we do not know where they begin, |
|
1722 |
* we will have to consider every byte as the possible begining |
|
1723 |
* of a valid frame. For this permutation, we ignore the first byte, |
|
1724 |
* and carry on from there... |
|
1725 |
*/ |
|
1726 |
found_aborted_frame = 1; |
|
1727 |
lb_data_purge(&recv_buf->data_buf, 1 /* skip one byte */); |
|
1728 |
recv_buf->frame_search_history = 0; |
|
1729 |
} |
|
1730 |
} |
|
1731 |
||
1732 |
/*-------------------------------* |
|
1733 |
* check for data yet to process * |
|
1734 |
*-------------------------------*/ |
|
1735 |
if ((lb_data_count(&recv_buf->data_buf) < MIN_FRAME_LENGTH) && |
|
1736 |
(recv_buf->found_frame_boundary == 1)) {
|
|
1737 |
/* We have no more data to process, and will not read anymore! */ |
|
1738 |
recv_buf_reset(recv_buf); |
|
1739 |
/* Return TIMEOUT error */ |
|
1740 |
return -2; |
|
1741 |
} |
|
1742 |
} /* while (1)*/ |
|
1743 |
||
1744 |
/* humour the compiler... */ |
|
1745 |
return -1; |
|
1746 |
} |
|
1747 |
||
1748 |
||
1749 |
||
1750 |
||
1751 |
||
1752 |
/************************************/ |
|
1753 |
/** **/ |
|
1754 |
/** Read a Modbus RTU frame **/ |
|
1755 |
/** **/ |
|
1756 |
/************************************/ |
|
1757 |
||
1758 |
/* The public function that reads a valid modbus frame. |
|
1759 |
* |
|
1760 |
* The returned frame is guaranteed to be different to the |
|
1761 |
* the frame stored in send_data, and to start with the |
|
1762 |
* same slave address stored in send_data[0]. |
|
1763 |
* |
|
1764 |
* If send_data is NULL, send_data_length = 0, or |
|
1765 |
* ignore_echo == 0, then the first valid frame read off |
|
1766 |
* the bus is returned. |
|
1767 |
* |
|
1768 |
* return value: The length (in bytes) of the valid frame, |
|
1769 |
* -1 on error |
|
1770 |
* -2 on timeout |
|
1771 |
*/ |
|
1772 |
||
1773 |
int modbus_rtu_read(int *nd, |
|
1774 |
u8 **recv_data_ptr, |
|
1775 |
u16 *transaction_id, |
|
1776 |
const u8 *send_data, |
|
1777 |
int send_length, |
|
1778 |
const struct timespec *recv_timeout) {
|
|
1779 |
struct timespec end_time, *ts_ptr; |
|
1780 |
int res, recv_length, iter; |
|
1781 |
u8 *local_recv_data_ptr; |
|
1782 |
u8 *slave_id, local_slave_id; |
|
1783 |
nd_entry_t *nd_entry; |
|
1784 |
||
1785 |
/* Check input parameters... */ |
|
1786 |
if (nd == NULL) |
|
1787 |
return -1; |
|
1788 |
||
1789 |
if (recv_data_ptr == NULL) |
|
1790 |
recv_data_ptr = &local_recv_data_ptr; |
|
1791 |
||
1792 |
if ((send_data == NULL) && (send_length != 0)) |
|
1793 |
return -1; |
|
1794 |
||
1795 |
/* check if nd is correct... */ |
|
1796 |
if ((nd_entry = nd_table_get_nd(&nd_table_, *nd)) == NULL) |
|
1797 |
return -1; |
|
1798 |
||
1799 |
/* check if nd is initialzed... */ |
|
1800 |
if (nd_entry->fd < 0) |
|
1801 |
return -1; |
|
1802 |
||
1803 |
slave_id = NULL; |
|
1804 |
if (send_length > L2_FRAME_SLAVEID_OFS) {
|
|
1805 |
local_slave_id = send_data[L2_FRAME_SLAVEID_OFS]; |
|
1806 |
slave_id = &local_slave_id; |
|
1807 |
} |
|
1808 |
||
1809 |
/* We will potentially read many frames, and we cannot reset the timeout |
|
1810 |
* for every frame we read. We therefore determine the absolute time_out, |
|
1811 |
* and use this as a parameter for each call to read_frame() instead of |
|
1812 |
* using a relative timeout. |
|
1813 |
* |
|
1814 |
* NOTE: see also the timeout related comment in the read_frame()= function! |
|
1815 |
*/ |
|
1816 |
/* get the current time... */ |
|
1817 |
ts_ptr = NULL; |
|
1818 |
if (recv_timeout != NULL) {
|
|
1819 |
ts_ptr = &end_time; |
|
1820 |
*ts_ptr = timespec_add_curtime(*recv_timeout); |
|
1821 |
} |
|
1822 |
||
1823 |
/* NOTE: When using a half-duplex RS-485 bus, some (most ?) RS232-485 |
|
1824 |
* converters will send back to the RS232 port whatever we write, |
|
1825 |
* so we will read in whatever we write out onto the bus. |
|
1826 |
* We will therefore have to compare |
|
1827 |
* the first frame we read with the one we sent. If they are |
|
1828 |
* identical it is because we are in fact working on a RS-485 |
|
1829 |
* bus and must therefore read in a second frame which will be |
|
1830 |
* the true response to our query. |
|
1831 |
* If the first frame we receive is different to the query we |
|
1832 |
* just sent, then we are *not* working on a RS-485 bus, and |
|
1833 |
* that is already the real response to our query. |
|
1834 |
* |
|
1835 |
* Flushing the input cache immediately after sending the query |
|
1836 |
* could solve this issue, but we have no guarantee that this |
|
1837 |
* process would not get swapped out between the write() and |
|
1838 |
* flush() calls, and we could therefore be flushing the response |
|
1839 |
* frame! |
|
1840 |
*/ |
|
1841 |
||
1842 |
iter = 0; |
|
1843 |
while ((res = recv_length = read_frame(nd_entry, recv_data_ptr, ts_ptr, slave_id)) >= 0) {
|
|
1844 |
if (iter < INT_MAX) iter++; |
|
1845 |
||
1846 |
if ((send_length <= 0) || (nd_entry->ignore_echo == 0)) |
|
1847 |
/* any valid frame will do... */ |
|
1848 |
return recv_length; |
|
1849 |
||
1850 |
if ((send_length > L2_FRAME_SLAVEID_OFS + 1) && (iter == 1)) |
|
1851 |
/* We have a frame in send_data, |
|
1852 |
* so we must make sure we are not reading in the frame just sent... |
|
1853 |
* |
|
1854 |
* We must only do this for the first frame we read. Subsequent |
|
1855 |
* frames are guaranteed not to be the previously sent frame |
|
1856 |
* since the modbus_rtu_write() resets the recv buffer. |
|
1857 |
* Remember too that valid modbus responses may be exactly the same |
|
1858 |
* as the request frame!! |
|
1859 |
*/ |
|
1860 |
if (recv_length == send_length) |
|
1861 |
if (memcmp(*recv_data_ptr, send_data, recv_length) == 0) |
|
1862 |
/* recv == send !!! */ |
|
1863 |
/* read in another frame. */ |
|
1864 |
continue; |
|
1865 |
||
1866 |
/* The frame read is either: |
|
1867 |
* - different to the frame in send_data |
|
1868 |
* - or there is only the slave id in send_data[0] |
|
1869 |
* - or both of the above... |
|
1870 |
*/ |
|
1871 |
if (send_length > L2_FRAME_SLAVEID_OFS) |
|
1872 |
if (recv_length > L2_FRAME_SLAVEID_OFS) |
|
1873 |
/* check that frame is from/to the correct slave... */ |
|
1874 |
if ((*recv_data_ptr)[L2_FRAME_SLAVEID_OFS] == send_data[L2_FRAME_SLAVEID_OFS]) |
|
1875 |
/* yep, it is... */ |
|
1876 |
return recv_length; |
|
1877 |
||
1878 |
/* The frame we have received is not acceptable... |
|
1879 |
* Let's read a new frame. |
|
1880 |
*/ |
|
1881 |
} /* while(...) */ |
|
1882 |
||
1883 |
/* error reading response! */ |
|
1884 |
/* Return the error returned by read_frame! */ |
|
1885 |
return res; |
|
1886 |
} |
|
1887 |
||
1888 |
||
1889 |
||
1890 |
||
1891 |
||
1892 |
/**************************************************************/ |
|
1893 |
/**************************************************************/ |
|
1894 |
/**** ****/ |
|
1895 |
/**** ****/ |
|
1896 |
/**** Initialising and Shutting Down Library ****/ |
|
1897 |
/**** ****/ |
|
1898 |
/**** ****/ |
|
1899 |
/**************************************************************/ |
|
1900 |
/**************************************************************/ |
|
1901 |
||
1902 |
/******************************/ |
|
1903 |
/** **/ |
|
1904 |
/** Load Default Values **/ |
|
1905 |
/** **/ |
|
1906 |
/******************************/ |
|
1907 |
||
1908 |
static void set_defaults(int *baud, |
|
1909 |
int *parity, |
|
1910 |
int *data_bits, |
|
1911 |
int *stop_bits) {
|
|
1912 |
/* Set the default values, if required... */ |
|
1913 |
if (*baud == 0) |
|
1914 |
*baud = DEF_BAUD_RATE; |
|
1915 |
if (*data_bits == 0) |
|
1916 |
*data_bits = DEF_DATA_BITS; |
|
1917 |
if (*stop_bits == 0) {
|
|
1918 |
if (*parity == 0) |
|
1919 |
*stop_bits = DEF_STOP_BITS_NOP; /* no parity */ |
|
1920 |
else |
|
1921 |
*stop_bits = DEF_STOP_BITS_PAR; /* parity used */ |
|
1922 |
} |
|
1923 |
} |
|
1924 |
||
1925 |
||
1926 |
/******************************/ |
|
1927 |
/** **/ |
|
1928 |
/** Initialise Library **/ |
|
1929 |
/** **/ |
|
1930 |
/******************************/ |
|
1931 |
||
1932 |
int modbus_rtu_init(int nd_count, |
|
1933 |
optimization_t opt, |
|
1934 |
int *extra_bytes) |
|
1935 |
{
|
|
1936 |
#ifdef DEBUG |
|
1937 |
fprintf(stderr, "modbus_rtu_init(): called...\n"); |
|
1938 |
fprintf(stderr, "creating %d node descriptors\n", nd_count); |
|
1939 |
if (opt == optimize_speed) |
|
1940 |
fprintf(stderr, "optimizing for speed\n"); |
|
1941 |
if (opt == optimize_size) |
|
1942 |
fprintf(stderr, "optimizing for size\n"); |
|
1943 |
#endif |
|
1944 |
||
1945 |
/* check input parameters...*/ |
|
1946 |
if (0 == nd_count) {
|
|
1947 |
if (extra_bytes != NULL) |
|
1948 |
// Not the corect value for this layer. |
|
1949 |
// What we set it to in case this layer is not used! |
|
1950 |
*extra_bytes = 0; |
|
1951 |
return 0; |
|
1952 |
} |
|
1953 |
if (nd_count <= 0) |
|
1954 |
goto error_exit_0; |
|
1955 |
||
1956 |
if (extra_bytes == NULL) |
|
1957 |
goto error_exit_0; |
|
1958 |
||
1959 |
if (crc_init(opt) < 0) {
|
|
1960 |
#ifdef ERRMSG |
|
1961 |
fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing crc buffers\n"); |
|
1962 |
#endif |
|
1963 |
goto error_exit_0; |
|
1964 |
} |
|
1965 |
||
1966 |
/* set the extra_bytes value... */ |
|
1967 |
/* Please see note before the modbus_rtu_write() function for a |
|
1968 |
* better understanding of this extremely ugly hack... |
|
1969 |
* |
|
1970 |
* The number of extra bytes that must be allocated to the data buffer |
|
1971 |
* before calling modbus_rtu_write() |
|
1972 |
*/ |
|
1973 |
*extra_bytes = RTU_FRAME_CRC_LENGTH; |
|
1974 |
||
1975 |
/* initialise nd table... */ |
|
1976 |
if (nd_table_init(&nd_table_, nd_count) < 0) |
|
1977 |
goto error_exit_0; |
|
1978 |
||
1979 |
/* remember the optimization choice for later reference... */ |
|
1980 |
optimization_ = opt; |
|
1981 |
||
1982 |
#ifdef DEBUG |
|
1983 |
fprintf(stderr, "modbus_rtu_init(): returning succesfuly...\n"); |
|
1984 |
#endif |
|
1985 |
return 0; |
|
1986 |
||
1987 |
error_exit_0: |
|
1988 |
if (extra_bytes != NULL) |
|
1989 |
// Not the corect value for this layer. |
|
1990 |
// What we set it to in case of error! |
|
1991 |
*extra_bytes = 0; |
|
1992 |
return -1; |
|
1993 |
} |
|
1994 |
||
1995 |
||
1996 |
||
1997 |
/******************************/ |
|
1998 |
/** **/ |
|
1999 |
/** Open node descriptor **/ |
|
2000 |
/** **/ |
|
2001 |
/******************************/ |
|
2002 |
||
2003 |
/* Open a node for master or slave operation. |
|
2004 |
* Returns the node descriptor, or -1 on error. |
|
2005 |
* |
|
2006 |
* This function is mapped onto both |
|
2007 |
* modbus_connect() and modbus_listen() |
|
2008 |
*/ |
|
2009 |
int modbus_rtu_connect(node_addr_t node_addr) {
|
|
2010 |
int node_descriptor; |
|
2011 |
nd_entry_t *nd_entry; |
|
2012 |
||
2013 |
#ifdef DEBUG |
|
2014 |
fprintf(stderr, "modbus_rtu_connect(): called...\n"); |
|
2015 |
fprintf(stderr, "opening %s\n", node_addr.addr.rtu.device); |
|
2016 |
fprintf(stderr, "baud_rate = %d\n", node_addr.addr.rtu.baud); |
|
2017 |
fprintf(stderr, "parity = %d\n", node_addr.addr.rtu.parity); |
|
2018 |
fprintf(stderr, "data_bits = %d\n", node_addr.addr.rtu.data_bits); |
|
2019 |
fprintf(stderr, "stop_bits = %d\n", node_addr.addr.rtu.stop_bits); |
|
2020 |
fprintf(stderr, "ignore_echo = %d\n", node_addr.addr.rtu.ignore_echo); |
|
2021 |
#endif |
|
2022 |
||
2023 |
/* Check for valid address family */ |
|
2024 |
if (node_addr.naf != naf_rtu) |
|
2025 |
/* wrong address type... */ |
|
2026 |
goto error_exit_0; |
|
2027 |
||
2028 |
/* find a free node descriptor */ |
|
2029 |
if ((node_descriptor = nd_table_get_free_nd(&nd_table_)) < 0) |
|
2030 |
/* if no free nodes to initialize, then we are finished... */ |
|
2031 |
goto error_exit_0; |
|
2032 |
if ((nd_entry = nd_table_get_nd(&nd_table_, node_descriptor)) == NULL) |
|
2033 |
/* strange, this should not occur... */ |
|
2034 |
goto error_exit_0; |
|
2035 |
||
2036 |
/* set the default values... */ |
|
2037 |
set_defaults(&(node_addr.addr.rtu.baud), |
|
2038 |
&(node_addr.addr.rtu.parity), |
|
2039 |
&(node_addr.addr.rtu.data_bits), |
|
2040 |
&(node_addr.addr.rtu.stop_bits)); |
|
2041 |
||
2042 |
#ifdef DEBUG |
|
2043 |
fprintf(stderr, "modbus_rtu_connect(): calling nd_entry_connect()\n"); |
|
2044 |
#endif |
|
2045 |
if (nd_entry_connect(nd_entry, &node_addr, optimization_) < 0) |
|
2046 |
goto error_exit_0; |
|
2047 |
||
2048 |
#ifdef DEBUG |
|
2049 |
fprintf(stderr, "modbus_rtu_connect(): %s open\n", node_addr.addr.rtu.device); |
|
2050 |
fprintf(stderr, "modbus_rtu_connect(): returning nd=%d\n", node_descriptor); |
|
2051 |
#endif |
|
2052 |
return node_descriptor; |
|
2053 |
||
2054 |
error_exit_0: |
|
2055 |
#ifdef DEBUG |
|
2056 |
fprintf(stderr, "modbus_rtu_connect(): error!\n"); |
|
2057 |
#endif |
|
2058 |
return -1; |
|
2059 |
} |
|
2060 |
||
2061 |
||
2062 |
||
2063 |
int modbus_rtu_listen(node_addr_t node_addr) {
|
|
2064 |
return modbus_rtu_connect(node_addr); |
|
2065 |
} |
|
2066 |
||
2067 |
||
2068 |
||
2069 |
/******************************/ |
|
2070 |
/** **/ |
|
2071 |
/** Close node descriptor **/ |
|
2072 |
/** **/ |
|
2073 |
/******************************/ |
|
2074 |
||
2075 |
int modbus_rtu_close(int nd) {
|
|
2076 |
return nd_table_free_nd(&nd_table_, nd); |
|
2077 |
} |
|
2078 |
||
2079 |
||
2080 |
||
2081 |
/******************************/ |
|
2082 |
/** **/ |
|
2083 |
/** Shutdown Library **/ |
|
2084 |
/** **/ |
|
2085 |
/******************************/ |
|
2086 |
||
2087 |
int modbus_rtu_done(void) {
|
|
2088 |
nd_table_done(&nd_table_); |
|
2089 |
crc_done(); |
|
2090 |
||
2091 |
return 0; |
|
2092 |
} |
|
2093 |
||
2094 |
||
2095 |
||
2096 |
||
2097 |
/******************************/ |
|
2098 |
/** **/ |
|
2099 |
/** **/ |
|
2100 |
/** **/ |
|
2101 |
/******************************/ |
|
2102 |
int modbus_rtu_silence_init(void) {
|
|
2103 |
return 0; |
|
2104 |
} |
|
2105 |
||
2106 |
||
2107 |
||
2108 |
||
2109 |
/******************************/ |
|
2110 |
/** **/ |
|
2111 |
/** **/ |
|
2112 |
/** **/ |
|
2113 |
/******************************/ |
|
2114 |
||
2115 |
||
2116 |
double modbus_rtu_get_min_timeout(int baud, |
|
2117 |
int parity, |
|
2118 |
int data_bits, |
|
2119 |
int stop_bits) {
|
|
2120 |
int parity_bits, start_bits, char_bits; |
|
2121 |
||
2122 |
set_defaults(&baud, &parity, &data_bits, &stop_bits); |
|
2123 |
parity_bits = (parity == 0)?0:1; |
|
2124 |
start_bits = 1; |
|
2125 |
char_bits = start_bits + data_bits + parity_bits + stop_bits; |
|
2126 |
return (double)((MAX_RTU_FRAME_LENGTH * char_bits) / baud); |
|
2127 |
} |
|
2128 |
||
2129 |