|
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 /* mb_slave.c */ |
|
25 |
|
26 #include <fcntl.h> /* File control definitions */ |
|
27 #include <stdio.h> /* Standard input/output */ |
|
28 #include <string.h> |
|
29 #include <stdlib.h> |
|
30 #include <termio.h> /* POSIX terminal control definitions */ |
|
31 #include <sys/time.h> /* Time structures for select() */ |
|
32 #include <unistd.h> /* POSIX Symbolic Constants */ |
|
33 #include <errno.h> /* Error definitions */ |
|
34 |
|
35 #include <netinet/in.h> /* required for htons() and ntohs() */ |
|
36 #include "mb_layer1.h" |
|
37 #include "mb_slave.h" |
|
38 #include "mb_slave_private.h" |
|
39 |
|
40 /* #define DEBUG */ /* uncomment to see the data sent and received */ |
|
41 |
|
42 |
|
43 #define modbus_write fptr_[layer1_fin].modbus_write |
|
44 #define modbus_read fptr_[layer1_fin].modbus_read |
|
45 #define modbus_init fptr_[layer1_fin].modbus_init |
|
46 #define modbus_done fptr_[layer1_fin].modbus_done |
|
47 #define modbus_connect fptr_[layer1_fin].modbus_connect |
|
48 #define modbus_listen fptr_[layer1_fin].modbus_listen |
|
49 #define modbus_close fptr_[layer1_fin].modbus_close |
|
50 #define modbus_silence_init fptr_[layer1_fin].modbus_silence_init |
|
51 #define modbus_get_min_timeout fptr_[layer1_fin].modbus_get_min_timeout |
|
52 |
|
53 /* the lower two bits of ttyfd are used to store the index to layer1 function pointers */ |
|
54 /* layer1_fin index to fptr_[] is in lowest 2 bits of fd */ |
|
55 #define get_ttyfd() int layer1_fin = fd & 3; int ttyfd = fd / 4;\ |
|
56 if (fd < 0) {ttyfd = fd; layer1_fin = 0; /* use modbusTCP */} |
|
57 |
|
58 |
|
59 |
|
60 |
|
61 /******************************************/ |
|
62 /******************************************/ |
|
63 /** **/ |
|
64 /** Global Variables... **/ |
|
65 /** **/ |
|
66 /******************************************/ |
|
67 /******************************************/ |
|
68 /* The layer 1 (RTU, ASCII, TCP) implementations will be adding some |
|
69 * header and tail bytes (e.g. CRC) to the packet we build here. Since |
|
70 * layer1 will re-use the same buffer allocated in this slave layer |
|
71 * (so as not to continuosly copy the same info from buffer to buffer), |
|
72 * we need to allocate more bytes than those strictly required for this |
|
73 * slave layer. Therefore, the extra_bytes parameter. |
|
74 * |
|
75 * Note that we add one more extra byte to the response buffer. |
|
76 * This is because some response packets will not be starting off |
|
77 * at byte 0, but rather at byte 1 of the buffer. This is in order |
|
78 * to guarantee that the data that is sent on the buffer is aligned |
|
79 * on even bytes (the 16 bit words!). This will allow the application |
|
80 * (layer above the one implemented in this file - i.e. the callback |
|
81 * functions) to reference this memory as an u16 *, without producing |
|
82 * 'bus error' messages in some embedded devices that do not allow |
|
83 * acessing u16 on odd numbered addresses. |
|
84 */ |
|
85 static int buff_extra_bytes_; |
|
86 #define RESP_BUFFER_SIZE (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1) |
|
87 |
|
88 /******************************************/ |
|
89 /******************************************/ |
|
90 /** **/ |
|
91 /** Local Utility functions... **/ |
|
92 /** **/ |
|
93 /******************************************/ |
|
94 /******************************************/ |
|
95 |
|
96 |
|
97 /* |
|
98 * Function to determine next transaction id. |
|
99 * |
|
100 * We use a library wide transaction id, which means that we |
|
101 * use a new transaction id no matter what slave to which we will |
|
102 * be sending the request... |
|
103 */ |
|
104 static inline u16 next_transaction_id(void) { |
|
105 static u16 next_id = 0; |
|
106 return next_id++; |
|
107 } |
|
108 |
|
109 |
|
110 /* |
|
111 * Functions to convert u16 variables |
|
112 * between network and host byte order |
|
113 * |
|
114 * NOTE: Modbus uses MSByte first, just like |
|
115 * tcp/ip, so we could be tempted to use the htons() and |
|
116 * ntohs() functions to guarantee code portability. |
|
117 * |
|
118 * However, on some embedded systems running Linux |
|
119 * these functions only work if the 16 bit words are |
|
120 * stored on even addresses. This is not always the |
|
121 * case in our code, so we have to define our own |
|
122 * conversion functions... |
|
123 */ |
|
124 |
|
125 /* if using gcc, use it to determine byte order... */ |
|
126 #ifndef __BYTE_ORDER |
|
127 #if defined(__GNUC__) |
|
128 /* We have GCC, which should define __LITTLE_ENDIAN__ */ |
|
129 # if defined(__LITTLE_ENDIAN__) |
|
130 # define __BYTE_ORDER __LITTLE_ENDIAN |
|
131 # else |
|
132 # define __BYTE_ORDER __BIG_ENDIAN |
|
133 # endif |
|
134 #endif /* __GNUC__ */ |
|
135 #endif /* __BYTE_ORDER */ |
|
136 |
|
137 |
|
138 /* If we still don't know byte order, try to get it from <sys/param.h> */ |
|
139 #ifndef __BYTE_ORDER |
|
140 #include <sys/param.h> |
|
141 #endif |
|
142 |
|
143 |
|
144 #ifndef __BYTE_ORDER |
|
145 # ifdef BYTE_ORDER |
|
146 # if BYTE_ORDER == LITTLE_ENDIAN |
|
147 # define __BYTE_ORDER __LITTLE_ENDIAN |
|
148 # else |
|
149 # if BYTE_ORDER == BIG_ENDIAN |
|
150 # define __BYTE_ORDER __BIG_ENDIAN |
|
151 # endif |
|
152 # endif |
|
153 # endif /* BYTE_ORDER */ |
|
154 #endif /* __BYTE_ORDER */ |
|
155 |
|
156 |
|
157 |
|
158 |
|
159 |
|
160 #ifdef __BYTE_ORDER |
|
161 # if __BYTE_ORDER == __LITTLE_ENDIAN |
|
162 |
|
163 /**************************************************************/ |
|
164 /* u16 conversion functions to use on little endian platforms */ |
|
165 /**************************************************************/ |
|
166 |
|
167 static inline u16 mb_hton(u16 w) { |
|
168 register u16 tmp; |
|
169 tmp = (w & 0x00FF); |
|
170 tmp = ((w & 0xFF00) >> 0x08) | (tmp << 0x08); |
|
171 return(tmp); |
|
172 } |
|
173 #define mb_ntoh(a) mb_hton(a) |
|
174 |
|
175 static inline void mb_hton_count(u16 *w, int count) { |
|
176 int i; |
|
177 for (i = 0; i < count; i++) { |
|
178 /* swap the bytes around... |
|
179 * a = a ^ b; |
|
180 * b = a ^ b; |
|
181 * a = a ^ b; |
|
182 */ |
|
183 ((u8 *)(w+i))[0] ^= ((u8 *)(w+i))[1]; |
|
184 ((u8 *)(w+i))[1] ^= ((u8 *)(w+i))[0]; |
|
185 ((u8 *)(w+i))[0] ^= ((u8 *)(w+i))[1]; |
|
186 } |
|
187 } |
|
188 #define mb_ntoh_count(w, count) mb_hton_count(w, count) |
|
189 |
|
190 |
|
191 |
|
192 # else |
|
193 # if __BYTE_ORDER == __BIG_ENDIAN |
|
194 /***********************************************************/ |
|
195 /* u16 conversion functions to use on big endian platforms */ |
|
196 /***********************************************************/ |
|
197 |
|
198 /* We do not need to swap the bytes around! */ |
|
199 #define mb_ntoh(val) (val) |
|
200 #define mb_hton(val) (val) |
|
201 #define mb_hton_count(w, count) /* empty ! */ |
|
202 #define mb_ntoh_count(w, count) /* empty ! */ |
|
203 |
|
204 |
|
205 # else |
|
206 |
|
207 /********************************************************/ |
|
208 /* u16 conversion functions to use on generic platforms */ |
|
209 /********************************************************/ |
|
210 |
|
211 /* We don't know the byte order, so we revert to the |
|
212 * standard htons() and ntohs() ... |
|
213 */ |
|
214 static inline u16 mb_hton(u16 h_value) |
|
215 {return htons(h_value); /* return h_value; */} |
|
216 |
|
217 static inline u16 mb_ntoh(u16 m_value) |
|
218 {return ntohs(m_value); /* return m_value; */} |
|
219 |
|
220 static inline void mb_hton_count(u16 *w, int count) |
|
221 {int i; for (i = 0; i < count; i++) {w[i] = mb_hton(w[i]);}} |
|
222 |
|
223 static inline void mb_ntoh_count(u16 *w, int count) |
|
224 {int i; for (i = 0; i < count; i++) {w[i] = mb_ntoh(w[i]);}} |
|
225 |
|
226 # endif |
|
227 # endif |
|
228 #endif /* __BYTE_ORDER */ |
|
229 |
|
230 |
|
231 |
|
232 |
|
233 /* Safe versions of the conversion functions! |
|
234 * |
|
235 * Note that these functions always work, whatever the endiannes |
|
236 * of the machine that executes it! |
|
237 * |
|
238 * It is also safe because the resulting value may be stored |
|
239 * on an odd address even on machines that do not allow directly |
|
240 * accessing u16 bit words on odd addresses. |
|
241 */ |
|
242 static inline int mb_hton_safe(u16 from, u16 *to_ptr) { |
|
243 ((u8 *)to_ptr)[1] = (from & 0x00FF); |
|
244 ((u8 *)to_ptr)[0] = ((from & 0xFF00) >> 0x08); |
|
245 return 0; |
|
246 } |
|
247 |
|
248 #define mb_ntoh_safe(a, b) mb_hton_safe(a, b) |
|
249 |
|
250 |
|
251 /* return Most Significant Byte of value; */ |
|
252 static inline u8 msb(u16 value) |
|
253 {return (value >> 8) & 0xFF;} |
|
254 |
|
255 /* return Least Significant Byte of value; */ |
|
256 static inline u8 lsb(u16 value) |
|
257 {return value & 0xFF;} |
|
258 |
|
259 #define u16_v(char_ptr) (*((u16 *)(&(char_ptr)))) |
|
260 |
|
261 |
|
262 |
|
263 |
|
264 |
|
265 |
|
266 |
|
267 |
|
268 |
|
269 /***********************************************/ |
|
270 /***********************************************/ |
|
271 /** **/ |
|
272 /** Handle requests from master/client **/ |
|
273 /** **/ |
|
274 /***********************************************/ |
|
275 /***********************************************/ |
|
276 |
|
277 |
|
278 /* Handle functions 0x01 and 0x02 */ |
|
279 typedef int (*read_bits_callback_t)(void *arg, u16 start_addr, u16 bit_count, u8 *data_bytes); |
|
280 static int handle_read_bits (u8 *query_packet, |
|
281 u8 **resp_packet_ptr, |
|
282 u8 *error_code, |
|
283 read_bits_callback_t read_bits_callback, |
|
284 void *callback_arg |
|
285 ) { |
|
286 u16 start_addr, count; |
|
287 int res; |
|
288 u8 *resp_packet; |
|
289 |
|
290 /* If no callback, handle as if function is not supported... */ |
|
291 if (read_bits_callback == NULL) |
|
292 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
|
293 |
|
294 /* in oprder for the data in this packet to be aligned on even numbered addresses, this |
|
295 * response packet will start off at an odd numbered byte... |
|
296 * We therefore add 1 to the address where the packet starts. |
|
297 */ |
|
298 (*resp_packet_ptr)++; |
|
299 resp_packet = *resp_packet_ptr; |
|
300 |
|
301 /* NOTE: |
|
302 * Modbus uses high level addressing starting off from 1, but |
|
303 * this is sent as 0 on the wire! |
|
304 * We could expect the user to specify high level addressing |
|
305 * starting at 1, and do the conversion to start off at 0 here. |
|
306 * However, to do this we would then need to use an u32 data type |
|
307 * to correctly hold the address supplied by the user (which could |
|
308 * correctly be 65536, which does not fit in an u16), which would |
|
309 * in turn require us to check whether the address supplied by the user |
|
310 * is correct (i.e. <= 65536). |
|
311 * I decided to go with the other option of using an u16, and |
|
312 * requiring the user to use addressing starting off at 0! |
|
313 */ |
|
314 /* start_addr = mb_ntoh(u16_v(query_packet[2])) + 1; */ |
|
315 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
|
316 mb_ntoh_safe(u16_v(query_packet[4]), &count); |
|
317 |
|
318 #ifdef DEBUG |
|
319 printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
|
320 query_packet[0], query_packet[1], start_addr, count); |
|
321 #endif |
|
322 |
|
323 if ((count > MAX_READ_BITS) || (count < 1)) |
|
324 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
|
325 |
|
326 /* Remember, we are using addressing starting off at 0, in the start_addr variable! */ |
|
327 /* This means that he highest acceptable address is 65535, when count=1 .... */ |
|
328 /* Note the use of 65536 in the comparison will force automatic upgrade of u16 variables! */ |
|
329 /* => start_addr + count will nver overflow the u16 type! */ |
|
330 if (start_addr + count > 65536) |
|
331 {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
332 |
|
333 /* start building response frame... */ |
|
334 resp_packet[0] = query_packet[0]; /* slave */ |
|
335 resp_packet[1] = query_packet[1]; /* function (either 0x01 or 0x02 ! */ |
|
336 resp_packet[2] = (count + 7) / 8; /* number of data bytes = ceil(count/8) */ |
|
337 |
|
338 res = read_bits_callback(callback_arg, start_addr, count, &(resp_packet[3])); |
|
339 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
340 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
|
341 |
|
342 return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */ |
|
343 } |
|
344 |
|
345 |
|
346 |
|
347 /* Handle function 0x01 */ |
|
348 int handle_read_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
349 {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_outbits, callbacks->arg);} |
|
350 |
|
351 /* Handle function 0x02 */ |
|
352 int handle_read_input_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
353 {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_inbits, callbacks->arg);} |
|
354 |
|
355 |
|
356 |
|
357 |
|
358 /* Handle functions 0x03 and 0x04 */ |
|
359 typedef int (*read_words_callback_t)(void *arg, u16 start_addr, u16 word_count, u16 *data_words); |
|
360 static int handle_read_words (u8 *query_packet, |
|
361 u8 **resp_packet_ptr, |
|
362 u8 *error_code, |
|
363 read_words_callback_t read_words_callback, |
|
364 void *callback_arg |
|
365 ) { |
|
366 u16 start_addr, count; |
|
367 int res; |
|
368 u8 *resp_packet; |
|
369 |
|
370 /* If no callback, handle as if function is not supported... */ |
|
371 if (read_words_callback == NULL) |
|
372 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
|
373 |
|
374 /* See equivalent comment in handle_read_bits() */ |
|
375 (*resp_packet_ptr)++; |
|
376 resp_packet = *resp_packet_ptr; |
|
377 |
|
378 /* See equivalent comment in handle_read_bits() */ |
|
379 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
|
380 mb_ntoh_safe(u16_v(query_packet[4]), &count); |
|
381 |
|
382 #ifdef DEBUG |
|
383 printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
|
384 query_packet[0], query_packet[1], start_addr, count); |
|
385 #endif |
|
386 |
|
387 if ((count > MAX_READ_REGS) || (count < 1)) |
|
388 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
|
389 |
|
390 /* See equivalent comment in handle_read_bits() */ |
|
391 if (start_addr + count > 65536) |
|
392 {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
393 |
|
394 /* start building response frame... */ |
|
395 resp_packet[0] = query_packet[0]; /* slave */ |
|
396 resp_packet[1] = query_packet[1]; /* function code, either 0x03 or 0x04 !!!*/ |
|
397 resp_packet[2] = count * 2; /* number of bytes of data... */ |
|
398 |
|
399 res = read_words_callback(callback_arg, start_addr, count, (u16 *)&(resp_packet[3])); |
|
400 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
401 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
|
402 |
|
403 /* convert all data from host to network byte order. */ |
|
404 mb_hton_count((u16 *)&(resp_packet[3]), count); |
|
405 |
|
406 return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */ |
|
407 } |
|
408 |
|
409 |
|
410 |
|
411 |
|
412 /* Handle function 0x03 */ |
|
413 int handle_read_output_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
414 {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_outwords, callbacks->arg);} |
|
415 |
|
416 /* Handle function 0x04 */ |
|
417 int handle_read_input_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
418 {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_inwords, callbacks->arg);} |
|
419 |
|
420 |
|
421 |
|
422 /* Handle function 0x05 */ |
|
423 int handle_write_output_bit (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) { |
|
424 u16 start_addr; |
|
425 int res; |
|
426 u8 *resp_packet; |
|
427 |
|
428 /* If no callback, handle as if function is not supported... */ |
|
429 if (callbacks->write_outbits == NULL) |
|
430 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
|
431 |
|
432 resp_packet = *resp_packet_ptr; |
|
433 |
|
434 /* See equivalent comment in handle_read_bits() */ |
|
435 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
|
436 |
|
437 #ifdef DEBUG |
|
438 printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n", |
|
439 query_packet[0], query_packet[1], start_addr); |
|
440 #endif |
|
441 |
|
442 // byte 5 Must be 0x00, byte 4 must be 0x00 or 0xFF !! |
|
443 if ( (query_packet[5] != 0) || |
|
444 ((query_packet[4] != 0) && (query_packet[4] != 0xFF))) |
|
445 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
|
446 |
|
447 /* Address will always be valid, no need to check! */ |
|
448 // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
449 |
|
450 /* start building response frame... */ |
|
451 resp_packet[0] = query_packet[0]; /* slave */ |
|
452 resp_packet[1] = query_packet[1]; /* function */ |
|
453 resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
454 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
455 resp_packet[4] = query_packet[4]; /* value: 0x00 or 0xFF */ |
|
456 resp_packet[5] = query_packet[5]; /* value: must be 0x00 */ |
|
457 |
|
458 res = (callbacks->write_outbits)(callbacks->arg, start_addr, 1, &(query_packet[4])); |
|
459 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
460 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
|
461 |
|
462 return 6; /* response packet size, including slave id in byte 0 */ |
|
463 } |
|
464 |
|
465 |
|
466 |
|
467 /* Handle function 0x06 */ |
|
468 int handle_write_output_word (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) { |
|
469 u16 start_addr; |
|
470 int res; |
|
471 u8 *resp_packet; |
|
472 |
|
473 /* If no callback, handle as if function is not supported... */ |
|
474 if (callbacks->write_outwords == NULL) |
|
475 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
|
476 |
|
477 resp_packet = *resp_packet_ptr; |
|
478 |
|
479 /* See equivalent comment in handle_read_bits() */ |
|
480 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
|
481 |
|
482 #ifdef DEBUG |
|
483 printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n", |
|
484 query_packet[0], query_packet[1], start_addr); |
|
485 #endif |
|
486 |
|
487 /* Address will always be valid, no need to check! */ |
|
488 // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
489 |
|
490 /* start building response frame... */ |
|
491 resp_packet[0] = query_packet[0]; /* slave */ |
|
492 resp_packet[1] = query_packet[1]; /* function */ |
|
493 resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
494 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
495 resp_packet[4] = query_packet[4]; /* value - hi byte */ |
|
496 resp_packet[5] = query_packet[5]; /* value - lo byte */ |
|
497 |
|
498 /* convert data from network to host byte order */ |
|
499 mb_ntoh_count((u16 *)&(query_packet[4]), 1); |
|
500 |
|
501 res = (callbacks->write_outwords)(callbacks->arg, start_addr, 1, (u16 *)&(query_packet[4])); |
|
502 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
503 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
|
504 |
|
505 return 6; /* packet size is 6 -> slave, function, addr(2), value(2) */ |
|
506 } |
|
507 |
|
508 |
|
509 |
|
510 /* Handle function 0x0F */ |
|
511 int handle_write_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) { |
|
512 u16 start_addr, count; |
|
513 int res; |
|
514 u8 *resp_packet; |
|
515 |
|
516 /* If no callback, handle as if function is not supported... */ |
|
517 if (callbacks->write_outbits == NULL) |
|
518 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
|
519 |
|
520 resp_packet = *resp_packet_ptr; |
|
521 |
|
522 /* See equivalent comment in handle_read_bits() */ |
|
523 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
|
524 mb_ntoh_safe(u16_v(query_packet[4]), &count); |
|
525 |
|
526 #ifdef DEBUG |
|
527 printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", |
|
528 query_packet[0], query_packet[1], start_addr, count); |
|
529 #endif |
|
530 |
|
531 if ((count > MAX_WRITE_COILS) || (count < 1) || ((count+7)/8 != query_packet[6]) ) |
|
532 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
|
533 |
|
534 /* See equivalent comment in handle_read_bits() */ |
|
535 if (start_addr + count > 65536) |
|
536 {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
537 |
|
538 /* start building response frame... */ |
|
539 resp_packet[0] = query_packet[0]; /* slave */ |
|
540 resp_packet[1] = query_packet[1]; /* function */ |
|
541 resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
542 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
543 resp_packet[4] = query_packet[4]; /* count - hi byte */ |
|
544 resp_packet[5] = query_packet[5]; /* count - lo byte */ |
|
545 |
|
546 res = (callbacks->write_outbits)(callbacks->arg, start_addr, count, &(query_packet[7])); |
|
547 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
548 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
|
549 |
|
550 return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */ |
|
551 } |
|
552 |
|
553 |
|
554 |
|
555 |
|
556 /* Handle function 0x10 */ |
|
557 int handle_write_output_words(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) { |
|
558 u16 start_addr, count; |
|
559 int res; |
|
560 u8 *resp_packet; |
|
561 |
|
562 /* If no callback, handle as if function is not supported... */ |
|
563 if (callbacks->write_outwords == NULL) |
|
564 {*error_code = ERR_ILLEGAL_FUNCTION; return -1;} |
|
565 |
|
566 resp_packet = *resp_packet_ptr; |
|
567 |
|
568 /* See equivalent comment in handle_read_bits() */ |
|
569 mb_ntoh_safe(u16_v(query_packet[2]), &start_addr); |
|
570 mb_ntoh_safe(u16_v(query_packet[4]), &count); |
|
571 |
|
572 if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) ) |
|
573 {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;} |
|
574 |
|
575 /* See equivalent comment in handle_read_bits() */ |
|
576 if (start_addr + count > 65536) |
|
577 {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
578 |
|
579 /* start building response frame... */ |
|
580 resp_packet[0] = query_packet[0]; /* slave */ |
|
581 resp_packet[1] = query_packet[1]; /* function */ |
|
582 resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
583 resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
584 resp_packet[4] = query_packet[4]; /* count - hi byte */ |
|
585 resp_packet[5] = query_packet[5]; /* count - lo byte */ |
|
586 |
|
587 /* convert all data from network to host byte order */ |
|
588 mb_ntoh_count((u16 *)&(query_packet[7]), count); |
|
589 |
|
590 res = (callbacks->write_outwords)(callbacks->arg, start_addr, count, (u16 *)&(query_packet[7])); |
|
591 if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;} |
|
592 if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;} |
|
593 |
|
594 return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */ |
|
595 } |
|
596 |
|
597 |
|
598 |
|
599 |
|
600 |
|
601 |
|
602 |
|
603 |
|
604 /***********************************************/ |
|
605 /***********************************************/ |
|
606 /** **/ |
|
607 /** initialise / shutdown the library **/ |
|
608 /** **/ |
|
609 /***********************************************/ |
|
610 /***********************************************/ |
|
611 |
|
612 int mb_slave_init__(int extra_bytes) { |
|
613 buff_extra_bytes_ = extra_bytes; |
|
614 return 0; |
|
615 } |
|
616 |
|
617 |
|
618 int mb_slave_done__(void) |
|
619 {return 0;} |
|
620 |
|
621 |
|
622 #if 0 |
|
623 int mb_slave_init(int nd_count) { |
|
624 int extra_bytes; |
|
625 |
|
626 #ifdef DEBUG |
|
627 fprintf( stderr, "mb_slave_init()\n"); |
|
628 fprintf( stderr, "creating %d nodes\n", nd_count); |
|
629 #endif |
|
630 |
|
631 /* initialise layer 1 library */ |
|
632 if (modbus_init(nd_count, DEF_OPTIMIZATION, &extra_bytes) < 0) |
|
633 goto error_exit_0; |
|
634 |
|
635 /* initialise this library */ |
|
636 if (mb_slave_init__(extra_bytes) < 0) |
|
637 goto error_exit_1; |
|
638 |
|
639 return 0; |
|
640 |
|
641 error_exit_1: |
|
642 modbus_done(); |
|
643 error_exit_0: |
|
644 return -1; |
|
645 } |
|
646 |
|
647 |
|
648 int mb_slave_done(void) { |
|
649 mb_slave_done__(void) |
|
650 return modbus_done(); |
|
651 } |
|
652 #endif |
|
653 |
|
654 |
|
655 |
|
656 /***********************************************/ |
|
657 /***********************************************/ |
|
658 /** **/ |
|
659 /** open/close slave connection **/ |
|
660 /** **/ |
|
661 /***********************************************/ |
|
662 /***********************************************/ |
|
663 |
|
664 /* Create a new slave/server */ |
|
665 /* NOTE: We use the lower 2 bits of the returned node id to identify which |
|
666 * layer1 implementation to use. |
|
667 * 0 -> TCP |
|
668 * 1 -> RTU |
|
669 * 2 -> ASCII |
|
670 * 4 -> unused |
|
671 * The node id used by the layer1 is shifted left 2 bits |
|
672 * before returning the node id to the caller! |
|
673 */ |
|
674 int mb_slave_new(node_addr_t node_addr) { |
|
675 int res = -1; |
|
676 #ifdef DEBUG |
|
677 fprintf( stderr, "mb_slave_connect()\n"); |
|
678 #endif |
|
679 |
|
680 /* call layer 1 library */ |
|
681 switch(node_addr.naf) { |
|
682 case naf_tcp: |
|
683 res = modbus_tcp_listen(node_addr); |
|
684 if (res >= 0) res = res*4 + 0 /* offset into fptr_ with TCP functions */; |
|
685 return res; |
|
686 case naf_rtu: |
|
687 res = modbus_rtu_listen(node_addr); |
|
688 if (res >= 0) res = res*4 + 1 /* offset into fptr_ with RTU functions */; |
|
689 return res; |
|
690 case naf_ascii: |
|
691 res = modbus_ascii_listen(node_addr); |
|
692 if (res >= 0) res = res*4 + 2 /* offset into fptr_ with ASCII functions */; |
|
693 return res; |
|
694 } |
|
695 |
|
696 return -1; |
|
697 } |
|
698 |
|
699 |
|
700 |
|
701 |
|
702 int mb_slave_close(int fd) { |
|
703 #ifdef DEBUG |
|
704 fprintf( stderr, "mb_slave_close(): nd = %d\n", fd); |
|
705 #endif |
|
706 get_ttyfd(); /* declare the ttyfd variable!! */ |
|
707 /* call layer 1 library */ |
|
708 /* will call one of modbus_tcp_close(), modbus_rtu_close(), modbus_ascii_close() */ |
|
709 return modbus_close(ttyfd); |
|
710 } |
|
711 |
|
712 |
|
713 |
|
714 |
|
715 |
|
716 /***********************************************/ |
|
717 /***********************************************/ |
|
718 /** **/ |
|
719 /** Run the slave **/ |
|
720 /** **/ |
|
721 /***********************************************/ |
|
722 /***********************************************/ |
|
723 |
|
724 /* Execute infinite loop waiting and replying to requests coming from clients/master |
|
725 * This function enters an infinite loop wating for new connection requests, |
|
726 * and for modbus requests over previoulsy open connections... |
|
727 * |
|
728 * The frames are read from: |
|
729 * - the node descriptor nd, if nd >= 0 |
|
730 * When using TCP, if the referenced node nd was created to listen for new connections |
|
731 * [mb_slave_listen()], then this function will also reply to Modbus data requests arriving |
|
732 * on other nodes that were created as a consequence of accepting connections requests to |
|
733 * the referenced node nd. |
|
734 * All other nodes are ignored! |
|
735 * |
|
736 * - any valid and initialised TCP node descriptor, if nd = -1 |
|
737 * In this case, will also accept connection requests arriving from a previously |
|
738 * created node to listen for new connection requests [mb_slave_listen() ]. |
|
739 * NOTE: (only avaliable if using TCP) |
|
740 * |
|
741 * slaveid identifies the address (RTU and ASCII) or slaveid (TCP) that we implement. |
|
742 * Any requests that we receive sent with a slaveid different |
|
743 * than the one specified, and also different to 0, will be silently ignored! |
|
744 * Whatever the slaveid specified, we always reply to requests |
|
745 * to slaveid 0 (the modbus broadcast address). |
|
746 * Calling this function with a slaveid of 0 means to ignore this |
|
747 * parameter and to reply to all requests (whatever the slaveid |
|
748 * used in the request). This should mostly be used by TCP servers... |
|
749 */ |
|
750 |
|
751 int mb_slave_run(int fd, mb_slave_callback_t callback_functions, u8 slaveid) { |
|
752 int byte_count; |
|
753 u16 transaction_id; |
|
754 int nd; |
|
755 u8 function, error_code = 0; |
|
756 int resp_length; |
|
757 u8 *query_packet = NULL; |
|
758 u8 *resp_packet; |
|
759 u8 resp_buffer_[RESP_BUFFER_SIZE]; |
|
760 u8 slave; |
|
761 |
|
762 get_ttyfd(); /* declare the ttyfd variable!! */ |
|
763 |
|
764 #ifdef DEBUG |
|
765 fprintf(stderr,"[%lu] mb_slave_run(): Called... fd=%d, ttyfd=%d\n", pthread_self(), fd, ttyfd); |
|
766 #endif |
|
767 |
|
768 while(1) { |
|
769 nd = ttyfd; |
|
770 /* will call one of modbus_tcp_read(), modbus_rtu_read(), modbus_ascii_read() */ |
|
771 do { |
|
772 byte_count = modbus_read(&nd, /* node descriptor */ |
|
773 &query_packet, /* u8 **recv_data_ptr, */ |
|
774 &transaction_id, /* u16 *transaction_id, */ |
|
775 NULL, /* const u8 *send_data, */ |
|
776 0, /* int send_length, */ |
|
777 NULL /* wait indefenitely */ /* const struct timespec *recv_timeout); */ |
|
778 ); |
|
779 } while (byte_count <= 2); |
|
780 |
|
781 #ifdef DEBUG |
|
782 {/* display the hex code of each character received */ |
|
783 int i; |
|
784 printf("[%lu] mb_slave_run() received %d bytes (ptr=%p): \n", pthread_self(), byte_count, query_packet); |
|
785 for (i=0; i < byte_count; i++) |
|
786 printf("<0x%2X>", query_packet[i]); |
|
787 printf("\n"); |
|
788 } |
|
789 #endif |
|
790 |
|
791 slave = query_packet[0]; |
|
792 function = query_packet[1]; |
|
793 |
|
794 /* We only reply if: |
|
795 * - request was sent to broadcast address (slave == 0) |
|
796 * OR - we were asked to reply to every request (slaveid == 0) |
|
797 * OR - request matches the slaveid we were asked to accept (slave == slaveid) |
|
798 * |
|
799 * Otherwise, silently ignore the received request!!! |
|
800 */ |
|
801 if ((slaveid == 0) || (slave == 0) || (slave == slaveid)) { |
|
802 resp_packet = resp_buffer_; |
|
803 |
|
804 switch(function) { |
|
805 case 0x01: resp_length = handle_read_output_bits (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
806 case 0x02: resp_length = handle_read_input_bits (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
807 case 0x03: resp_length = handle_read_output_words (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
808 case 0x04: resp_length = handle_read_input_words (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
809 case 0x05: resp_length = handle_write_output_bit (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
810 case 0x06: resp_length = handle_write_output_word (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
811 case 0x0F: resp_length = handle_write_output_bits (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
812 case 0x10: resp_length = handle_write_output_words(query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
813 /* return exception code 0x01 -> function not supported! */ |
|
814 default : resp_length = -1; error_code = 0x01; break; |
|
815 }; /* switch(function) */ |
|
816 |
|
817 if (resp_length < 0) { |
|
818 /* return error... */ |
|
819 /* build exception response frame... */ |
|
820 resp_packet = resp_buffer_; |
|
821 resp_packet[0] = query_packet[0]; /* slave */ |
|
822 resp_packet[1] = query_packet[1] | 0x80; /* function code with error bit activated! */ |
|
823 resp_packet[2] = error_code; |
|
824 resp_length = 3; |
|
825 } |
|
826 modbus_write(nd, resp_packet, resp_length, transaction_id, NULL /*transmit_timeout*/); |
|
827 }; /* if not ignore request */ |
|
828 }; /* while(1) */ |
|
829 |
|
830 /* humour the compiler... */ |
|
831 return 0; |
|
832 } |
|
833 |
|
834 |
|
835 |
|
836 |
|
837 |
|
838 |
|
839 |
|
840 |
|
841 |
|
842 |
|
843 |
|
844 |
|
845 |