1 /*
2 * Copyright (c) 2023 Nordic Semiconductor
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_echo_server_svc_sample, LOG_LEVEL_DBG);
9
10 #include <stdio.h>
11 #include <stdlib.h>
12 #include <errno.h>
13
14 #include <zephyr/kernel.h>
15 #include <zephyr/posix/unistd.h>
16 #include <zephyr/posix/poll.h>
17 #include <zephyr/posix/arpa/inet.h>
18 #include <zephyr/posix/sys/socket.h>
19 #include <zephyr/net/socket_service.h>
20
21 #define MY_PORT 4242
22
23 #define MAX_SERVICES (CONFIG_ZVFS_POLL_MAX - 3)
24
25 BUILD_ASSERT(MAX_SERVICES > 0, "Need at least 4 poll fds, increase CONFIG_ZVFS_POLL_MAX");
26
27 K_MUTEX_DEFINE(lock);
28 static struct pollfd sockfd_tcp[MAX_SERVICES];
29 static int tcp_socket = -1;
30 static int udp_socket = -1;
31
32 static void receive_data(bool is_udp, struct net_socket_service_event *pev,
33 char *buf, size_t buflen);
34 static void tcp_accept_handler(struct net_socket_service_event *pev);
35 static void restart_echo_service(void);
36
tcp_service_handler(struct net_socket_service_event * pev)37 static void tcp_service_handler(struct net_socket_service_event *pev)
38 {
39 static char buf[1500];
40
41 receive_data(false, pev, buf, sizeof(buf));
42 }
43
udp_service_handler(struct net_socket_service_event * pev)44 static void udp_service_handler(struct net_socket_service_event *pev)
45 {
46 static char buf[1500];
47
48 receive_data(true, pev, buf, sizeof(buf));
49 }
50
51 NET_SOCKET_SERVICE_SYNC_DEFINE_STATIC(service_udp, udp_service_handler, 1);
52 NET_SOCKET_SERVICE_SYNC_DEFINE_STATIC(service_accept, tcp_accept_handler, 1);
53 NET_SOCKET_SERVICE_SYNC_DEFINE_STATIC(service_tcp, tcp_service_handler, MAX_SERVICES);
54
client_list_init(void)55 static void client_list_init(void)
56 {
57 k_mutex_lock(&lock, K_FOREVER);
58 ARRAY_FOR_EACH_PTR(sockfd_tcp, sockfd) {
59 sockfd->fd = -1;
60 }
61 k_mutex_unlock(&lock);
62 }
63
client_list_deinit(void)64 static void client_list_deinit(void)
65 {
66 (void)net_socket_service_unregister(&service_tcp);
67
68 k_mutex_lock(&lock, K_FOREVER);
69 ARRAY_FOR_EACH_PTR(sockfd_tcp, sockfd) {
70 if (sockfd->fd != -1) {
71 close(sockfd->fd);
72 sockfd->fd = -1;
73 }
74 }
75 k_mutex_unlock(&lock);
76 }
77
client_list_try_add(int sock)78 static void client_list_try_add(int sock)
79 {
80 int ret;
81
82 k_mutex_lock(&lock, K_FOREVER);
83 ARRAY_FOR_EACH_PTR(sockfd_tcp, sockfd) {
84 if (sockfd->fd == -1) {
85 sockfd->fd = sock;
86 sockfd->events = POLLIN;
87
88 ret = net_socket_service_register(&service_tcp, sockfd_tcp,
89 ARRAY_SIZE(sockfd_tcp), NULL);
90 if (ret < 0) {
91 LOG_ERR("Cannot register socket service handler (%d). "
92 "Attempting to restart service.", ret);
93 restart_echo_service();
94 }
95 k_mutex_unlock(&lock);
96 return;
97 }
98 }
99 k_mutex_unlock(&lock);
100 LOG_WRN("Max capacity reached");
101 close(sock);
102 }
103
client_list_remove(int client)104 static void client_list_remove(int client)
105 {
106 int ret;
107
108 k_mutex_lock(&lock, K_FOREVER);
109 ARRAY_FOR_EACH_PTR(sockfd_tcp, sockfd) {
110 if (sockfd->fd == client) {
111 close(client);
112 sockfd->fd = -1;
113 ret = net_socket_service_register(&service_tcp, sockfd_tcp,
114 ARRAY_SIZE(sockfd_tcp), NULL);
115 if (ret < 0) {
116 LOG_ERR("Cannot register socket service handler (%d). "
117 "Attempting to restart service.", ret);
118 restart_echo_service();
119 }
120 break;
121 }
122 }
123 k_mutex_unlock(&lock);
124 }
125
receive_data(bool is_udp,struct net_socket_service_event * pev,char * buf,size_t buflen)126 static void receive_data(bool is_udp, struct net_socket_service_event *pev,
127 char *buf, size_t buflen)
128 {
129 struct pollfd *pfd = &pev->event;
130 int client = pfd->fd;
131 struct sockaddr_in6 addr;
132 char addr_str[INET6_ADDRSTRLEN];
133 socklen_t addrlen = sizeof(addr);
134 int len, out_len;
135 char *p;
136
137 len = recvfrom(client, buf, buflen, 0,
138 (struct sockaddr *)&addr, &addrlen);
139 if (len <= 0) {
140 if (len < 0) {
141 LOG_ERR("recv: %d", -errno);
142 }
143 if (!is_udp) {
144 client_list_remove(client);
145 inet_ntop(addr.sin6_family, &addr.sin6_addr, addr_str, sizeof(addr_str));
146 LOG_INF("Connection from %s closed", addr_str);
147 }
148 return;
149 }
150
151 p = buf;
152 do {
153 if (is_udp) {
154 out_len = sendto(client, p, len, 0,
155 (struct sockaddr *)&addr, addrlen);
156 } else {
157 out_len = send(client, p, len, 0);
158 }
159
160 if (out_len < 0) {
161 LOG_ERR("sendto: %d", -errno);
162 break;
163 }
164
165 p += out_len;
166 len -= out_len;
167 } while (len);
168 }
169
tcp_accept_handler(struct net_socket_service_event * pev)170 static void tcp_accept_handler(struct net_socket_service_event *pev)
171 {
172 int client;
173 int sock = pev->event.fd;
174 char addr_str[INET6_ADDRSTRLEN];
175 struct sockaddr_in6 client_addr;
176 socklen_t client_addr_len = sizeof(client_addr);
177
178 client = accept(sock, (struct sockaddr *)&client_addr, &client_addr_len);
179 if (client < 0) {
180 LOG_ERR("accept: %d. Restarting service.", -errno);
181 restart_echo_service();
182 return;
183 }
184
185 inet_ntop(client_addr.sin6_family, &client_addr.sin6_addr, addr_str, sizeof(addr_str));
186 LOG_INF("Connection from %s (%d)", addr_str, client);
187
188 client_list_try_add(client);
189 }
190
setup_tcp_socket(struct sockaddr_in6 * addr)191 static int setup_tcp_socket(struct sockaddr_in6 *addr)
192 {
193 socklen_t optlen = sizeof(int);
194 int ret, sock, opt;
195
196 sock = socket(AF_INET6, SOCK_STREAM, IPPROTO_TCP);
197 if (sock < 0) {
198 LOG_ERR("socket: %d", -errno);
199 return -errno;
200 }
201
202 ret = getsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &opt, &optlen);
203 if (ret == 0 && opt) {
204 LOG_INF("IPV6_V6ONLY option is on, turning it off.");
205
206 opt = 0;
207 ret = setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &opt, optlen);
208 if (ret < 0) {
209 LOG_WRN("Cannot turn off IPV6_V6ONLY option");
210 } else {
211 LOG_INF("Sharing same socket between IPv6 and IPv4");
212 }
213 }
214
215 if (bind(sock, (struct sockaddr *)addr, sizeof(*addr)) < 0) {
216 LOG_ERR("bind: %d", -errno);
217 close(sock);
218 return -errno;
219 }
220
221 if (listen(sock, 5) < 0) {
222 LOG_ERR("listen: %d", -errno);
223 close(sock);
224 return -errno;
225 }
226
227 return sock;
228 }
229
setup_udp_socket(struct sockaddr_in6 * addr)230 static int setup_udp_socket(struct sockaddr_in6 *addr)
231 {
232 socklen_t optlen = sizeof(int);
233 int ret, sock, opt;
234
235 sock = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
236 if (sock < 0) {
237 LOG_ERR("socket: %d", -errno);
238 return -errno;
239 }
240
241 ret = getsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &opt, &optlen);
242 if (ret == 0 && opt) {
243 LOG_INF("IPV6_V6ONLY option is on, turning it off.");
244
245 opt = 0;
246 ret = setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &opt, optlen);
247 if (ret < 0) {
248 LOG_WRN("Cannot turn off IPV6_V6ONLY option");
249 } else {
250 LOG_INF("Sharing same socket between IPv6 and IPv4");
251 }
252 }
253
254 if (bind(sock, (struct sockaddr *)addr, sizeof(*addr)) < 0) {
255 LOG_ERR("bind: %d", -errno);
256 close(sock);
257 return -errno;
258 }
259
260 return sock;
261 }
262
start_echo_service(void)263 static int start_echo_service(void)
264 {
265 struct pollfd sockfd_accept, sockfd_udp;
266 int tcp_sock, udp_sock, ret;
267 struct sockaddr_in6 addr = {
268 .sin6_family = AF_INET6,
269 .sin6_addr = IN6ADDR_ANY_INIT,
270 .sin6_port = htons(MY_PORT),
271 };
272
273 client_list_init();
274
275 tcp_sock = setup_tcp_socket(&addr);
276 if (tcp_sock < 0) {
277 LOG_ERR("Failed to setup tcp listening socket");
278 return tcp_sock;
279 }
280
281 udp_sock = setup_udp_socket(&addr);
282 if (udp_sock < 0) {
283 LOG_ERR("Failed to setup udp socket");
284 ret = udp_sock;
285 goto cleanup_tcp;
286 }
287
288 /* Register TCP listening socket to service handler */
289 sockfd_accept = (struct pollfd){ .fd = tcp_sock, .events = POLLIN };
290 ret = net_socket_service_register(&service_accept, &sockfd_accept, 1, NULL);
291 if (ret < 0) {
292 LOG_ERR("Cannot register socket service handler (%d)", ret);
293 goto cleanup_sockets;
294 }
295
296 /* Register UDP socket to service handler */
297 sockfd_udp = (struct pollfd){ .fd = udp_sock, .events = POLLIN };
298 ret = net_socket_service_register(&service_udp, &sockfd_udp, 1, NULL);
299 if (ret < 0) {
300 LOG_ERR("Cannot register socket service handler (%d)", ret);
301 goto cleanup_sockets;
302 }
303
304 k_mutex_lock(&lock, K_FOREVER);
305 tcp_socket = tcp_sock;
306 udp_socket = udp_sock;
307 k_mutex_unlock(&lock);
308
309 LOG_INF("Single-threaded TCP/UDP echo server waits "
310 "for a connection on port %d", MY_PORT);
311
312 return 0;
313
314 cleanup_sockets:
315 close(udp_sock);
316 cleanup_tcp:
317 close(tcp_sock);
318 return -1;
319
320 }
321
stop_echo_service(void)322 static int stop_echo_service(void)
323 {
324 client_list_deinit();
325 (void)net_socket_service_unregister(&service_udp);
326 (void)net_socket_service_unregister(&service_accept);
327
328 k_mutex_lock(&lock, K_FOREVER);
329 if (tcp_socket >= 0) {
330 close(tcp_socket);
331 tcp_socket = -1;
332 }
333 if (udp_socket >= 0) {
334 close(udp_socket);
335 udp_socket = -1;
336 }
337 k_mutex_unlock(&lock);
338
339 return 0;
340 }
341
restart_echo_service(void)342 static void restart_echo_service(void)
343 {
344 stop_echo_service();
345 start_echo_service();
346 }
347
348 SYS_INIT(start_echo_service, APPLICATION, 99);
349
