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_storage addr_storage;
167 struct sockaddr *addr = (struct sockaddr *)&addr_storage;
168 socklen_t addr_len = 0;
169
170 if (IS_ENABLED(CONFIG_MCUMGR_TRANSPORT_UDP_IPV4) &&
171 proto == PROTOCOL_IPV4) {
172 struct sockaddr_in *addr4 = (struct sockaddr_in *)addr;
173
174 addr_len = sizeof(*addr4);
175 memset(addr4, 0, sizeof(*addr4));
176 addr4->sin_family = AF_INET;
177 addr4->sin_port = htons(CONFIG_MCUMGR_TRANSPORT_UDP_PORT);
178 addr4->sin_addr.s_addr = htonl(INADDR_ANY);
179 } else if (IS_ENABLED(CONFIG_MCUMGR_TRANSPORT_UDP_IPV6) &&
180 proto == PROTOCOL_IPV6) {
181 struct sockaddr_in6 *addr6 = (struct sockaddr_in6 *)addr;
182
183 addr_len = sizeof(*addr6);
184 memset(addr6, 0, sizeof(*addr6));
185 addr6->sin6_family = AF_INET6;
186 addr6->sin6_port = htons(CONFIG_MCUMGR_TRANSPORT_UDP_PORT);
187 addr6->sin6_addr = in6addr_any;
188 }
189
190 tmp_sock = zsock_socket(addr->sa_family, SOCK_DGRAM, IPPROTO_UDP);
191 err = errno;
192
193 if (tmp_sock < 0) {
194 LOG_ERR("Could not open receive socket (%s), err: %i",
195 smp_udp_proto_to_name(proto), err);
196
197 return -err;
198 }
199
200 if (zsock_bind(tmp_sock, addr, addr_len) < 0) {
201 err = errno;
202 LOG_ERR("Could not bind to receive socket (%s), err: %i",
203 smp_udp_proto_to_name(proto), err);
204
205 zsock_close(tmp_sock);
206
207 return -err;
208 }
209
210 *sock = tmp_sock;
211 return 0;
212 }
213
smp_udp_receive_thread(void * p1,void * p2,void * p3)214 static void smp_udp_receive_thread(void *p1, void *p2, void *p3)
215 {
216 struct config *conf = (struct config *)p1;
217 int rc;
218
219 ARG_UNUSED(p2);
220 ARG_UNUSED(p3);
221
222 (void)k_sem_take(&conf->network_ready_sem, K_FOREVER);
223 rc = create_socket(conf->proto, &conf->sock);
224
225 if (rc < 0) {
226 return;
227 }
228
229 __ASSERT(rc >= 0, "Socket is invalid");
230 LOG_INF("Started (%s)", smp_udp_proto_to_name(conf->proto));
231
232 while (1) {
233 struct sockaddr addr;
234 socklen_t addr_len = sizeof(addr);
235
236 int len = zsock_recvfrom(conf->sock, conf->recv_buffer,
237 CONFIG_MCUMGR_TRANSPORT_UDP_MTU, 0, &addr, &addr_len);
238
239 if (len > 0) {
240 struct sockaddr *ud;
241 struct net_buf *nb;
242
243 /* Store sender address in user data for reply */
244 nb = smp_packet_alloc();
245 if (!nb) {
246 LOG_ERR("Failed to allocate mcumgr buffer");
247 /* No free space, drop SMP frame */
248 continue;
249 }
250 net_buf_add_mem(nb, conf->recv_buffer, len);
251 ud = net_buf_user_data(nb);
252 net_ipaddr_copy(ud, &addr);
253
254 smp_rx_req(&conf->smp_transport, nb);
255 } else if (len < 0) {
256 LOG_ERR("recvfrom error (%s): %i, %d", smp_udp_proto_to_name(conf->proto),
257 errno, len);
258 }
259 }
260 }
261
smp_udp_open_iface(struct net_if * iface,void * user_data)262 static void smp_udp_open_iface(struct net_if *iface, void *user_data)
263 {
264 ARG_UNUSED(user_data);
265
266 if (net_if_is_up(iface)) {
267 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
268 if (net_if_flag_is_set(iface, NET_IF_IPV4) &&
269 k_thread_join(&smp_udp_configs.ipv4.thread, K_NO_WAIT) == -EBUSY) {
270 k_sem_give(&smp_udp_configs.ipv4.network_ready_sem);
271 }
272 #endif
273
274 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
275 if (net_if_flag_is_set(iface, NET_IF_IPV6) &&
276 k_thread_join(&smp_udp_configs.ipv6.thread, K_NO_WAIT) == -EBUSY) {
277 k_sem_give(&smp_udp_configs.ipv6.network_ready_sem);
278 }
279 #endif
280 }
281 }
282
smp_udp_net_event_handler(struct net_mgmt_event_callback * cb,uint32_t mgmt_event,struct net_if * iface)283 static void smp_udp_net_event_handler(struct net_mgmt_event_callback *cb, uint32_t mgmt_event,
284 struct net_if *iface)
285 {
286 ARG_UNUSED(cb);
287
288 if (mgmt_event == NET_EVENT_IF_UP) {
289 smp_udp_open_iface(iface, NULL);
290 }
291 }
292
create_thread(struct config * conf,const char * name)293 static void create_thread(struct config *conf, const char *name)
294 {
295 k_thread_create(&(conf->thread), conf->stack,
296 K_KERNEL_STACK_SIZEOF(conf->stack),
297 smp_udp_receive_thread, conf, NULL, NULL,
298 CONFIG_MCUMGR_TRANSPORT_UDP_THREAD_PRIO, 0, K_FOREVER);
299
300 k_thread_name_set(&(conf->thread), name);
301 k_thread_start(&(conf->thread));
302 }
303
smp_udp_open(void)304 int smp_udp_open(void)
305 {
306 bool started = false;
307
308 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
309 if (k_thread_join(&smp_udp_configs.ipv4.thread, K_NO_WAIT) == 0 ||
310 threads_created == false) {
311 (void)k_sem_reset(&smp_udp_configs.ipv4.network_ready_sem);
312 create_thread(&smp_udp_configs.ipv4, "smp_udp4");
313 started = true;
314 } else {
315 LOG_ERR("IPv4 UDP MCUmgr thread is already running");
316 }
317 #endif
318
319 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
320 if (k_thread_join(&smp_udp_configs.ipv6.thread, K_NO_WAIT) == 0 ||
321 threads_created == false) {
322 (void)k_sem_reset(&smp_udp_configs.ipv6.network_ready_sem);
323 create_thread(&smp_udp_configs.ipv6, "smp_udp6");
324 started = true;
325 } else {
326 LOG_ERR("IPv6 UDP MCUmgr thread is already running");
327 }
328 #endif
329
330 if (started) {
331 /* One or more threads were started, check existing interfaces */
332 threads_created = true;
333 net_if_foreach(smp_udp_open_iface, NULL);
334 }
335
336 return 0;
337 }
338
smp_udp_close(void)339 int smp_udp_close(void)
340 {
341 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
342 if (k_thread_join(&smp_udp_configs.ipv4.thread, K_NO_WAIT) == -EBUSY) {
343 k_thread_abort(&(smp_udp_configs.ipv4.thread));
344
345 if (smp_udp_configs.ipv4.sock >= 0) {
346 zsock_close(smp_udp_configs.ipv4.sock);
347 smp_udp_configs.ipv4.sock = -1;
348 }
349 } else {
350 LOG_ERR("IPv4 UDP MCUmgr thread is not running");
351 }
352 #endif
353
354 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
355 if (k_thread_join(&smp_udp_configs.ipv6.thread, K_NO_WAIT) == -EBUSY) {
356 k_thread_abort(&(smp_udp_configs.ipv6.thread));
357
358 if (smp_udp_configs.ipv6.sock >= 0) {
359 zsock_close(smp_udp_configs.ipv6.sock);
360 smp_udp_configs.ipv6.sock = -1;
361 }
362 } else {
363 LOG_ERR("IPv6 UDP MCUmgr thread is not running");
364 }
365 #endif
366
367 return 0;
368 }
369
smp_udp_start(void)370 static void smp_udp_start(void)
371 {
372 int rc;
373
374 threads_created = false;
375
376 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV4
377 smp_udp_configs.ipv4.proto = PROTOCOL_IPV4;
378 smp_udp_configs.ipv4.sock = -1;
379
380 k_sem_init(&smp_udp_configs.ipv4.network_ready_sem, 0, 1);
381 smp_udp_configs.ipv4.smp_transport.functions.output = smp_udp4_tx;
382 smp_udp_configs.ipv4.smp_transport.functions.get_mtu = smp_udp_get_mtu;
383 smp_udp_configs.ipv4.smp_transport.functions.ud_copy = smp_udp_ud_copy;
384
385 rc = smp_transport_init(&smp_udp_configs.ipv4.smp_transport);
386 #ifdef CONFIG_SMP_CLIENT
387 if (rc == 0) {
388 smp_udp_configs.ipv4_transport.smpt = &smp_udp_configs.ipv4.smp_transport;
389 smp_udp_configs.ipv4_transport.smpt_type = SMP_UDP_IPV4_TRANSPORT;
390 smp_client_transport_register(&smp_udp_configs.ipv4_transport);
391 }
392 #endif
393 if (rc) {
394 LOG_ERR("Failed to register IPv4 UDP MCUmgr SMP transport: %d", rc);
395 }
396 #endif
397
398 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_IPV6
399 smp_udp_configs.ipv6.proto = PROTOCOL_IPV6;
400 smp_udp_configs.ipv6.sock = -1;
401
402 k_sem_init(&smp_udp_configs.ipv6.network_ready_sem, 0, 1);
403 smp_udp_configs.ipv6.smp_transport.functions.output = smp_udp6_tx;
404 smp_udp_configs.ipv6.smp_transport.functions.get_mtu = smp_udp_get_mtu;
405 smp_udp_configs.ipv6.smp_transport.functions.ud_copy = smp_udp_ud_copy;
406
407 rc = smp_transport_init(&smp_udp_configs.ipv6.smp_transport);
408 #ifdef CONFIG_SMP_CLIENT
409 if (rc == 0) {
410 smp_udp_configs.ipv6_transport.smpt = &smp_udp_configs.ipv6.smp_transport;
411 smp_udp_configs.ipv6_transport.smpt_type = SMP_UDP_IPV6_TRANSPORT;
412 smp_client_transport_register(&smp_udp_configs.ipv6_transport);
413 }
414 #endif
415
416 if (rc) {
417 LOG_ERR("Failed to register IPv6 UDP MCUmgr SMP transport: %d", rc);
418 }
419 #endif
420
421 net_mgmt_init_event_callback(&smp_udp_mgmt_cb, smp_udp_net_event_handler, NET_EVENT_IF_UP);
422 net_mgmt_add_event_callback(&smp_udp_mgmt_cb);
423
424 #ifdef CONFIG_MCUMGR_TRANSPORT_UDP_AUTOMATIC_INIT
425 smp_udp_open();
426 #endif
427 }
428
429 MCUMGR_HANDLER_DEFINE(smp_udp, smp_udp_start);
430
