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