| author | Mario de Sousa <msousa@fe.up.pt> |
| Mon, 01 Jun 2020 14:15:03 +0100 | |
| changeset 8 | f14859c24751 |
| parent 6 | fe4088d5573a |
| child 9 | d6effe86bc2f |
| permissions | -rw-r--r-- |
| 0 | 1 |
/* |
2 |
* Copyright (c) 2001,2016 Mario de Sousa (msousa@fe.up.pt) |
|
3 |
* |
|
4 |
* This file is part of the Modbus library for Beremiz and matiec. |
|
5 |
* |
|
6 |
* This Modbus library is free software: you can redistribute it and/or modify |
|
7 |
* it under the terms of the GNU Lesser General Public License as published by |
|
8 |
* the Free Software Foundation, either version 3 of the License, or |
|
9 |
* (at your option) any later version. |
|
10 |
* |
|
11 |
* This program is distributed in the hope that it will be useful, but |
|
12 |
* WITHOUT ANY WARRANTY; without even the implied warranty of |
|
13 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser |
|
14 |
* General Public License for more details. |
|
15 |
* |
|
16 |
* You should have received a copy of the GNU Lesser General Public License |
|
17 |
* along with this Modbus library. If not, see <http://www.gnu.org/licenses/>. |
|
18 |
* |
|
19 |
* This code is made available on the understanding that it will not be |
|
20 |
* used in safety-critical situations without a full and competent review. |
|
21 |
*/ |
|
22 |
||
23 |
||
24 |
/* 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 |
||
|
8
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
110 |
/* Determine endianess of platform... */ |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
111 |
|
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
112 |
/* WARNING: The following files are being included: |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
113 |
* <stdib.h> --> <endian.h> --> <bits/endian.h> |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
114 |
* |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
115 |
* endian.h defines the following constants as: |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
116 |
* #define __LITTLE_ENDIAN and LITTLE_ENDIAN as 1234 |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
117 |
* #define __BIG_ENDIAN and BIG_ENDIAN as 4321 |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
118 |
* #define __PDP_ENDIAN and PDP_ENDIAN as 3412 |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
119 |
* |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
120 |
* bits/endian.h defines the constant BYTE_ORDER as: |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
121 |
* #define __BYTE_ORDER as __LITTLE_ENDIAN |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
122 |
* |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
123 |
* endian.h then sets the following constants |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
124 |
* (if __USE_BSD is set, which seems to be true): |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
125 |
* # define LITTLE_ENDIAN __LITTLE_ENDIAN |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
126 |
* # define BIG_ENDIAN __BIG_ENDIAN |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
127 |
* # define PDP_ENDIAN __PDP_ENDIAN |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
128 |
* # define BYTE_ORDER __BYTE_ORDER |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
129 |
*/ |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
130 |
|
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
131 |
/* If we still don't know byte order, try to get it from <endian.h> */ |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
132 |
#ifndef __BYTE_ORDER |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
133 |
#include <endian.h> |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
134 |
#endif |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
135 |
|
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
136 |
|
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
137 |
/* If we still don't know byte order => if using gcc, use it to determine byte order... */ |
| 0 | 138 |
#ifndef __BYTE_ORDER |
139 |
#if defined(__GNUC__) |
|
140 |
/* We have GCC, which should define __LITTLE_ENDIAN__ */ |
|
141 |
# if defined(__LITTLE_ENDIAN__) |
|
142 |
# define __BYTE_ORDER __LITTLE_ENDIAN |
|
143 |
# else |
|
144 |
# define __BYTE_ORDER __BIG_ENDIAN |
|
145 |
# endif |
|
146 |
#endif /* __GNUC__ */ |
|
147 |
#endif /* __BYTE_ORDER */ |
|
148 |
||
149 |
||
150 |
#ifndef __BYTE_ORDER |
|
|
8
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
151 |
# error "Unable to determine platform's byte order. Aborting compilation." |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
152 |
#elif __BYTE_ORDER == __BIG_ENDIAN |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
153 |
# warning "Compiling for BIG endian platform." |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
154 |
#elif __BYTE_ORDER == __LITTLE_ENDIAN |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
155 |
# warning "Compiling for LITTLE endian platform." |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
156 |
#else |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
157 |
# error "Aborting compilation due to unsuported byte order (neither BIG not LITTLE endian)." |
| 0 | 158 |
#endif |
159 |
||
160 |
||
|
8
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
161 |
|
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
162 |
/* |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
163 |
* Functions to convert u16 variables |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
164 |
* between network and host byte order |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
165 |
* |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
166 |
* NOTE: Modbus uses MSByte first, just like |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
167 |
* tcp/ip, so we could be tempted to use the htons() and |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
168 |
* ntohs() functions to guarantee code portability. |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
169 |
* |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
170 |
* However, on some embedded systems running Linux |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
171 |
* these functions only work if the 16 bit words are |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
172 |
* stored on even addresses. This is not always the |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
173 |
* case in our code, so we have to define our own |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
174 |
* conversion functions... |
|
f14859c24751
Allow "" and NULL on TCP host and service (=> IP=INADDR_ANY, and port=0)
Mario de Sousa <msousa@fe.up.pt>
parents:
6
diff
changeset
|
175 |
*/ |
| 0 | 176 |
|
177 |
||
178 |
#ifdef __BYTE_ORDER |
|
179 |
# if __BYTE_ORDER == __LITTLE_ENDIAN |
|
180 |
||
181 |
/**************************************************************/ |
|
182 |
/* u16 conversion functions to use on little endian platforms */ |
|
183 |
/**************************************************************/ |
|
184 |
||
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
185 |
/* NOTE: The input parameter must be the address |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
186 |
* of an u16 passed as a pointer to u8 |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
187 |
* |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
188 |
* We use u8 *ptr as input parameter and read both (ptr+0) and (ptr+1) |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
189 |
* instead of using u16 *ptr because we sometimes receive data in packtes |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
190 |
* that are not aligned on even addresses, so some compilers recognize that |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
191 |
* the given odd address cannot be used as a pointer to u16 and therefore |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
192 |
* adjust the pointer by (+1) or (-1), basicaly breacking our code! |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
193 |
* So, we revert to u8 pointers... for u16 values. |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
194 |
*/ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
195 |
static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
196 |
u16_to_ptr[0] = u16_from_ptr[1]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
197 |
u16_to_ptr[1] = u16_from_ptr[0]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
198 |
} |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
199 |
#define mb_ntoh(a, b) mb_hton(a, b) |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
200 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
201 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
202 |
static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
203 |
unsigned i; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
204 |
for (i = 0; i < count*2; i += 2) {
|
| 0 | 205 |
/* swap the bytes around... |
206 |
* a = a ^ b; |
|
207 |
* b = a ^ b; |
|
208 |
* a = a ^ b; |
|
209 |
*/ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
210 |
(u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
211 |
(u16_ptr+i)[1] ^= (u16_ptr+i)[0]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
212 |
(u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
| 0 | 213 |
} |
214 |
} |
|
215 |
#define mb_ntoh_count(w, count) mb_hton_count(w, count) |
|
216 |
||
217 |
||
218 |
||
219 |
# else |
|
220 |
# if __BYTE_ORDER == __BIG_ENDIAN |
|
221 |
/***********************************************************/ |
|
222 |
/* u16 conversion functions to use on big endian platforms */ |
|
223 |
/***********************************************************/ |
|
224 |
||
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
225 |
/* We don't need to swap the bytes around! */ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
226 |
static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
227 |
u16_to_ptr[0] = u16_from_ptr[0]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
228 |
u16_to_ptr[1] = u16_from_ptr[1]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
229 |
} |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
230 |
#define mb_ntoh(a, b) mb_hton(a, b) |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
231 |
|
| 0 | 232 |
#define mb_hton_count(w, count) /* empty ! */ |
233 |
#define mb_ntoh_count(w, count) /* empty ! */ |
|
234 |
||
235 |
||
236 |
# else |
|
237 |
/********************************************************/ |
|
238 |
/* u16 conversion functions to use on generic platforms */ |
|
239 |
/********************************************************/ |
|
240 |
||
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
241 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
242 |
/* We can't determine endiannes at compile time, so we do it at runtime. |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
243 |
* With any luck the compiler will be able to determine the result of the |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
244 |
* comparison at compile time and end up discarding the non-used code |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
245 |
* and the 'if' itself from the final executable. |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
246 |
*/ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
247 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
248 |
static union {u16 u16;
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
249 |
u8 u8[2];} endian_ = 0x0102; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
250 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
251 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
252 |
static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
253 |
if (endian_.u8[0] == 0x01) {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
254 |
/* machine is big endian -> no swapping */ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
255 |
u16_to_ptr[0] = u16_from_ptr[0]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
256 |
u16_to_ptr[1] = u16_from_ptr[1]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
257 |
} else {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
258 |
/* machine is little endian -> we swap bytes around */ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
259 |
u16_to_ptr[0] = u16_from_ptr[1]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
260 |
u16_to_ptr[1] = u16_from_ptr[0]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
261 |
} |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
262 |
} |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
263 |
#define mb_ntoh(a, b) mb_hton(a, b) |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
264 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
265 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
266 |
static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
267 |
unsigned i; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
268 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
269 |
if (endian_.u8[0] == 0x01) |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
270 |
/* machine is big endian. Nothing to do */ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
271 |
return; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
272 |
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
273 |
/* machine is little endian -> we swap bytes around */ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
274 |
for (i = 0; i < count*2; i += 2) {
|
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
275 |
/* swap the bytes around... |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
276 |
* a = a ^ b; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
277 |
* b = a ^ b; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
278 |
* a = a ^ b; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
279 |
*/ |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
280 |
(u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
281 |
(u16_ptr+i)[1] ^= (u16_ptr+i)[0]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
282 |
(u16_ptr+i)[0] ^= (u16_ptr+i)[1]; |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
283 |
} |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
284 |
} |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
285 |
#define mb_ntoh_count(w, count) mb_hton_count(w, count) |
| 0 | 286 |
|
287 |
# endif |
|
288 |
# endif |
|
289 |
#endif /* __BYTE_ORDER */ |
|
290 |
||
291 |
||
292 |
||
293 |
||
294 |
||
295 |
||
296 |
||
297 |
||
298 |
||
299 |
||
300 |
/***********************************************/ |
|
301 |
/***********************************************/ |
|
302 |
/** **/ |
|
303 |
/** Handle requests from master/client **/ |
|
304 |
/** **/ |
|
305 |
/***********************************************/ |
|
306 |
/***********************************************/ |
|
307 |
||
308 |
||
309 |
/* Handle functions 0x01 and 0x02 */ |
|
310 |
typedef int (*read_bits_callback_t)(void *arg, u16 start_addr, u16 bit_count, u8 *data_bytes); |
|
311 |
static int handle_read_bits (u8 *query_packet, |
|
312 |
u8 **resp_packet_ptr, |
|
313 |
u8 *error_code, |
|
314 |
read_bits_callback_t read_bits_callback, |
|
315 |
void *callback_arg |
|
316 |
) {
|
|
317 |
u16 start_addr, count; |
|
318 |
int res; |
|
319 |
u8 *resp_packet; |
|
320 |
||
321 |
/* If no callback, handle as if function is not supported... */ |
|
322 |
if (read_bits_callback == NULL) |
|
323 |
{*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
|
|
324 |
||
325 |
/* in oprder for the data in this packet to be aligned on even numbered addresses, this |
|
326 |
* response packet will start off at an odd numbered byte... |
|
327 |
* We therefore add 1 to the address where the packet starts. |
|
328 |
*/ |
|
329 |
(*resp_packet_ptr)++; |
|
330 |
resp_packet = *resp_packet_ptr; |
|
331 |
||
332 |
/* NOTE: |
|
333 |
* Modbus uses high level addressing starting off from 1, but |
|
334 |
* this is sent as 0 on the wire! |
|
335 |
* We could expect the user to specify high level addressing |
|
336 |
* starting at 1, and do the conversion to start off at 0 here. |
|
337 |
* However, to do this we would then need to use an u32 data type |
|
338 |
* to correctly hold the address supplied by the user (which could |
|
339 |
* correctly be 65536, which does not fit in an u16), which would |
|
340 |
* in turn require us to check whether the address supplied by the user |
|
341 |
* is correct (i.e. <= 65536). |
|
342 |
* I decided to go with the other option of using an u16, and |
|
343 |
* requiring the user to use addressing starting off at 0! |
|
344 |
*/ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
345 |
mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
346 |
mb_ntoh(&(query_packet[4]), (u8 *)&count); |
| 0 | 347 |
|
348 |
#ifdef DEBUG |
|
349 |
printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
|
|
350 |
query_packet[0], query_packet[1], start_addr, count); |
|
351 |
#endif |
|
352 |
||
353 |
if ((count > MAX_READ_BITS) || (count < 1)) |
|
354 |
{*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
|
|
355 |
||
356 |
/* Remember, we are using addressing starting off at 0, in the start_addr variable! */ |
|
357 |
/* This means that he highest acceptable address is 65535, when count=1 .... */ |
|
358 |
/* Note the use of 65536 in the comparison will force automatic upgrade of u16 variables! */ |
|
359 |
/* => start_addr + count will nver overflow the u16 type! */ |
|
360 |
if (start_addr + count > 65536) |
|
361 |
{*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
362 |
||
363 |
/* start building response frame... */ |
|
364 |
resp_packet[0] = query_packet[0]; /* slave */ |
|
365 |
resp_packet[1] = query_packet[1]; /* function (either 0x01 or 0x02 ! */ |
|
366 |
resp_packet[2] = (count + 7) / 8; /* number of data bytes = ceil(count/8) */ |
|
367 |
||
368 |
res = read_bits_callback(callback_arg, start_addr, count, &(resp_packet[3])); |
|
369 |
if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
370 |
if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
|
|
371 |
||
372 |
return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */ |
|
373 |
} |
|
374 |
||
375 |
||
376 |
||
377 |
/* Handle function 0x01 */ |
|
378 |
int handle_read_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
379 |
{return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_outbits, callbacks->arg);}
|
|
380 |
||
381 |
/* Handle function 0x02 */ |
|
382 |
int handle_read_input_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
383 |
{return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_inbits, callbacks->arg);}
|
|
384 |
||
385 |
||
386 |
||
387 |
||
388 |
/* Handle functions 0x03 and 0x04 */ |
|
389 |
typedef int (*read_words_callback_t)(void *arg, u16 start_addr, u16 word_count, u16 *data_words); |
|
390 |
static int handle_read_words (u8 *query_packet, |
|
391 |
u8 **resp_packet_ptr, |
|
392 |
u8 *error_code, |
|
393 |
read_words_callback_t read_words_callback, |
|
394 |
void *callback_arg |
|
395 |
) {
|
|
396 |
u16 start_addr, count; |
|
397 |
int res; |
|
398 |
u8 *resp_packet; |
|
399 |
||
400 |
/* If no callback, handle as if function is not supported... */ |
|
401 |
if (read_words_callback == NULL) |
|
402 |
{*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
|
|
403 |
||
404 |
/* See equivalent comment in handle_read_bits() */ |
|
405 |
(*resp_packet_ptr)++; |
|
406 |
resp_packet = *resp_packet_ptr; |
|
407 |
||
408 |
/* See equivalent comment in handle_read_bits() */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
409 |
mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
410 |
mb_ntoh(&(query_packet[4]), (u8 *)&count); |
| 0 | 411 |
|
412 |
#ifdef DEBUG |
|
413 |
printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
|
|
414 |
query_packet[0], query_packet[1], start_addr, count); |
|
415 |
#endif |
|
416 |
||
417 |
if ((count > MAX_READ_REGS) || (count < 1)) |
|
418 |
{*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
|
|
419 |
||
420 |
/* See equivalent comment in handle_read_bits() */ |
|
421 |
if (start_addr + count > 65536) |
|
422 |
{*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
423 |
||
424 |
/* start building response frame... */ |
|
425 |
resp_packet[0] = query_packet[0]; /* slave */ |
|
426 |
resp_packet[1] = query_packet[1]; /* function code, either 0x03 or 0x04 !!!*/ |
|
427 |
resp_packet[2] = count * 2; /* number of bytes of data... */ |
|
428 |
||
429 |
res = read_words_callback(callback_arg, start_addr, count, (u16 *)&(resp_packet[3])); |
|
430 |
if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
431 |
if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
|
|
432 |
||
433 |
/* convert all data from host to network byte order. */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
434 |
mb_hton_count(&(resp_packet[3]), count); |
| 0 | 435 |
|
436 |
return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */ |
|
437 |
} |
|
438 |
||
439 |
||
440 |
||
441 |
||
442 |
/* Handle function 0x03 */ |
|
443 |
int handle_read_output_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
444 |
{return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_outwords, callbacks->arg);}
|
|
445 |
||
446 |
/* Handle function 0x04 */ |
|
447 |
int handle_read_input_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) |
|
448 |
{return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_inwords, callbacks->arg);}
|
|
449 |
||
450 |
||
451 |
||
452 |
/* Handle function 0x05 */ |
|
453 |
int handle_write_output_bit (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
|
|
454 |
u16 start_addr; |
|
455 |
int res; |
|
456 |
u8 *resp_packet; |
|
457 |
||
458 |
/* If no callback, handle as if function is not supported... */ |
|
459 |
if (callbacks->write_outbits == NULL) |
|
460 |
{*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
|
|
461 |
||
462 |
resp_packet = *resp_packet_ptr; |
|
463 |
||
464 |
/* See equivalent comment in handle_read_bits() */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
465 |
mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
| 0 | 466 |
|
467 |
#ifdef DEBUG |
|
468 |
printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n",
|
|
469 |
query_packet[0], query_packet[1], start_addr); |
|
470 |
#endif |
|
471 |
||
472 |
// byte 5 Must be 0x00, byte 4 must be 0x00 or 0xFF !! |
|
473 |
if ( (query_packet[5] != 0) || |
|
474 |
((query_packet[4] != 0) && (query_packet[4] != 0xFF))) |
|
475 |
{*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
|
|
476 |
||
477 |
/* Address will always be valid, no need to check! */ |
|
478 |
// if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
479 |
||
480 |
/* start building response frame... */ |
|
481 |
resp_packet[0] = query_packet[0]; /* slave */ |
|
482 |
resp_packet[1] = query_packet[1]; /* function */ |
|
483 |
resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
484 |
resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
485 |
resp_packet[4] = query_packet[4]; /* value: 0x00 or 0xFF */ |
|
486 |
resp_packet[5] = query_packet[5]; /* value: must be 0x00 */ |
|
487 |
||
488 |
res = (callbacks->write_outbits)(callbacks->arg, start_addr, 1, &(query_packet[4])); |
|
489 |
if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
490 |
if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
|
|
491 |
||
492 |
return 6; /* response packet size, including slave id in byte 0 */ |
|
493 |
} |
|
494 |
||
495 |
||
496 |
||
497 |
/* Handle function 0x06 */ |
|
498 |
int handle_write_output_word (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
|
|
499 |
u16 start_addr; |
|
500 |
int res; |
|
501 |
u8 *resp_packet; |
|
502 |
||
503 |
/* If no callback, handle as if function is not supported... */ |
|
504 |
if (callbacks->write_outwords == NULL) |
|
505 |
{*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
|
|
506 |
||
507 |
resp_packet = *resp_packet_ptr; |
|
508 |
||
509 |
/* See equivalent comment in handle_read_bits() */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
510 |
mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
| 0 | 511 |
|
512 |
#ifdef DEBUG |
|
513 |
printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n",
|
|
514 |
query_packet[0], query_packet[1], start_addr); |
|
515 |
#endif |
|
516 |
||
517 |
/* Address will always be valid, no need to check! */ |
|
518 |
// if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
519 |
||
520 |
/* start building response frame... */ |
|
521 |
resp_packet[0] = query_packet[0]; /* slave */ |
|
522 |
resp_packet[1] = query_packet[1]; /* function */ |
|
523 |
resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
524 |
resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
525 |
resp_packet[4] = query_packet[4]; /* value - hi byte */ |
|
526 |
resp_packet[5] = query_packet[5]; /* value - lo byte */ |
|
527 |
||
528 |
/* convert data from network to host byte order */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
529 |
mb_ntoh_count(&(query_packet[4]), 1); |
| 0 | 530 |
|
531 |
res = (callbacks->write_outwords)(callbacks->arg, start_addr, 1, (u16 *)&(query_packet[4])); |
|
532 |
if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
533 |
if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
|
|
534 |
||
535 |
return 6; /* packet size is 6 -> slave, function, addr(2), value(2) */ |
|
536 |
} |
|
537 |
||
538 |
||
539 |
||
540 |
/* Handle function 0x0F */ |
|
541 |
int handle_write_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
|
|
542 |
u16 start_addr, count; |
|
543 |
int res; |
|
544 |
u8 *resp_packet; |
|
545 |
||
546 |
/* If no callback, handle as if function is not supported... */ |
|
547 |
if (callbacks->write_outbits == NULL) |
|
548 |
{*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
|
|
549 |
||
550 |
resp_packet = *resp_packet_ptr; |
|
551 |
||
552 |
/* See equivalent comment in handle_read_bits() */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
553 |
mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
554 |
mb_ntoh(&(query_packet[4]), (u8 *)&count); |
| 0 | 555 |
|
556 |
#ifdef DEBUG |
|
557 |
printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
|
|
558 |
query_packet[0], query_packet[1], start_addr, count); |
|
559 |
#endif |
|
560 |
||
561 |
if ((count > MAX_WRITE_COILS) || (count < 1) || ((count+7)/8 != query_packet[6]) ) |
|
562 |
{*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
|
|
563 |
||
564 |
/* See equivalent comment in handle_read_bits() */ |
|
565 |
if (start_addr + count > 65536) |
|
566 |
{*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
567 |
||
568 |
/* start building response frame... */ |
|
569 |
resp_packet[0] = query_packet[0]; /* slave */ |
|
570 |
resp_packet[1] = query_packet[1]; /* function */ |
|
571 |
resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
572 |
resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
573 |
resp_packet[4] = query_packet[4]; /* count - hi byte */ |
|
574 |
resp_packet[5] = query_packet[5]; /* count - lo byte */ |
|
575 |
||
576 |
res = (callbacks->write_outbits)(callbacks->arg, start_addr, count, &(query_packet[7])); |
|
577 |
if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
578 |
if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
|
|
579 |
||
580 |
return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */ |
|
581 |
} |
|
582 |
||
583 |
||
584 |
||
585 |
||
586 |
/* Handle function 0x10 */ |
|
587 |
int handle_write_output_words(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
|
|
588 |
u16 start_addr, count; |
|
589 |
int res; |
|
590 |
u8 *resp_packet; |
|
591 |
||
592 |
/* If no callback, handle as if function is not supported... */ |
|
593 |
if (callbacks->write_outwords == NULL) |
|
594 |
{*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
|
|
595 |
||
596 |
resp_packet = *resp_packet_ptr; |
|
597 |
||
598 |
/* See equivalent comment in handle_read_bits() */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
599 |
mb_ntoh(&(query_packet[2]), (u8 *)&start_addr); |
|
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
600 |
mb_ntoh(&(query_packet[4]), (u8 *)&count); |
| 0 | 601 |
|
602 |
if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) ) |
|
603 |
{*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
|
|
604 |
||
605 |
/* See equivalent comment in handle_read_bits() */ |
|
606 |
if (start_addr + count > 65536) |
|
607 |
{*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
608 |
||
609 |
/* start building response frame... */ |
|
610 |
resp_packet[0] = query_packet[0]; /* slave */ |
|
611 |
resp_packet[1] = query_packet[1]; /* function */ |
|
612 |
resp_packet[2] = query_packet[2]; /* start address - hi byte */ |
|
613 |
resp_packet[3] = query_packet[3]; /* start address - lo byte */ |
|
614 |
resp_packet[4] = query_packet[4]; /* count - hi byte */ |
|
615 |
resp_packet[5] = query_packet[5]; /* count - lo byte */ |
|
616 |
||
617 |
/* convert all data from network to host byte order */ |
|
|
6
fe4088d5573a
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents:
0
diff
changeset
|
618 |
mb_ntoh_count(&(query_packet[7]), count); |
| 0 | 619 |
|
620 |
res = (callbacks->write_outwords)(callbacks->arg, start_addr, count, (u16 *)&(query_packet[7])); |
|
621 |
if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
|
|
622 |
if (res < 0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
|
|
623 |
||
624 |
return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */ |
|
625 |
} |
|
626 |
||
627 |
||
628 |
||
629 |
||
630 |
||
631 |
||
632 |
||
633 |
||
634 |
/***********************************************/ |
|
635 |
/***********************************************/ |
|
636 |
/** **/ |
|
637 |
/** initialise / shutdown the library **/ |
|
638 |
/** **/ |
|
639 |
/***********************************************/ |
|
640 |
/***********************************************/ |
|
641 |
||
642 |
int mb_slave_init__(int extra_bytes) {
|
|
643 |
buff_extra_bytes_ = extra_bytes; |
|
644 |
return 0; |
|
645 |
} |
|
646 |
||
647 |
||
648 |
int mb_slave_done__(void) |
|
649 |
{return 0;}
|
|
650 |
||
651 |
||
652 |
#if 0 |
|
653 |
int mb_slave_init(int nd_count) {
|
|
654 |
int extra_bytes; |
|
655 |
||
656 |
#ifdef DEBUG |
|
657 |
fprintf( stderr, "mb_slave_init()\n"); |
|
658 |
fprintf( stderr, "creating %d nodes\n", nd_count); |
|
659 |
#endif |
|
660 |
||
661 |
/* initialise layer 1 library */ |
|
662 |
if (modbus_init(nd_count, DEF_OPTIMIZATION, &extra_bytes) < 0) |
|
663 |
goto error_exit_0; |
|
664 |
||
665 |
/* initialise this library */ |
|
666 |
if (mb_slave_init__(extra_bytes) < 0) |
|
667 |
goto error_exit_1; |
|
668 |
||
669 |
return 0; |
|
670 |
||
671 |
error_exit_1: |
|
672 |
modbus_done(); |
|
673 |
error_exit_0: |
|
674 |
return -1; |
|
675 |
} |
|
676 |
||
677 |
||
678 |
int mb_slave_done(void) {
|
|
679 |
mb_slave_done__(void) |
|
680 |
return modbus_done(); |
|
681 |
} |
|
682 |
#endif |
|
683 |
||
684 |
||
685 |
||
686 |
/***********************************************/ |
|
687 |
/***********************************************/ |
|
688 |
/** **/ |
|
689 |
/** open/close slave connection **/ |
|
690 |
/** **/ |
|
691 |
/***********************************************/ |
|
692 |
/***********************************************/ |
|
693 |
||
694 |
/* Create a new slave/server */ |
|
695 |
/* NOTE: We use the lower 2 bits of the returned node id to identify which |
|
696 |
* layer1 implementation to use. |
|
697 |
* 0 -> TCP |
|
698 |
* 1 -> RTU |
|
699 |
* 2 -> ASCII |
|
700 |
* 4 -> unused |
|
701 |
* The node id used by the layer1 is shifted left 2 bits |
|
702 |
* before returning the node id to the caller! |
|
703 |
*/ |
|
704 |
int mb_slave_new(node_addr_t node_addr) {
|
|
705 |
int res = -1; |
|
706 |
#ifdef DEBUG |
|
707 |
fprintf( stderr, "mb_slave_connect()\n"); |
|
708 |
#endif |
|
709 |
||
710 |
/* call layer 1 library */ |
|
711 |
switch(node_addr.naf) {
|
|
712 |
case naf_tcp: |
|
713 |
res = modbus_tcp_listen(node_addr); |
|
714 |
if (res >= 0) res = res*4 + 0 /* offset into fptr_ with TCP functions */; |
|
715 |
return res; |
|
716 |
case naf_rtu: |
|
717 |
res = modbus_rtu_listen(node_addr); |
|
718 |
if (res >= 0) res = res*4 + 1 /* offset into fptr_ with RTU functions */; |
|
719 |
return res; |
|
720 |
case naf_ascii: |
|
721 |
res = modbus_ascii_listen(node_addr); |
|
722 |
if (res >= 0) res = res*4 + 2 /* offset into fptr_ with ASCII functions */; |
|
723 |
return res; |
|
724 |
} |
|
725 |
||
726 |
return -1; |
|
727 |
} |
|
728 |
||
729 |
||
730 |
||
731 |
||
732 |
int mb_slave_close(int fd) {
|
|
733 |
#ifdef DEBUG |
|
734 |
fprintf( stderr, "mb_slave_close(): nd = %d\n", fd); |
|
735 |
#endif |
|
736 |
get_ttyfd(); /* declare the ttyfd variable!! */ |
|
737 |
/* call layer 1 library */ |
|
738 |
/* will call one of modbus_tcp_close(), modbus_rtu_close(), modbus_ascii_close() */ |
|
739 |
return modbus_close(ttyfd); |
|
740 |
} |
|
741 |
||
742 |
||
743 |
||
744 |
||
745 |
||
746 |
/***********************************************/ |
|
747 |
/***********************************************/ |
|
748 |
/** **/ |
|
749 |
/** Run the slave **/ |
|
750 |
/** **/ |
|
751 |
/***********************************************/ |
|
752 |
/***********************************************/ |
|
753 |
||
754 |
/* Execute infinite loop waiting and replying to requests coming from clients/master |
|
755 |
* This function enters an infinite loop wating for new connection requests, |
|
756 |
* and for modbus requests over previoulsy open connections... |
|
757 |
* |
|
758 |
* The frames are read from: |
|
759 |
* - the node descriptor nd, if nd >= 0 |
|
760 |
* When using TCP, if the referenced node nd was created to listen for new connections |
|
761 |
* [mb_slave_listen()], then this function will also reply to Modbus data requests arriving |
|
762 |
* on other nodes that were created as a consequence of accepting connections requests to |
|
763 |
* the referenced node nd. |
|
764 |
* All other nodes are ignored! |
|
765 |
* |
|
766 |
* - any valid and initialised TCP node descriptor, if nd = -1 |
|
767 |
* In this case, will also accept connection requests arriving from a previously |
|
768 |
* created node to listen for new connection requests [mb_slave_listen() ]. |
|
769 |
* NOTE: (only avaliable if using TCP) |
|
770 |
* |
|
771 |
* slaveid identifies the address (RTU and ASCII) or slaveid (TCP) that we implement. |
|
772 |
* Any requests that we receive sent with a slaveid different |
|
773 |
* than the one specified, and also different to 0, will be silently ignored! |
|
774 |
* Whatever the slaveid specified, we always reply to requests |
|
775 |
* to slaveid 0 (the modbus broadcast address). |
|
776 |
* Calling this function with a slaveid of 0 means to ignore this |
|
777 |
* parameter and to reply to all requests (whatever the slaveid |
|
778 |
* used in the request). This should mostly be used by TCP servers... |
|
779 |
*/ |
|
780 |
||
781 |
int mb_slave_run(int fd, mb_slave_callback_t callback_functions, u8 slaveid) {
|
|
782 |
int byte_count; |
|
783 |
u16 transaction_id; |
|
784 |
int nd; |
|
785 |
u8 function, error_code = 0; |
|
786 |
int resp_length; |
|
787 |
u8 *query_packet = NULL; |
|
788 |
u8 *resp_packet; |
|
789 |
u8 resp_buffer_[RESP_BUFFER_SIZE]; |
|
790 |
u8 slave; |
|
791 |
||
792 |
get_ttyfd(); /* declare the ttyfd variable!! */ |
|
793 |
||
794 |
#ifdef DEBUG |
|
795 |
fprintf(stderr,"[%lu] mb_slave_run(): Called... fd=%d, ttyfd=%d\n", pthread_self(), fd, ttyfd); |
|
796 |
#endif |
|
797 |
||
798 |
while(1) {
|
|
799 |
nd = ttyfd; |
|
800 |
/* will call one of modbus_tcp_read(), modbus_rtu_read(), modbus_ascii_read() */ |
|
801 |
do {
|
|
802 |
byte_count = modbus_read(&nd, /* node descriptor */ |
|
803 |
&query_packet, /* u8 **recv_data_ptr, */ |
|
804 |
&transaction_id, /* u16 *transaction_id, */ |
|
805 |
NULL, /* const u8 *send_data, */ |
|
806 |
0, /* int send_length, */ |
|
807 |
NULL /* wait indefenitely */ /* const struct timespec *recv_timeout); */ |
|
808 |
); |
|
809 |
} while (byte_count <= 2); |
|
810 |
||
811 |
#ifdef DEBUG |
|
812 |
{/* display the hex code of each character received */
|
|
813 |
int i; |
|
814 |
printf("[%lu] mb_slave_run() received %d bytes (ptr=%p): \n", pthread_self(), byte_count, query_packet);
|
|
815 |
for (i=0; i < byte_count; i++) |
|
816 |
printf("<0x%2X>", query_packet[i]);
|
|
817 |
printf("\n");
|
|
818 |
} |
|
819 |
#endif |
|
820 |
||
821 |
slave = query_packet[0]; |
|
822 |
function = query_packet[1]; |
|
823 |
||
824 |
/* We only reply if: |
|
825 |
* - request was sent to broadcast address (slave == 0) |
|
826 |
* OR - we were asked to reply to every request (slaveid == 0) |
|
827 |
* OR - request matches the slaveid we were asked to accept (slave == slaveid) |
|
828 |
* |
|
829 |
* Otherwise, silently ignore the received request!!! |
|
830 |
*/ |
|
831 |
if ((slaveid == 0) || (slave == 0) || (slave == slaveid)) {
|
|
832 |
resp_packet = resp_buffer_; |
|
833 |
||
834 |
switch(function) {
|
|
835 |
case 0x01: resp_length = handle_read_output_bits (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
836 |
case 0x02: resp_length = handle_read_input_bits (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
837 |
case 0x03: resp_length = handle_read_output_words (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
838 |
case 0x04: resp_length = handle_read_input_words (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
839 |
case 0x05: resp_length = handle_write_output_bit (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
840 |
case 0x06: resp_length = handle_write_output_word (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
841 |
case 0x0F: resp_length = handle_write_output_bits (query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
842 |
case 0x10: resp_length = handle_write_output_words(query_packet, &resp_packet, &error_code, &callback_functions); break; |
|
843 |
/* return exception code 0x01 -> function not supported! */ |
|
844 |
default : resp_length = -1; error_code = 0x01; break; |
|
845 |
}; /* switch(function) */ |
|
846 |
||
847 |
if (resp_length < 0) {
|
|
848 |
/* return error... */ |
|
849 |
/* build exception response frame... */ |
|
850 |
resp_packet = resp_buffer_; |
|
851 |
resp_packet[0] = query_packet[0]; /* slave */ |
|
852 |
resp_packet[1] = query_packet[1] | 0x80; /* function code with error bit activated! */ |
|
853 |
resp_packet[2] = error_code; |
|
854 |
resp_length = 3; |
|
855 |
} |
|
856 |
modbus_write(nd, resp_packet, resp_length, transaction_id, NULL /*transmit_timeout*/); |
|
857 |
}; /* if not ignore request */ |
|
858 |
}; /* while(1) */ |
|
859 |
||
860 |
/* humour the compiler... */ |
|
861 |
return 0; |
|
862 |
} |
|
863 |
||
864 |
||
865 |
||
866 |
||
867 |
||
868 |
||
869 |
||
870 |
||
871 |
||
872 |
||
873 |
||
874 |
||
875 |