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