1 /*
2 * Copyright (c) 2019-2020, Prevas A/S
3 * Copyright (c) 2022-2023 Nordic Semiconductor ASA
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 /**
9 * @file
10 * @brief UDP transport for the mcumgr SMP protocol.
11 */
12
13 #include <assert.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/init.h>
16 #include <zephyr/net/socket.h>
17 #include <zephyr/net/net_if.h>
18 #include <zephyr/mgmt/mcumgr/mgmt/mgmt.h>
19 #include <zephyr/mgmt/mcumgr/smp/smp.h>
20 #include <zephyr/mgmt/mcumgr/transport/smp.h>
21 #include <zephyr/mgmt/mcumgr/transport/smp_udp.h>
22 #include <zephyr/mgmt/mcumgr/mgmt/handlers.h>
23 #include <zephyr/net/net_mgmt.h>
24 #include <zephyr/net/net_event.h>
25 #include <zephyr/net/conn_mgr_monitor.h>
26 #include <errno.h>
27
28 #include <mgmt/mcumgr/transport/smp_internal.h>
29
30 #define LOG_LEVEL CONFIG_MCUMGR_LOG_LEVEL
31 #include <zephyr/logging/log.h>
32 LOG_MODULE_REGISTER(smp_udp);
33
34 BUILD_ASSERT(CONFIG_MCUMGR_TRANSPORT_UDP_MTU != 0, "CONFIG_MCUMGR_TRANSPORT_UDP_MTU must be > 0");
35
36 #if !defined(CONFIG_MCUMGR_TRANSPORT_UDP_IPV4) && !defined(CONFIG_MCUMGR_TRANSPORT_UDP_IPV6)
37 BUILD_ASSERT(0, "Either IPv4 or IPv6 SMP must be enabled for the MCUmgr UDP SMP transport using "
38 "CONFIG_MCUMGR_TRANSPORT_UDP_IPV4 or CONFIG_MCUMGR_TRANSPORT_UDP_IPV6");
39 #endif
40
41 BUILD_ASSERT(sizeof(struct sockaddr) <= CONFIG_MCUMGR_TRANSPORT_NETBUF_USER_DATA_SIZE,
42 "CONFIG_MCUMGR_TRANSPORT_NETBUF_USER_DATA_SIZE must be >= sizeof(struct sockaddr)");
43
44 enum proto_type {
45 PROTOCOL_IPV4 = 0,
46 PROTOCOL_IPV6,
47 };
48
49 struct config {
50 int sock;
51 enum proto_type proto;
52 struct k_sem network_ready_sem;
53 struct smp_transport smp_transport;
54 char recv_buffer[CONFIG_MCUMGR_TRANSPORT_UDP_MTU];
55 struct k_thread thread;
56 K_KERNEL_STACK_MEMBER(stack, CONFIG_MCUMGR_TRANSPORT_UDP_STACK_SIZE);
57 };
58
59 struct configs {
60 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
61 struct config ipv4;
62 #ifdef CONFIG_SMP_CLIENT
63 struct smp_client_transport_entry ipv4_transport;
64 #endif
65 #endif
66 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
67 struct config ipv6;
68 #ifdef CONFIG_SMP_CLIENT
69 struct smp_client_transport_entry ipv6_transport;
70 #endif
71 #endif
72 };
73
74 static bool threads_created;
75
76 static struct configs smp_udp_configs;
77
78 static struct net_mgmt_event_callback smp_udp_mgmt_cb;
79
smp_udp_proto_to_name(enum proto_type proto)80 static const char *smp_udp_proto_to_name(enum proto_type proto)
81 {
82 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
83 if (proto == PROTOCOL_IPV4) {
84 return "IPv4";
85 }
86 #endif
87
88 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
89 if (proto == PROTOCOL_IPV6) {
90 return "IPv6";
91 }
92 #endif
93
94 return "??";
95 }
96
97 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
smp_udp4_tx(struct net_buf * nb)98 static int smp_udp4_tx(struct net_buf *nb)
99 {
100 int ret;
101 struct sockaddr *addr = net_buf_user_data(nb);
102
103 ret = zsock_sendto(smp_udp_configs.ipv4.sock, nb->data, nb->len, 0, addr, sizeof(*addr));
104
105 if (ret < 0) {
106 if (errno == ENOMEM) {
107 ret = MGMT_ERR_EMSGSIZE;
108 } else {
109 ret = MGMT_ERR_EINVAL;
110 }
111 } else {
112 ret = MGMT_ERR_EOK;
113 }
114
115 smp_packet_free(nb);
116
117 return ret;
118 }
119 #endif
120
121 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
smp_udp6_tx(struct net_buf * nb)122 static int smp_udp6_tx(struct net_buf *nb)
123 {
124 int ret;
125 struct sockaddr *addr = net_buf_user_data(nb);
126
127 ret = zsock_sendto(smp_udp_configs.ipv6.sock, nb->data, nb->len, 0, addr, sizeof(*addr));
128
129 if (ret < 0) {
130 if (errno == ENOMEM) {
131 ret = MGMT_ERR_EMSGSIZE;
132 } else {
133 ret = MGMT_ERR_EINVAL;
134 }
135 } else {
136 ret = MGMT_ERR_EOK;
137 }
138
139 smp_packet_free(nb);
140
141 return ret;
142 }
143 #endif
144
smp_udp_get_mtu(const struct net_buf * nb)145 static uint16_t smp_udp_get_mtu(const struct net_buf *nb)
146 {
147 ARG_UNUSED(nb);
148
149 return CONFIG_MCUMGR_TRANSPORT_UDP_MTU;
150 }
151
smp_udp_ud_copy(struct net_buf * dst,const struct net_buf * src)152 static int smp_udp_ud_copy(struct net_buf *dst, const struct net_buf *src)
153 {
154 struct sockaddr *src_ud = net_buf_user_data(src);
155 struct sockaddr *dst_ud = net_buf_user_data(dst);
156
157 net_ipaddr_copy(dst_ud, src_ud);
158
159 return MGMT_ERR_EOK;
160 }
161
create_socket(enum proto_type proto,int * sock)162 static int create_socket(enum proto_type proto, int *sock)
163 {
164 int tmp_sock;
165 int err;
166 struct sockaddr *addr;
167 socklen_t addr_len = 0;
168
169 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
170 struct sockaddr_in addr4;
171 #endif
172
173 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
174 struct sockaddr_in6 addr6;
175 #endif
176
177 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
178 if (proto == PROTOCOL_IPV4) {
179 addr_len = sizeof(struct sockaddr_in);
180 memset(&addr4, 0, sizeof(addr4));
181 addr4.sin_family = AF_INET;
182 addr4.sin_port = htons(CONFIG_MCUMGR_TRANSPORT_UDP_PORT);
183 addr4.sin_addr.s_addr = htonl(INADDR_ANY);
184 addr = (struct sockaddr *)&addr4;
185 }
186 #endif
187
188 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
189 if (proto == PROTOCOL_IPV6) {
190 addr_len = sizeof(struct sockaddr_in6);
191 memset(&addr6, 0, sizeof(addr6));
192 addr6.sin6_family = AF_INET6;
193 addr6.sin6_port = htons(CONFIG_MCUMGR_TRANSPORT_UDP_PORT);
194 addr6.sin6_addr = in6addr_any;
195 addr = (struct sockaddr *)&addr6;
196 }
197 #endif
198
199 tmp_sock = zsock_socket(addr->sa_family, SOCK_DGRAM, IPPROTO_UDP);
200 err = errno;
201
202 if (tmp_sock < 0) {
203 LOG_ERR("Could not open receive socket (%s), err: %i",
204 smp_udp_proto_to_name(proto), err);
205
206 return -err;
207 }
208
209 if (zsock_bind(tmp_sock, addr, addr_len) < 0) {
210 err = errno;
211 LOG_ERR("Could not bind to receive socket (%s), err: %i",
212 smp_udp_proto_to_name(proto), err);
213
214 zsock_close(tmp_sock);
215
216 return -err;
217 }
218
219 *sock = tmp_sock;
220 return 0;
221 }
222
smp_udp_receive_thread(void * p1,void * p2,void * p3)223 static void smp_udp_receive_thread(void *p1, void *p2, void *p3)
224 {
225 struct config *conf = (struct config *)p1;
226 int rc;
227
228 ARG_UNUSED(p2);
229 ARG_UNUSED(p3);
230
231 (void)k_sem_take(&conf->network_ready_sem, K_FOREVER);
232 rc = create_socket(conf->proto, &conf->sock);
233
234 if (rc < 0) {
235 return;
236 }
237
238 __ASSERT(rc >= 0, "Socket is invalid");
239 LOG_INF("Started (%s)", smp_udp_proto_to_name(conf->proto));
240
241 while (1) {
242 struct sockaddr addr;
243 socklen_t addr_len = sizeof(addr);
244
245 int len = zsock_recvfrom(conf->sock, conf->recv_buffer,
246 CONFIG_MCUMGR_TRANSPORT_UDP_MTU, 0, &addr, &addr_len);
247
248 if (len > 0) {
249 struct sockaddr *ud;
250 struct net_buf *nb;
251
252 /* Store sender address in user data for reply */
253 nb = smp_packet_alloc();
254 if (!nb) {
255 LOG_ERR("Failed to allocate mcumgr buffer");
256 /* No free space, drop SMP frame */
257 continue;
258 }
259 net_buf_add_mem(nb, conf->recv_buffer, len);
260 ud = net_buf_user_data(nb);
261 net_ipaddr_copy(ud, &addr);
262
263 smp_rx_req(&conf->smp_transport, nb);
264 } else if (len < 0) {
265 LOG_ERR("recvfrom error (%s): %i, %d", smp_udp_proto_to_name(conf->proto),
266 errno, len);
267 }
268 }
269 }
270
smp_udp_open_iface(struct net_if * iface,void * user_data)271 static void smp_udp_open_iface(struct net_if *iface, void *user_data)
272 {
273 ARG_UNUSED(user_data);
274
275 if (net_if_is_up(iface)) {
276 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
277 if (net_if_flag_is_set(iface, NET_IF_IPV4) &&
278 k_thread_join(&smp_udp_configs.ipv4.thread, K_NO_WAIT) == -EBUSY) {
279 k_sem_give(&smp_udp_configs.ipv4.network_ready_sem);
280 }
281 #endif
282
283 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
284 if (net_if_flag_is_set(iface, NET_IF_IPV6) &&
285 k_thread_join(&smp_udp_configs.ipv6.thread, K_NO_WAIT) == -EBUSY) {
286 k_sem_give(&smp_udp_configs.ipv6.network_ready_sem);
287 }
288 #endif
289 }
290 }
291
smp_udp_net_event_handler(struct net_mgmt_event_callback * cb,uint32_t mgmt_event,struct net_if * iface)292 static void smp_udp_net_event_handler(struct net_mgmt_event_callback *cb, uint32_t mgmt_event,
293 struct net_if *iface)
294 {
295 ARG_UNUSED(cb);
296
297 if (mgmt_event == NET_EVENT_IF_UP) {
298 smp_udp_open_iface(iface, NULL);
299 }
300 }
301
create_thread(struct config * conf,const char * name)302 static void create_thread(struct config *conf, const char *name)
303 {
304 k_thread_create(&(conf->thread), conf->stack,
305 K_KERNEL_STACK_SIZEOF(conf->stack),
306 smp_udp_receive_thread, conf, NULL, NULL,
307 CONFIG_MCUMGR_TRANSPORT_UDP_THREAD_PRIO, 0, K_FOREVER);
308
309 k_thread_name_set(&(conf->thread), name);
310 k_thread_start(&(conf->thread));
311 }
312
smp_udp_open(void)313 int smp_udp_open(void)
314 {
315 bool started = false;
316
317 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
318 if (k_thread_join(&smp_udp_configs.ipv4.thread, K_NO_WAIT) == 0 ||
319 threads_created == false) {
320 (void)k_sem_reset(&smp_udp_configs.ipv4.network_ready_sem);
321 create_thread(&smp_udp_configs.ipv4, "smp_udp4");
322 started = true;
323 } else {
324 LOG_ERR("IPv4 UDP MCUmgr thread is already running");
325 }
326 #endif
327
328 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
329 if (k_thread_join(&smp_udp_configs.ipv6.thread, K_NO_WAIT) == 0 ||
330 threads_created == false) {
331 (void)k_sem_reset(&smp_udp_configs.ipv6.network_ready_sem);
332 create_thread(&smp_udp_configs.ipv6, "smp_udp6");
333 started = true;
334 } else {
335 LOG_ERR("IPv6 UDP MCUmgr thread is already running");
336 }
337 #endif
338
339 if (started) {
340 /* One or more threads were started, check existing interfaces */
341 threads_created = true;
342 net_if_foreach(smp_udp_open_iface, NULL);
343 }
344
345 return 0;
346 }
347
smp_udp_close(void)348 int smp_udp_close(void)
349 {
350 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
351 if (k_thread_join(&smp_udp_configs.ipv4.thread, K_NO_WAIT) == -EBUSY) {
352 k_thread_abort(&(smp_udp_configs.ipv4.thread));
353
354 if (smp_udp_configs.ipv4.sock >= 0) {
355 zsock_close(smp_udp_configs.ipv4.sock);
356 smp_udp_configs.ipv4.sock = -1;
357 }
358 } else {
359 LOG_ERR("IPv4 UDP MCUmgr thread is not running");
360 }
361 #endif
362
363 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
364 if (k_thread_join(&smp_udp_configs.ipv6.thread, K_NO_WAIT) == -EBUSY) {
365 k_thread_abort(&(smp_udp_configs.ipv6.thread));
366
367 if (smp_udp_configs.ipv6.sock >= 0) {
368 zsock_close(smp_udp_configs.ipv6.sock);
369 smp_udp_configs.ipv6.sock = -1;
370 }
371 } else {
372 LOG_ERR("IPv6 UDP MCUmgr thread is not running");
373 }
374 #endif
375
376 return 0;
377 }
378
smp_udp_start(void)379 static void smp_udp_start(void)
380 {
381 int rc;
382
383 threads_created = false;
384
385 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
386 smp_udp_configs.ipv4.proto = PROTOCOL_IPV4;
387 smp_udp_configs.ipv4.sock = -1;
388
389 k_sem_init(&smp_udp_configs.ipv4.network_ready_sem, 0, 1);
390 smp_udp_configs.ipv4.smp_transport.functions.output = smp_udp4_tx;
391 smp_udp_configs.ipv4.smp_transport.functions.get_mtu = smp_udp_get_mtu;
392 smp_udp_configs.ipv4.smp_transport.functions.ud_copy = smp_udp_ud_copy;
393
394 rc = smp_transport_init(&smp_udp_configs.ipv4.smp_transport);
395 #ifdef CONFIG_SMP_CLIENT
396 if (rc == 0) {
397 smp_udp_configs.ipv4_transport.smpt = &smp_udp_configs.ipv4.smp_transport;
398 smp_udp_configs.ipv4_transport.smpt_type = SMP_UDP_IPV4_TRANSPORT;
399 smp_client_transport_register(&smp_udp_configs.ipv4_transport);
400 }
401 #endif
402 if (rc) {
403 LOG_ERR("Failed to register IPv4 UDP MCUmgr SMP transport: %d", rc);
404 }
405 #endif
406
407 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
408 smp_udp_configs.ipv6.proto = PROTOCOL_IPV6;
409 smp_udp_configs.ipv6.sock = -1;
410
411 k_sem_init(&smp_udp_configs.ipv6.network_ready_sem, 0, 1);
412 smp_udp_configs.ipv6.smp_transport.functions.output = smp_udp6_tx;
413 smp_udp_configs.ipv6.smp_transport.functions.get_mtu = smp_udp_get_mtu;
414 smp_udp_configs.ipv6.smp_transport.functions.ud_copy = smp_udp_ud_copy;
415
416 rc = smp_transport_init(&smp_udp_configs.ipv6.smp_transport);
417 #ifdef CONFIG_SMP_CLIENT
418 if (rc == 0) {
419 smp_udp_configs.ipv6_transport.smpt = &smp_udp_configs.ipv6.smp_transport;
420 smp_udp_configs.ipv6_transport.smpt_type = SMP_UDP_IPV6_TRANSPORT;
421 smp_client_transport_register(&smp_udp_configs.ipv6_transport);
422 }
423 #endif
424
425 if (rc) {
426 LOG_ERR("Failed to register IPv6 UDP MCUmgr SMP transport: %d", rc);
427 }
428 #endif
429
430 net_mgmt_init_event_callback(&smp_udp_mgmt_cb, smp_udp_net_event_handler, NET_EVENT_IF_UP);
431 net_mgmt_add_event_callback(&smp_udp_mgmt_cb);
432
433 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_AUTOMATIC_INIT
434 smp_udp_open();
435 #endif
436 }
437
438 MCUMGR_HANDLER_DEFINE(smp_udp, smp_udp_start);
439
