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/modbus/modbus.h>
11 #include <zephyr/net/socket.h>
12
13 #include <zephyr/logging/log.h>
14 LOG_MODULE_REGISTER(tcp_gateway, LOG_LEVEL_INF);
15
16 #define MODBUS_TCP_PORT 502
17
18 static struct modbus_adu tmp_adu;
19 static int backend;
20
21 const static struct modbus_iface_param backend_param = {
22 .mode = MODBUS_MODE_RTU,
23 .rx_timeout = 50000,
24 .serial = {
25 .baud = 19200,
26 .parity = UART_CFG_PARITY_NONE,
27 },
28 };
29
30 #define MODBUS_NODE DT_COMPAT_GET_ANY_STATUS_OKAY(zephyr_modbus_serial)
31
init_backend_iface(void)32 static int init_backend_iface(void)
33 {
34 const char bend_name[] = {DEVICE_DT_NAME(MODBUS_NODE)};
35
36 backend = modbus_iface_get_by_name(bend_name);
37 if (backend < 0) {
38 LOG_ERR("Failed to get iface index for %s",
39 bend_name);
40 return -ENODEV;
41 }
42
43 return modbus_init_client(backend, backend_param);
44 }
45
modbus_tcp_reply(int client,struct modbus_adu * adu)46 static int modbus_tcp_reply(int client, struct modbus_adu *adu)
47 {
48 uint8_t header[MODBUS_MBAP_AND_FC_LENGTH];
49
50 modbus_raw_put_header(adu, header);
51 if (send(client, header, sizeof(header), 0) < 0) {
52 return -errno;
53 }
54
55 if (send(client, adu->data, adu->length, 0) < 0) {
56 return -errno;
57 }
58
59 return 0;
60 }
61
modbus_tcp_connection(int client)62 static int modbus_tcp_connection(int client)
63 {
64 uint8_t header[MODBUS_MBAP_AND_FC_LENGTH];
65 int rc;
66 int data_len;
67
68 rc = recv(client, header, sizeof(header), MSG_WAITALL);
69 if (rc <= 0) {
70 return rc == 0 ? -ENOTCONN : -errno;
71 }
72
73 LOG_HEXDUMP_DBG(header, sizeof(header), "h:>");
74 modbus_raw_get_header(&tmp_adu, header);
75 data_len = tmp_adu.length;
76
77 rc = recv(client, tmp_adu.data, data_len, MSG_WAITALL);
78 if (rc <= 0) {
79 return rc == 0 ? -ENOTCONN : -errno;
80 }
81
82 LOG_HEXDUMP_DBG(tmp_adu.data, tmp_adu.length, "d:>");
83 rc = modbus_raw_backend_txn(backend, &tmp_adu);
84 if (rc == -ENOTSUP || rc == -ENODEV) {
85 LOG_WRN("Backend interface error: %d", rc);
86 }
87
88 return modbus_tcp_reply(client, &tmp_adu);
89 }
90
main(void)91 int main(void)
92 {
93 int serv;
94 struct sockaddr_in bind_addr;
95 static int counter;
96
97 if (init_backend_iface()) {
98 LOG_ERR("Modbus initialization failed");
99 return 0;
100 }
101
102 serv = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
103
104 if (serv < 0) {
105 LOG_ERR("error: socket: %d", errno);
106 return 0;
107 }
108
109 bind_addr.sin_family = AF_INET;
110 bind_addr.sin_addr.s_addr = htonl(INADDR_ANY);
111 bind_addr.sin_port = htons(MODBUS_TCP_PORT);
112
113 if (bind(serv, (struct sockaddr *)&bind_addr, sizeof(bind_addr)) < 0) {
114 LOG_ERR("error: bind: %d", errno);
115 return 0;
116 }
117
118 if (listen(serv, 5) < 0) {
119 LOG_ERR("error: listen: %d", errno);
120 return 0;
121 }
122
123 LOG_INF("Started MODBUS TCP gateway example on port %d", MODBUS_TCP_PORT);
124
125 while (1) {
126 struct sockaddr_in client_addr;
127 socklen_t client_addr_len = sizeof(client_addr);
128 char addr_str[INET_ADDRSTRLEN];
129 int client;
130 int rc;
131
132 client = accept(serv, (struct sockaddr *)&client_addr,
133 &client_addr_len);
134
135 if (client < 0) {
136 LOG_ERR("error: accept: %d", errno);
137 continue;
138 }
139
140 inet_ntop(client_addr.sin_family, &client_addr.sin_addr,
141 addr_str, sizeof(addr_str));
142 LOG_INF("Connection #%d from %s",
143 counter++, addr_str);
144
145 do {
146 rc = modbus_tcp_connection(client);
147 } while (!rc);
148
149 close(client);
150 LOG_INF("Connection from %s closed, errno %d",
151 addr_str, rc);
152 }
153 return 0;
154 }
155
