|
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));
|
|
|
1425 |
/* remove the frame bytes off the buffer */
|
|
|
1426 |
lb_data_purge(&(buf->data_buf), frame_length);
|
|
|
1427 |
/* reset the search_history flag */
|
|
|
1428 |
buf->frame_search_history = 0;
|
|
|
1429 |
/* if the buffer becomes empty, then reset boundary flag */
|
|
|
1430 |
if (lb_data_count(&(buf->data_buf)) <= 0)
|
|
|
1431 |
buf->found_frame_boundary = 0;
|
|
|
1432 |
/* return the frame length, excluding CRC */
|
|
|
1433 |
return frame_length - RTU_FRAME_CRC_LENGTH;
|
|
|
1434 |
}
|
|
|
1435 |
|
|
|
1436 |
/* A function to read a valid frame off the rtu bus.
|
|
|
1437 |
*
|
|
|
1438 |
* NOTES:
|
|
|
1439 |
* - The returned frame is guaranteed to be a valid frame.
|
|
|
1440 |
* - The returned length does *not* include the CRC.
|
|
|
1441 |
* - The returned frame is not guaranteed to have the same
|
|
|
1442 |
* slave id as that stored in (*slave_id). This value is used
|
|
|
1443 |
* merely in optimizing the search for wanted valid frames
|
|
|
1444 |
* after reading an aborted frame. Only in this situation do
|
|
|
1445 |
* we limit our search for frames with a slvae id == (*slave_id).
|
|
|
1446 |
* Under normal circumstances, the value in (*slave_id) is
|
|
|
1447 |
* simply ignored...
|
|
|
1448 |
* If any valid frame is desired, then slave_id should be NULL.
|
|
|
1449 |
*
|
|
|
1450 |
*/
|
|
|
1451 |
|
|
|
1452 |
/* NOTE: We cannot relly on the 3.5 character interval between frames to detect
|
|
|
1453 |
* end of frame. We are reading the bytes from a user process, so in
|
|
|
1454 |
* essence the bytes we are reading are coming off a cache.
|
|
|
1455 |
* Any inter-character delays between the arrival of the bytes are
|
|
|
1456 |
* lost as soon as they were placed in the cache.
|
|
|
1457 |
*
|
|
|
1458 |
* Our only recourse is to analyse the frame we are reading in real-time,
|
|
|
1459 |
* and check if it is a valid frame by checking it's CRC.
|
|
|
1460 |
* To optimise this, we must be able to figure out the length
|
|
|
1461 |
* of the frame currently being received by analysing the first bytes
|
|
|
1462 |
* of that frame. Unfortunately, we have three problems with this:
|
|
|
1463 |
* 1) The spec does not specify the format of every possible modbus
|
|
|
1464 |
* frame. For ex.functions 9, 10, 13, 14, 18 and 19(?).
|
|
|
1465 |
* 2) It is not possible to figure out whether a frame is a query
|
|
|
1466 |
* or a response by just analysing the frame, and query and response
|
|
|
1467 |
* frames have different sizes...
|
|
|
1468 |
* 3) A frame may be aborted in the middle! We have no easy way of telling
|
|
|
1469 |
* if what we are reading is a partial (aborted) frame, followed by a
|
|
|
1470 |
* correct frame.
|
|
|
1471 |
* Possible solutions to:
|
|
|
1472 |
* 1) We could try to reverse engineer, but at the moment I have no
|
|
|
1473 |
* PLCs that will generate the required frames.
|
|
|
1474 |
* The chosen method is to verify the CRC if we are lucky enough to
|
|
|
1475 |
* detect the 3.5 frame boundary imediately following one of these
|
|
|
1476 |
* frames of unknown length.
|
|
|
1477 |
* If we do not detect any frame boundary, then our only option
|
|
|
1478 |
* is to consider it an aborted frame.
|
|
|
1479 |
* 2) We aim for the query frame (usually the shortest), and check
|
|
|
1480 |
* it's CRC. If it matches, we accept, the frame, otherwise we try
|
|
|
1481 |
* a response frame.
|
|
|
1482 |
* 3) The only way is to consider a frame boundary after each byte,
|
|
|
1483 |
* (i.e. ignore one bye at a time) and verify if the following bytes
|
|
|
1484 |
* constitue a valid frame (by checking the CRC).
|
|
|
1485 |
*
|
|
|
1486 |
* When reading an aborted frame followed by two or more valid frames, if
|
|
|
1487 |
* we are unlucky and do not detetect any frame boundary using the 3.5
|
|
|
1488 |
* character interval, then we will most likely be reading in bytes
|
|
|
1489 |
* beyond the first valid frame. This means we will have to store the extra
|
|
|
1490 |
* bytes we have already read, so they may be handled the next time the
|
|
|
1491 |
* read_frame() function is called.
|
|
|
1492 |
*/
|
|
|
1493 |
/*
|
|
|
1494 |
* NOTE: The modbus RTU spec is inconsistent on how to handle
|
|
|
1495 |
* inter-character delays larger than 1.5 characters.
|
|
|
1496 |
* - On one paragraph it is stated that any delay larger than
|
|
|
1497 |
* 1.5 character times aborts the current frame, and a new
|
|
|
1498 |
* frame is started.
|
|
|
1499 |
* - On another paragraph it is stated that a frame must begin
|
|
|
1500 |
* with a silence of 3.5 character times.
|
|
|
1501 |
*
|
|
|
1502 |
* We will therefore consider that any delay larger than 1.5 character
|
|
|
1503 |
* times terminates a valid frame. All the above references to the 3.5 character
|
|
|
1504 |
* interval should therefore be read as a 1.5 character interval.
|
|
|
1505 |
*/
|
|
|
1506 |
/* NOTE: This function is only called from one place in the rest of the code,
|
|
|
1507 |
* so we might just as well make it inline...
|
|
|
1508 |
*/
|
|
|
1509 |
/* RETURNS: number of bytes in received frame
|
|
|
1510 |
* -1 on read file error
|
|
|
1511 |
* -2 on timeout
|
|
|
1512 |
*/
|
|
|
1513 |
static inline int read_frame(nd_entry_t *nd_entry,
|
|
|
1514 |
u8 **recv_data_ptr,
|
|
|
1515 |
struct timespec *end_time,
|
|
|
1516 |
u8 *slave_id)
|
|
|
1517 |
{
|
|
|
1518 |
/* temporary variables... */
|
|
|
1519 |
fd_set rfds;
|
|
|
1520 |
struct timeval timeout;
|
|
|
1521 |
int res, read_stat;
|
|
|
1522 |
int frame_length;
|
|
|
1523 |
recv_buf_t *recv_buf = &nd_entry->recv_buf_;
|
|
|
1524 |
|
|
|
1525 |
/* Flag:
|
|
|
1526 |
* 1 => we are reading in an aborted frame, so we must
|
|
|
1527 |
* start ignoring bytes...
|
|
|
1528 |
*/
|
|
|
1529 |
int found_aborted_frame;
|
|
|
1530 |
|
|
|
1531 |
/* assume error... */
|
|
|
1532 |
*recv_data_ptr = NULL;
|
|
|
1533 |
|
|
|
1534 |
/*===================================*
|
|
|
1535 |
* Check for frame in left over data *
|
|
|
1536 |
*===================================*/
|
|
|
1537 |
/* If we have any data left over from previous call to read_frame()
|
|
|
1538 |
* (i.e. this very same function), then we try to interpret that
|
|
|
1539 |
* data, and do not wait for any extra bytes...
|
|
|
1540 |
*/
|
|
|
1541 |
frame_length = search_for_frame(lb_data(&recv_buf->data_buf),
|
|
|
1542 |
lb_data_count(&recv_buf->data_buf),
|
|
|
1543 |
&recv_buf->frame_search_history);
|
|
|
1544 |
if (frame_length > 0)
|
|
|
1545 |
/* We found a valid frame! */
|
|
|
1546 |
return return_frame(recv_buf, frame_length, recv_data_ptr);
|
|
|
1547 |
|
|
|
1548 |
/* If the left over data finished at a frame boundary, and since it
|
|
|
1549 |
* doesn't contain any valid frame, we discard those bytes...
|
|
|
1550 |
*/
|
|
|
1551 |
if (recv_buf->found_frame_boundary == 1)
|
|
|
1552 |
recv_buf_reset(recv_buf);
|
|
|
1553 |
|
|
|
1554 |
/*============================*
|
|
|
1555 |
* wait for data availability *
|
|
|
1556 |
*============================*/
|
|
|
1557 |
/* if we can't find a valid frame in the existing data, or no data
|
|
|
1558 |
* was left over, then we need to read more bytes!
|
|
|
1559 |
*/
|
|
|
1560 |
FD_ZERO(&rfds);
|
|
|
1561 |
FD_SET(nd_entry->fd, &rfds);
|
|
|
1562 |
{int sel_res = my_select(nd_entry->fd + 1, &rfds, NULL, end_time);
|
|
|
1563 |
if (sel_res < 0)
|
|
|
1564 |
return -1;
|
|
|
1565 |
if (sel_res == 0)
|
|
|
1566 |
return -2;
|
|
|
1567 |
}
|
|
|
1568 |
|
|
|
1569 |
/*==============*
|
|
|
1570 |
* read a frame *
|
|
|
1571 |
*==============*/
|
|
|
1572 |
/* The main loop that reads one frame */
|
|
|
1573 |
/* (multiple calls to read() ) */
|
|
|
1574 |
/* and jumps out as soon as it finds a valid frame. */
|
|
|
1575 |
|
|
|
1576 |
found_aborted_frame = 0;
|
|
|
1577 |
FD_ZERO(&rfds);
|
|
|
1578 |
FD_SET(nd_entry->fd, &rfds);
|
|
|
1579 |
while (1) {
|
|
|
1580 |
|
|
|
1581 |
/*------------------*
|
|
|
1582 |
* read frame bytes *
|
|
|
1583 |
*------------------*/
|
|
|
1584 |
/* Read in as many bytes as possible...
|
|
|
1585 |
* But only if we have not found a frame boundary. Once we find
|
|
|
1586 |
* a frame boundary, we do not want to read in any more bytes
|
|
|
1587 |
* and mix them up with the current frame's bytes.
|
|
|
1588 |
*/
|
|
|
1589 |
if (recv_buf->found_frame_boundary == 0) {
|
|
|
1590 |
read_stat = read(nd_entry->fd,
|
|
|
1591 |
lb_free(&recv_buf->data_buf),
|
|
|
1592 |
lb_free_count(&recv_buf->data_buf));
|
|
|
1593 |
if (read_stat < 0) {
|
|
|
1594 |
if (errno != EINTR)
|
|
|
1595 |
return -1;
|
|
|
1596 |
else
|
|
|
1597 |
read_stat = 0;
|
|
|
1598 |
}
|
|
|
1599 |
#ifdef DEBUG
|
|
|
1600 |
{/* display the hex code of each character received */
|
|
|
1601 |
int i;
|
|
|
1602 |
fprintf(stderr, "-");
|
|
|
1603 |
for (i=0; i < read_stat; i++)
|
|
|
1604 |
fprintf(stderr, "<0x%2X>", *(lb_free(&recv_buf->data_buf) + i));
|
|
|
1605 |
}
|
|
|
1606 |
#endif
|
|
|
1607 |
lb_data_add(&recv_buf->data_buf, read_stat);
|
|
|
1608 |
}
|
|
|
1609 |
|
|
|
1610 |
/*-----------------------*
|
|
|
1611 |
* check for valid frame *
|
|
|
1612 |
*-----------------------*/
|
|
|
1613 |
frame_length = search_for_frame(lb_data(&recv_buf->data_buf),
|
|
|
1614 |
lb_data_count(&recv_buf->data_buf),
|
|
|
1615 |
&recv_buf->frame_search_history);
|
|
|
1616 |
if (frame_length > 0)
|
|
|
1617 |
/* We found a valid frame! */
|
|
|
1618 |
return return_frame(recv_buf, frame_length, recv_data_ptr);
|
|
|
1619 |
|
|
|
1620 |
/* if we reach this point, we are sure we do not have valid frame
|
|
|
1621 |
* of known length in the current data with the current offset...
|
|
|
1622 |
*/
|
|
|
1623 |
|
|
|
1624 |
/*---------------------------------*
|
|
|
1625 |
* Have we found an aborted frame? *
|
|
|
1626 |
*---------------------------------*/
|
|
|
1627 |
if (lb_data_count(&recv_buf->data_buf) >= MAX_RTU_FRAME_LENGTH)
|
|
|
1628 |
found_aborted_frame = 1;
|
|
|
1629 |
|
|
|
1630 |
/*---------------------------------*
|
|
|
1631 |
* Must we try a new frame_offset? *
|
|
|
1632 |
*---------------------------------*/
|
|
|
1633 |
if (found_aborted_frame == 1) {
|
|
|
1634 |
/* Note that the found_aborted_frame flag is only set if:
|
|
|
1635 |
* 1 - we have previously detected a frame_boundary,
|
|
|
1636 |
* (i.e. found_frame_boundary is == 1 !!) so we won't be
|
|
|
1637 |
* reading in more bytes;
|
|
|
1638 |
* 2 - we have read more bytes than the maximum frame length
|
|
|
1639 |
*
|
|
|
1640 |
* Considering we have just failed finding a valid frame, and the above
|
|
|
1641 |
* points (1) and (2), then there is no way we are still going to
|
|
|
1642 |
* find a valid frame in the current data.
|
|
|
1643 |
* We must therefore try a new first byte for the frame...
|
|
|
1644 |
*/
|
|
|
1645 |
next_frame_offset(recv_buf, slave_id);
|
|
|
1646 |
}
|
|
|
1647 |
|
|
|
1648 |
/*-----------------------------*
|
|
|
1649 |
* check for data availability *
|
|
|
1650 |
*-----------------------------*/
|
|
|
1651 |
if (recv_buf->found_frame_boundary == 0) {
|
|
|
1652 |
/* We need more bytes!! */
|
|
|
1653 |
/*
|
|
|
1654 |
* if no character at the buffer, then we wait time_15_char_
|
|
|
1655 |
* before accepting end of frame
|
|
|
1656 |
*/
|
|
|
1657 |
/* NOTES:
|
|
|
1658 |
* - On Linux, timeout is modified by select() to reflect
|
|
|
1659 |
* the amount of time not slept; most other implementations do
|
|
|
1660 |
* not do this. On those platforms we will simply have to wait
|
|
|
1661 |
* longer than we wished if select() is by any chance interrupted
|
|
|
1662 |
* by a signal...
|
|
|
1663 |
*/
|
|
|
1664 |
timeout = nd_entry->time_15_char_;
|
|
|
1665 |
while ((res = select(nd_entry->fd+1, &rfds, NULL, NULL, &timeout)) < 0) {
|
|
|
1666 |
if (errno != EINTR)
|
|
|
1667 |
return -1;
|
|
|
1668 |
/* We will be calling select() again.
|
|
|
1669 |
* We need to reset the FD SET !
|
|
|
1670 |
*/
|
|
|
1671 |
FD_ZERO(&rfds);
|
|
|
1672 |
FD_SET(nd_entry->fd, &rfds);
|
|
|
1673 |
}
|
|
|
1674 |
|
|
|
1675 |
if (res == 0) {
|
|
|
1676 |
int frame_length = lb_data_count(&recv_buf->data_buf);
|
|
|
1677 |
/* We have detected an end of frame using timing boundaries... */
|
|
|
1678 |
recv_buf->found_frame_boundary = 1; /* => stop trying to read any more bytes! */
|
|
|
1679 |
|
|
|
1680 |
/* Let's check if we happen to have a correct frame... */
|
|
|
1681 |
if ((frame_length <= MAX_RTU_FRAME_LENGTH) &&
|
|
|
1682 |
(frame_length - RTU_FRAME_CRC_LENGTH > 0)) {
|
|
|
1683 |
if ( crc_calc(lb_data(&recv_buf->data_buf), frame_length - RTU_FRAME_CRC_LENGTH)
|
|
|
1684 |
== crc_read(lb_data(&recv_buf->data_buf), frame_length - RTU_FRAME_CRC_LENGTH)) {
|
|
|
1685 |
/* We have found a valid frame. Let's get out of here! */
|
|
|
1686 |
return return_frame(recv_buf, frame_length, recv_data_ptr);
|
|
|
1687 |
}
|
|
|
1688 |
}
|
|
|
1689 |
|
|
|
1690 |
/* We have detected a frame boundary, but the frame we read
|
|
|
1691 |
* is not valid...
|
|
|
1692 |
*
|
|
|
1693 |
* One of the following reasons must be the cause:
|
|
|
1694 |
* 1 - we are reading a single aborted frame.
|
|
|
1695 |
* 2 - we are reading more than one frame. The first frame,
|
|
|
1696 |
* followed by any number of valid and/or aborted frames,
|
|
|
1697 |
* may be one of:
|
|
|
1698 |
* a - a valid frame whose length is unknown to us,
|
|
|
1699 |
* i.e. it is not specified in the public Modbus spec.
|
|
|
1700 |
* b - an aborted frame.
|
|
|
1701 |
*
|
|
|
1702 |
* Due to the complexity of reading 2a as a correct frame, we will
|
|
|
1703 |
* consider it as an aborted frame. (NOTE: it is possible, but
|
|
|
1704 |
* we will ignore it until the need arises... hopefully, never!)
|
|
|
1705 |
*
|
|
|
1706 |
* To put it succintly, what wee now have is an 'aborted' frame
|
|
|
1707 |
* followed by one or more aborted and/or valid frames. To get to
|
|
|
1708 |
* any valid frames, and since we do not know where they begin,
|
|
|
1709 |
* we will have to consider every byte as the possible begining
|
|
|
1710 |
* of a valid frame. For this permutation, we ignore the first byte,
|
|
|
1711 |
* and carry on from there...
|
|
|
1712 |
*/
|
|
|
1713 |
found_aborted_frame = 1;
|
|
|
1714 |
lb_data_purge(&recv_buf->data_buf, 1 /* skip one byte */);
|
|
|
1715 |
recv_buf->frame_search_history = 0;
|
|
|
1716 |
}
|
|
|
1717 |
}
|
|
|
1718 |
|
|
|
1719 |
/*-------------------------------*
|
|
|
1720 |
* check for data yet to process *
|
|
|
1721 |
*-------------------------------*/
|
|
|
1722 |
if ((lb_data_count(&recv_buf->data_buf) < MIN_FRAME_LENGTH) &&
|
|
|
1723 |
(recv_buf->found_frame_boundary == 1)) {
|
|
|
1724 |
/* We have no more data to process, and will not read anymore! */
|
|
|
1725 |
recv_buf_reset(recv_buf);
|
|
|
1726 |
/* Return TIMEOUT error */
|
|
|
1727 |
return -2;
|
|
|
1728 |
}
|
|
|
1729 |
} /* while (1)*/
|
|
|
1730 |
|
|
|
1731 |
/* humour the compiler... */
|
|
|
1732 |
return -1;
|
|
|
1733 |
}
|
|
|
1734 |
|
|
|
1735 |
|
|
|
1736 |
|
|
|
1737 |
|
|
|
1738 |
|
|
|
1739 |
/************************************/
|
|
|
1740 |
/** **/
|
|
|
1741 |
/** Read a Modbus RTU frame **/
|
|
|
1742 |
/** **/
|
|
|
1743 |
/************************************/
|
|
|
1744 |
|
|
|
1745 |
/* The public function that reads a valid modbus frame.
|
|
|
1746 |
*
|
|
|
1747 |
* The returned frame is guaranteed to be different to the
|
|
|
1748 |
* the frame stored in send_data, and to start with the
|
|
|
1749 |
* same slave address stored in send_data[0].
|
|
|
1750 |
*
|
|
|
1751 |
* If send_data is NULL, send_data_length = 0, or
|
|
|
1752 |
* ignore_echo == 0, then the first valid frame read off
|
|
|
1753 |
* the bus is returned.
|
|
|
1754 |
*
|
|
|
1755 |
* return value: The length (in bytes) of the valid frame,
|
|
|
1756 |
* -1 on error
|
|
|
1757 |
* -2 on timeout
|
|
|
1758 |
*/
|
|
|
1759 |
|
|
|
1760 |
int modbus_rtu_read(int *nd,
|
|
|
1761 |
u8 **recv_data_ptr,
|
|
|
1762 |
u16 *transaction_id,
|
|
|
1763 |
const u8 *send_data,
|
|
|
1764 |
int send_length,
|
|
|
1765 |
const struct timespec *recv_timeout) {
|
|
|
1766 |
struct timespec end_time, *ts_ptr;
|
|
|
1767 |
int res, recv_length, iter;
|
|
|
1768 |
u8 *local_recv_data_ptr;
|
|
|
1769 |
u8 *slave_id, local_slave_id;
|
|
|
1770 |
nd_entry_t *nd_entry;
|
|
|
1771 |
|
|
|
1772 |
/* Check input parameters... */
|
|
|
1773 |
if (nd == NULL)
|
|
|
1774 |
return -1;
|
|
|
1775 |
|
|
|
1776 |
if (recv_data_ptr == NULL)
|
|
|
1777 |
recv_data_ptr = &local_recv_data_ptr;
|
|
|
1778 |
|
|
|
1779 |
if ((send_data == NULL) && (send_length != 0))
|
|
|
1780 |
return -1;
|
|
|
1781 |
|
|
|
1782 |
/* check if nd is correct... */
|
|
|
1783 |
if ((nd_entry = nd_table_get_nd(&nd_table_, *nd)) == NULL)
|
|
|
1784 |
return -1;
|
|
|
1785 |
|
|
|
1786 |
/* check if nd is initialzed... */
|
|
|
1787 |
if (nd_entry->fd < 0)
|
|
|
1788 |
return -1;
|
|
|
1789 |
|
|
|
1790 |
slave_id = NULL;
|
|
|
1791 |
if (send_length > L2_FRAME_SLAVEID_OFS) {
|
|
|
1792 |
local_slave_id = send_data[L2_FRAME_SLAVEID_OFS];
|
|
|
1793 |
slave_id = &local_slave_id;
|
|
|
1794 |
}
|
|
|
1795 |
|
|
|
1796 |
/* We will potentially read many frames, and we cannot reset the timeout
|
|
|
1797 |
* for every frame we read. We therefore determine the absolute time_out,
|
|
|
1798 |
* and use this as a parameter for each call to read_frame() instead of
|
|
|
1799 |
* using a relative timeout.
|
|
|
1800 |
*
|
|
|
1801 |
* NOTE: see also the timeout related comment in the read_frame()= function!
|
|
|
1802 |
*/
|
|
|
1803 |
/* get the current time... */
|
|
|
1804 |
ts_ptr = NULL;
|
|
|
1805 |
if (recv_timeout != NULL) {
|
|
|
1806 |
ts_ptr = &end_time;
|
|
|
1807 |
*ts_ptr = timespec_add_curtime(*recv_timeout);
|
|
|
1808 |
}
|
|
|
1809 |
|
|
|
1810 |
/* NOTE: When using a half-duplex RS-485 bus, some (most ?) RS232-485
|
|
|
1811 |
* converters will send back to the RS232 port whatever we write,
|
|
|
1812 |
* so we will read in whatever we write out onto the bus.
|
|
|
1813 |
* We will therefore have to compare
|
|
|
1814 |
* the first frame we read with the one we sent. If they are
|
|
|
1815 |
* identical it is because we are in fact working on a RS-485
|
|
|
1816 |
* bus and must therefore read in a second frame which will be
|
|
|
1817 |
* the true response to our query.
|
|
|
1818 |
* If the first frame we receive is different to the query we
|
|
|
1819 |
* just sent, then we are *not* working on a RS-485 bus, and
|
|
|
1820 |
* that is already the real response to our query.
|
|
|
1821 |
*
|
|
|
1822 |
* Flushing the input cache immediately after sending the query
|
|
|
1823 |
* could solve this issue, but we have no guarantee that this
|
|
|
1824 |
* process would not get swapped out between the write() and
|
|
|
1825 |
* flush() calls, and we could therefore be flushing the response
|
|
|
1826 |
* frame!
|
|
|
1827 |
*/
|
|
|
1828 |
|
|
|
1829 |
iter = 0;
|
|
|
1830 |
while ((res = recv_length = read_frame(nd_entry, recv_data_ptr, ts_ptr, slave_id)) >= 0) {
|
|
|
1831 |
if (iter < INT_MAX) iter++;
|
|
|
1832 |
|
|
|
1833 |
if ((send_length <= 0) || (nd_entry->ignore_echo == 0))
|
|
|
1834 |
/* any valid frame will do... */
|
|
|
1835 |
return recv_length;
|
|
|
1836 |
|
|
|
1837 |
if ((send_length > L2_FRAME_SLAVEID_OFS + 1) && (iter == 1))
|
|
|
1838 |
/* We have a frame in send_data,
|
|
|
1839 |
* so we must make sure we are not reading in the frame just sent...
|
|
|
1840 |
*
|
|
|
1841 |
* We must only do this for the first frame we read. Subsequent
|
|
|
1842 |
* frames are guaranteed not to be the previously sent frame
|
|
|
1843 |
* since the modbus_rtu_write() resets the recv buffer.
|
|
|
1844 |
* Remember too that valid modbus responses may be exactly the same
|
|
|
1845 |
* as the request frame!!
|
|
|
1846 |
*/
|
|
|
1847 |
if (recv_length == send_length)
|
|
|
1848 |
if (memcmp(*recv_data_ptr, send_data, recv_length) == 0)
|
|
|
1849 |
/* recv == send !!! */
|
|
|
1850 |
/* read in another frame. */
|
|
|
1851 |
continue;
|
|
|
1852 |
|
|
|
1853 |
/* The frame read is either:
|
|
|
1854 |
* - different to the frame in send_data
|
|
|
1855 |
* - or there is only the slave id in send_data[0]
|
|
|
1856 |
* - or both of the above...
|
|
|
1857 |
*/
|
|
|
1858 |
if (send_length > L2_FRAME_SLAVEID_OFS)
|
|
|
1859 |
if (recv_length > L2_FRAME_SLAVEID_OFS)
|
|
|
1860 |
/* check that frame is from/to the correct slave... */
|
|
|
1861 |
if ((*recv_data_ptr)[L2_FRAME_SLAVEID_OFS] == send_data[L2_FRAME_SLAVEID_OFS])
|
|
|
1862 |
/* yep, it is... */
|
|
|
1863 |
return recv_length;
|
|
|
1864 |
|
|
|
1865 |
/* The frame we have received is not acceptable...
|
|
|
1866 |
* Let's read a new frame.
|
|
|
1867 |
*/
|
|
|
1868 |
} /* while(...) */
|
|
|
1869 |
|
|
|
1870 |
/* error reading response! */
|
|
|
1871 |
/* Return the error returned by read_frame! */
|
|
|
1872 |
return res;
|
|
|
1873 |
}
|
|
|
1874 |
|
|
|
1875 |
|
|
|
1876 |
|
|
|
1877 |
|
|
|
1878 |
|
|
|
1879 |
/**************************************************************/
|
|
|
1880 |
/**************************************************************/
|
|
|
1881 |
/**** ****/
|
|
|
1882 |
/**** ****/
|
|
|
1883 |
/**** Initialising and Shutting Down Library ****/
|
|
|
1884 |
/**** ****/
|
|
|
1885 |
/**** ****/
|
|
|
1886 |
/**************************************************************/
|
|
|
1887 |
/**************************************************************/
|
|
|
1888 |
|
|
|
1889 |
/******************************/
|
|
|
1890 |
/** **/
|
|
|
1891 |
/** Load Default Values **/
|
|
|
1892 |
/** **/
|
|
|
1893 |
/******************************/
|
|
|
1894 |
|
|
|
1895 |
static void set_defaults(int *baud,
|
|
|
1896 |
int *parity,
|
|
|
1897 |
int *data_bits,
|
|
|
1898 |
int *stop_bits) {
|
|
|
1899 |
/* Set the default values, if required... */
|
|
|
1900 |
if (*baud == 0)
|
|
|
1901 |
*baud = DEF_BAUD_RATE;
|
|
|
1902 |
if (*data_bits == 0)
|
|
|
1903 |
*data_bits = DEF_DATA_BITS;
|
|
|
1904 |
if (*stop_bits == 0) {
|
|
|
1905 |
if (*parity == 0)
|
|
|
1906 |
*stop_bits = DEF_STOP_BITS_NOP; /* no parity */
|
|
|
1907 |
else
|
|
|
1908 |
*stop_bits = DEF_STOP_BITS_PAR; /* parity used */
|
|
|
1909 |
}
|
|
|
1910 |
}
|
|
|
1911 |
|
|
|
1912 |
|
|
|
1913 |
/******************************/
|
|
|
1914 |
/** **/
|
|
|
1915 |
/** Initialise Library **/
|
|
|
1916 |
/** **/
|
|
|
1917 |
/******************************/
|
|
|
1918 |
|
|
|
1919 |
int modbus_rtu_init(int nd_count,
|
|
|
1920 |
optimization_t opt,
|
|
|
1921 |
int *extra_bytes)
|
|
|
1922 |
{
|
|
|
1923 |
#ifdef DEBUG
|
|
|
1924 |
fprintf(stderr, "modbus_rtu_init(): called...\n");
|
|
|
1925 |
fprintf(stderr, "creating %d node descriptors\n", nd_count);
|
|
|
1926 |
if (opt == optimize_speed)
|
|
|
1927 |
fprintf(stderr, "optimizing for speed\n");
|
|
|
1928 |
if (opt == optimize_size)
|
|
|
1929 |
fprintf(stderr, "optimizing for size\n");
|
|
|
1930 |
#endif
|
|
|
1931 |
|
|
|
1932 |
/* check input parameters...*/
|
|
|
1933 |
if (0 == nd_count) {
|
|
|
1934 |
if (extra_bytes != NULL)
|
|
|
1935 |
// Not the corect value for this layer.
|
|
|
1936 |
// What we set it to in case this layer is not used!
|
|
|
1937 |
*extra_bytes = 0;
|
|
|
1938 |
return 0;
|
|
|
1939 |
}
|
|
|
1940 |
if (nd_count <= 0)
|
|
|
1941 |
goto error_exit_0;
|
|
|
1942 |
|
|
|
1943 |
if (extra_bytes == NULL)
|
|
|
1944 |
goto error_exit_0;
|
|
|
1945 |
|
|
|
1946 |
if (crc_init(opt) < 0) {
|
|
|
1947 |
#ifdef ERRMSG
|
|
|
1948 |
fprintf(stderr, ERRMSG_HEAD "Out of memory: error initializing crc buffers\n");
|
|
|
1949 |
#endif
|
|
|
1950 |
goto error_exit_0;
|
|
|
1951 |
}
|
|
|
1952 |
|
|
|
1953 |
/* set the extra_bytes value... */
|
|
|
1954 |
/* Please see note before the modbus_rtu_write() function for a
|
|
|
1955 |
* better understanding of this extremely ugly hack...
|
|
|
1956 |
*
|
|
|
1957 |
* The number of extra bytes that must be allocated to the data buffer
|
|
|
1958 |
* before calling modbus_rtu_write()
|
|
|
1959 |
*/
|
|
|
1960 |
*extra_bytes = RTU_FRAME_CRC_LENGTH;
|
|
|
1961 |
|
|
|
1962 |
/* initialise nd table... */
|
|
|
1963 |
if (nd_table_init(&nd_table_, nd_count) < 0)
|
|
|
1964 |
goto error_exit_0;
|
|
|
1965 |
|
|
|
1966 |
/* remember the optimization choice for later reference... */
|
|
|
1967 |
optimization_ = opt;
|
|
|
1968 |
|
|
|
1969 |
#ifdef DEBUG
|
|
|
1970 |
fprintf(stderr, "modbus_rtu_init(): returning succesfuly...\n");
|
|
|
1971 |
#endif
|
|
|
1972 |
return 0;
|
|
|
1973 |
|
|
|
1974 |
error_exit_0:
|
|
|
1975 |
if (extra_bytes != NULL)
|
|
|
1976 |
// Not the corect value for this layer.
|
|
|
1977 |
// What we set it to in case of error!
|
|
|
1978 |
*extra_bytes = 0;
|
|
|
1979 |
return -1;
|
|
|
1980 |
}
|
|
|
1981 |
|
|
|
1982 |
|
|
|
1983 |
|
|
|
1984 |
/******************************/
|
|
|
1985 |
/** **/
|
|
|
1986 |
/** Open node descriptor **/
|
|
|
1987 |
/** **/
|
|
|
1988 |
/******************************/
|
|
|
1989 |
|
|
|
1990 |
/* Open a node for master or slave operation.
|
|
|
1991 |
* Returns the node descriptor, or -1 on error.
|
|
|
1992 |
*
|
|
|
1993 |
* This function is mapped onto both
|
|
|
1994 |
* modbus_connect() and modbus_listen()
|
|
|
1995 |
*/
|
|
|
1996 |
int modbus_rtu_connect(node_addr_t node_addr) {
|
|
|
1997 |
int node_descriptor;
|
|
|
1998 |
nd_entry_t *nd_entry;
|
|
|
1999 |
|
|
|
2000 |
#ifdef DEBUG
|
|
|
2001 |
fprintf(stderr, "modbus_rtu_connect(): called...\n");
|
|
|
2002 |
fprintf(stderr, "opening %s\n", node_addr.addr.rtu.device);
|
|
|
2003 |
fprintf(stderr, "baud_rate = %d\n", node_addr.addr.rtu.baud);
|
|
|
2004 |
fprintf(stderr, "parity = %d\n", node_addr.addr.rtu.parity);
|
|
|
2005 |
fprintf(stderr, "data_bits = %d\n", node_addr.addr.rtu.data_bits);
|
|
|
2006 |
fprintf(stderr, "stop_bits = %d\n", node_addr.addr.rtu.stop_bits);
|
|
|
2007 |
fprintf(stderr, "ignore_echo = %d\n", node_addr.addr.rtu.ignore_echo);
|
|
|
2008 |
#endif
|
|
|
2009 |
|
|
|
2010 |
/* Check for valid address family */
|
|
|
2011 |
if (node_addr.naf != naf_rtu)
|
|
|
2012 |
/* wrong address type... */
|
|
|
2013 |
goto error_exit_0;
|
|
|
2014 |
|
|
|
2015 |
/* find a free node descriptor */
|
|
|
2016 |
if ((node_descriptor = nd_table_get_free_nd(&nd_table_)) < 0)
|
|
|
2017 |
/* if no free nodes to initialize, then we are finished... */
|
|
|
2018 |
goto error_exit_0;
|
|
|
2019 |
if ((nd_entry = nd_table_get_nd(&nd_table_, node_descriptor)) == NULL)
|
|
|
2020 |
/* strange, this should not occur... */
|
|
|
2021 |
goto error_exit_0;
|
|
|
2022 |
|
|
|
2023 |
/* set the default values... */
|
|
|
2024 |
set_defaults(&(node_addr.addr.rtu.baud),
|
|
|
2025 |
&(node_addr.addr.rtu.parity),
|
|
|
2026 |
&(node_addr.addr.rtu.data_bits),
|
|
|
2027 |
&(node_addr.addr.rtu.stop_bits));
|
|
|
2028 |
|
|
|
2029 |
#ifdef DEBUG
|
|
|
2030 |
fprintf(stderr, "modbus_rtu_connect(): calling nd_entry_connect()\n");
|
|
|
2031 |
#endif
|
|
|
2032 |
if (nd_entry_connect(nd_entry, &node_addr, optimization_) < 0)
|
|
|
2033 |
goto error_exit_0;
|
|
|
2034 |
|
|
|
2035 |
#ifdef DEBUG
|
|
|
2036 |
fprintf(stderr, "modbus_rtu_connect(): %s open\n", node_addr.addr.rtu.device);
|
|
|
2037 |
fprintf(stderr, "modbus_rtu_connect(): returning nd=%d\n", node_descriptor);
|
|
|
2038 |
#endif
|
|
|
2039 |
return node_descriptor;
|
|
|
2040 |
|
|
|
2041 |
error_exit_0:
|
|
|
2042 |
#ifdef DEBUG
|
|
|
2043 |
fprintf(stderr, "modbus_rtu_connect(): error!\n");
|
|
|
2044 |
#endif
|
|
|
2045 |
return -1;
|
|
|
2046 |
}
|
|
|
2047 |
|
|
|
2048 |
|
|
|
2049 |
|
|
|
2050 |
int modbus_rtu_listen(node_addr_t node_addr) {
|
|
|
2051 |
return modbus_rtu_connect(node_addr);
|
|
|
2052 |
}
|
|
|
2053 |
|
|
|
2054 |
|
|
|
2055 |
|
|
|
2056 |
/******************************/
|
|
|
2057 |
/** **/
|
|
|
2058 |
/** Close node descriptor **/
|
|
|
2059 |
/** **/
|
|
|
2060 |
/******************************/
|
|
|
2061 |
|
|
|
2062 |
int modbus_rtu_close(int nd) {
|
|
|
2063 |
return nd_table_free_nd(&nd_table_, nd);
|
|
|
2064 |
}
|
|
|
2065 |
|
|
|
2066 |
|
|
|
2067 |
|
|
|
2068 |
/******************************/
|
|
|
2069 |
/** **/
|
|
|
2070 |
/** Shutdown Library **/
|
|
|
2071 |
/** **/
|
|
|
2072 |
/******************************/
|
|
|
2073 |
|
|
|
2074 |
int modbus_rtu_done(void) {
|
|
|
2075 |
nd_table_done(&nd_table_);
|
|
|
2076 |
crc_done();
|
|
|
2077 |
|
|
|
2078 |
return 0;
|
|
|
2079 |
}
|
|
|
2080 |
|
|
|
2081 |
|
|
|
2082 |
|
|
|
2083 |
|
|
|
2084 |
/******************************/
|
|
|
2085 |
/** **/
|
|
|
2086 |
/** **/
|
|
|
2087 |
/** **/
|
|
|
2088 |
/******************************/
|
|
|
2089 |
int modbus_rtu_silence_init(void) {
|
|
|
2090 |
return 0;
|
|
|
2091 |
}
|
|
|
2092 |
|
|
|
2093 |
|
|
|
2094 |
|
|
|
2095 |
|
|
|
2096 |
/******************************/
|
|
|
2097 |
/** **/
|
|
|
2098 |
/** **/
|
|
|
2099 |
/** **/
|
|
|
2100 |
/******************************/
|
|
|
2101 |
|
|
|
2102 |
|
|
|
2103 |
double modbus_rtu_get_min_timeout(int baud,
|
|
|
2104 |
int parity,
|
|
|
2105 |
int data_bits,
|
|
|
2106 |
int stop_bits) {
|
|
|
2107 |
int parity_bits, start_bits, char_bits;
|
|
|
2108 |
|
|
|
2109 |
set_defaults(&baud, &parity, &data_bits, &stop_bits);
|
|
|
2110 |
parity_bits = (parity == 0)?0:1;
|
|
|
2111 |
start_bits = 1;
|
|
|
2112 |
char_bits = start_bits + data_bits + parity_bits + stop_bits;
|
|
|
2113 |
return (double)((MAX_RTU_FRAME_LENGTH * char_bits) / baud);
|
|
|
2114 |
}
|
|
|
2115 |
|
|
|
2116 |
|