1 /*
2 * Copyright (c) 2020 PHYTEC Messtechnik GmbH
3 * Copyright (c) 2021 Nordic Semiconductor ASA
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/kernel.h>
9 #include <zephyr/sys/util.h>
10 #include <zephyr/drivers/gpio.h>
11 #include <zephyr/modbus/modbus.h>
12
13 #include <zephyr/posix/netinet/in.h>
14 #include <zephyr/posix/sys/socket.h>
15 #include <zephyr/posix/arpa/inet.h>
16 #include <zephyr/posix/unistd.h>
17 #include <zephyr/posix/poll.h>
18 #include <zephyr/posix/netdb.h>
19
20 #include <zephyr/net/socket.h>
21
22 #include <zephyr/logging/log.h>
23 LOG_MODULE_REGISTER(tcp_modbus, LOG_LEVEL_INF);
24
25 #define MODBUS_TCP_PORT 502
26
27 static uint16_t holding_reg[8];
28 static uint8_t coils_state;
29
30 static const struct gpio_dt_spec led_dev[] = {
31 GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios),
32 GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios),
33 GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios),
34 };
35
36 static int custom_read_count;
37
custom_handler(const int iface,const struct modbus_adu * rx_adu,struct modbus_adu * tx_adu,uint8_t * const excep_code,void * const user_data)38 static bool custom_handler(const int iface,
39 const struct modbus_adu *rx_adu,
40 struct modbus_adu *tx_adu,
41 uint8_t *const excep_code,
42 void *const user_data)
43 {
44 const uint8_t request_len = 2;
45 const uint8_t response_len = 6;
46 int *read_counter = (int *)user_data;
47 uint8_t subfunc;
48 uint8_t data_len;
49
50 LOG_INF("Custom Modbus handler called");
51
52 if (rx_adu->length != request_len) {
53 LOG_WRN("Custom request length doesn't match");
54 *excep_code = MODBUS_EXC_ILLEGAL_DATA_VAL;
55 return true;
56 }
57
58 subfunc = rx_adu->data[0];
59 data_len = rx_adu->data[1];
60
61 LOG_INF("Custom function called with subfunc=%u, data_len=%u", subfunc, data_len);
62 (*read_counter)++;
63 sys_put_be16(0x5555, tx_adu->data);
64 sys_put_be16(0xAAAA, &tx_adu->data[2]);
65 sys_put_be16(*read_counter, &tx_adu->data[4]);
66 tx_adu->length = response_len;
67
68 return true;
69 }
70
71 MODBUS_CUSTOM_FC_DEFINE(custom, custom_handler, 101, &custom_read_count);
72
init_leds(void)73 static int init_leds(void)
74 {
75 int err;
76
77 for (int i = 0; i < ARRAY_SIZE(led_dev); i++) {
78 if (!gpio_is_ready_dt(&led_dev[i])) {
79 LOG_ERR("LED%u GPIO device not ready", i);
80 return -ENODEV;
81 }
82
83 err = gpio_pin_configure_dt(&led_dev[i], GPIO_OUTPUT_INACTIVE);
84 if (err != 0) {
85 LOG_ERR("Failed to configure LED%u pin", i);
86 return err;
87 }
88 }
89
90 return 0;
91 }
92
coil_rd(uint16_t addr,bool * state)93 static int coil_rd(uint16_t addr, bool *state)
94 {
95 if (addr >= ARRAY_SIZE(led_dev)) {
96 return -ENOTSUP;
97 }
98
99 if (coils_state & BIT(addr)) {
100 *state = true;
101 } else {
102 *state = false;
103 }
104
105 LOG_INF("Coil read, addr %u, %d", addr, (int)*state);
106
107 return 0;
108 }
109
coil_wr(uint16_t addr,bool state)110 static int coil_wr(uint16_t addr, bool state)
111 {
112 bool on;
113
114 if (addr >= ARRAY_SIZE(led_dev)) {
115 return -ENOTSUP;
116 }
117
118 if (state == true) {
119 coils_state |= BIT(addr);
120 on = true;
121 } else {
122 coils_state &= ~BIT(addr);
123 on = false;
124 }
125
126 gpio_pin_set(led_dev[addr].port, led_dev[addr].pin, (int)on);
127
128 LOG_INF("Coil write, addr %u, %d", addr, (int)state);
129
130 return 0;
131 }
132
holding_reg_rd(uint16_t addr,uint16_t * reg)133 static int holding_reg_rd(uint16_t addr, uint16_t *reg)
134 {
135 if (addr >= ARRAY_SIZE(holding_reg)) {
136 return -ENOTSUP;
137 }
138
139 *reg = holding_reg[addr];
140
141 LOG_INF("Holding register read, addr %u", addr);
142
143 return 0;
144 }
145
holding_reg_wr(uint16_t addr,uint16_t reg)146 static int holding_reg_wr(uint16_t addr, uint16_t reg)
147 {
148 if (addr >= ARRAY_SIZE(holding_reg)) {
149 return -ENOTSUP;
150 }
151
152 holding_reg[addr] = reg;
153
154 LOG_INF("Holding register write, addr %u", addr);
155
156 return 0;
157 }
158
159 static struct modbus_user_callbacks mbs_cbs = {
160 .coil_rd = coil_rd,
161 .coil_wr = coil_wr,
162 .holding_reg_rd = holding_reg_rd,
163 .holding_reg_wr = holding_reg_wr,
164 };
165
166 static struct modbus_adu tmp_adu;
167 K_SEM_DEFINE(received, 0, 1);
168 static int server_iface;
169
server_raw_cb(const int iface,const struct modbus_adu * adu,void * user_data)170 static int server_raw_cb(const int iface, const struct modbus_adu *adu,
171 void *user_data)
172 {
173 LOG_DBG("Server raw callback from interface %d", iface);
174
175 tmp_adu.trans_id = adu->trans_id;
176 tmp_adu.proto_id = adu->proto_id;
177 tmp_adu.length = adu->length;
178 tmp_adu.unit_id = adu->unit_id;
179 tmp_adu.fc = adu->fc;
180 memcpy(tmp_adu.data, adu->data,
181 MIN(adu->length, CONFIG_MODBUS_BUFFER_SIZE));
182
183 LOG_HEXDUMP_DBG(tmp_adu.data, tmp_adu.length, "resp");
184 k_sem_give(&received);
185
186 return 0;
187 }
188
189 const static struct modbus_iface_param server_param = {
190 .mode = MODBUS_MODE_RAW,
191 .server = {
192 .user_cb = &mbs_cbs,
193 .unit_id = 1,
194 },
195 .rawcb.raw_tx_cb = server_raw_cb,
196 .rawcb.user_data = NULL
197 };
198
init_modbus_server(void)199 static int init_modbus_server(void)
200 {
201 char iface_name[] = "RAW_0";
202 int err;
203
204 server_iface = modbus_iface_get_by_name(iface_name);
205
206 if (server_iface < 0) {
207 LOG_ERR("Failed to get iface index for %s",
208 iface_name);
209 return -ENODEV;
210 }
211
212 err = modbus_init_server(server_iface, server_param);
213
214 if (err < 0) {
215 return err;
216 }
217
218 return modbus_register_user_fc(server_iface, &modbus_cfg_custom);
219 }
220
modbus_tcp_reply(int client,struct modbus_adu * adu)221 static int modbus_tcp_reply(int client, struct modbus_adu *adu)
222 {
223 uint8_t header[MODBUS_MBAP_AND_FC_LENGTH];
224
225 modbus_raw_put_header(adu, header);
226 if (send(client, header, sizeof(header), 0) < 0) {
227 return -errno;
228 }
229
230 if (send(client, adu->data, adu->length, 0) < 0) {
231 return -errno;
232 }
233
234 return 0;
235 }
236
modbus_tcp_connection(int client)237 static int modbus_tcp_connection(int client)
238 {
239 uint8_t header[MODBUS_MBAP_AND_FC_LENGTH];
240 int rc;
241 int data_len;
242
243 rc = recv(client, header, sizeof(header), MSG_WAITALL);
244 if (rc <= 0) {
245 return rc == 0 ? -ENOTCONN : -errno;
246 }
247
248 LOG_HEXDUMP_DBG(header, sizeof(header), "h:>");
249 modbus_raw_get_header(&tmp_adu, header);
250 data_len = tmp_adu.length;
251
252 rc = recv(client, tmp_adu.data, data_len, MSG_WAITALL);
253 if (rc <= 0) {
254 return rc == 0 ? -ENOTCONN : -errno;
255 }
256
257 LOG_HEXDUMP_DBG(tmp_adu.data, tmp_adu.length, "d:>");
258 if (modbus_raw_submit_rx(server_iface, &tmp_adu)) {
259 LOG_ERR("Failed to submit raw ADU");
260 return -EIO;
261 }
262
263 if (k_sem_take(&received, K_MSEC(1000)) != 0) {
264 LOG_ERR("MODBUS RAW wait time expired");
265 modbus_raw_set_server_failure(&tmp_adu);
266 }
267
268 return modbus_tcp_reply(client, &tmp_adu);
269 }
270
main(void)271 int main(void)
272 {
273 int serv;
274 struct sockaddr_in bind_addr;
275 static int counter;
276
277 if (init_modbus_server()) {
278 LOG_ERR("Modbus TCP server initialization failed");
279 return 0;
280 }
281
282 if (init_leds()) {
283 LOG_ERR("Modbus TCP server initialization failed");
284 return 0;
285 }
286
287 serv = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
288
289 if (serv < 0) {
290 LOG_ERR("error: socket: %d", errno);
291 return 0;
292 }
293
294 bind_addr.sin_family = AF_INET;
295 bind_addr.sin_addr.s_addr = htonl(INADDR_ANY);
296 bind_addr.sin_port = htons(MODBUS_TCP_PORT);
297
298 if (bind(serv, (struct sockaddr *)&bind_addr, sizeof(bind_addr)) < 0) {
299 LOG_ERR("error: bind: %d", errno);
300 return 0;
301 }
302
303 if (listen(serv, 5) < 0) {
304 LOG_ERR("error: listen: %d", errno);
305 return 0;
306 }
307
308 LOG_INF("Started MODBUS TCP server example on port %d", MODBUS_TCP_PORT);
309
310 while (1) {
311 struct sockaddr_in client_addr;
312 socklen_t client_addr_len = sizeof(client_addr);
313 char addr_str[INET_ADDRSTRLEN];
314 int client;
315 int rc;
316
317 client = accept(serv, (struct sockaddr *)&client_addr,
318 &client_addr_len);
319
320 if (client < 0) {
321 LOG_ERR("error: accept: %d", errno);
322 continue;
323 }
324
325 inet_ntop(client_addr.sin_family, &client_addr.sin_addr,
326 addr_str, sizeof(addr_str));
327 LOG_INF("Connection #%d from %s",
328 counter++, addr_str);
329
330 do {
331 rc = modbus_tcp_connection(client);
332 } while (!rc);
333
334 close(client);
335 LOG_INF("Connection from %s closed, errno %d",
336 addr_str, rc);
337 }
338 return 0;
339 }
340
