zephyr/subsys/mgmt/smp_udp.c

/*
 * Copyright (c) 2019, Prevas A/S
 *
 * SPDX-License-Identifier: Apache-2.0
 */

/**
 * @file
 * @brief UDP transport for the mcumgr SMP protocol.
 */

#include <zephyr.h>
#include <init.h>
#include <net/socket.h>
#include <errno.h>
#include <mgmt/mgmt.h>
#include <mgmt/smp_udp.h>
#include <mgmt/buf.h>
#include <mgmt/smp.h>

#define LOG_LEVEL CONFIG_MCUMGR_LOG_LEVEL
#include <logging/log.h>
LOG_MODULE_REGISTER(smp_udp);

struct config {
	int sock;
	const char *proto;
	struct zephyr_smp_transport smp_transport;
	char recv_buffer[CONFIG_MCUMGR_SMP_UDP_MTU];
	struct k_thread thread;
	K_THREAD_STACK_MEMBER(stack, CONFIG_MCUMGR_SMP_UDP_STACK_SIZE);
};

struct configs {
#if CONFIG_MCUMGR_SMP_UDP_IPV4
	struct config ipv4;
#endif
#if CONFIG_MCUMGR_SMP_UDP_IPV6
	struct config ipv6;
#endif
};

static struct configs configs = {
#if CONFIG_MCUMGR_SMP_UDP_IPV4
	.ipv4 = {
		.proto = "IPv4",
	},
#endif
#if CONFIG_MCUMGR_SMP_UDP_IPV6
	.ipv6 = {
		.proto = "IPv6",
	},
#endif
};

#if CONFIG_MCUMGR_SMP_UDP_IPV4
static int smp_udp4_tx(struct zephyr_smp_transport *zst, struct net_buf *nb)
{
	ARG_UNUSED(zst);

	struct sockaddr *addr = net_buf_user_data(nb);
	int ret = sendto(configs.ipv4.sock, nb->data, nb->len,
			 0, addr, sizeof(*addr));
	mcumgr_buf_free(nb);

	return ret < 0 ? MGMT_ERR_EINVAL : MGMT_ERR_EOK;
}
#endif

#if CONFIG_MCUMGR_SMP_UDP_IPV6
static int smp_udp6_tx(struct zephyr_smp_transport *zst, struct net_buf *nb)
{
	ARG_UNUSED(zst);

	struct sockaddr *addr = net_buf_user_data(nb);
	int ret = sendto(configs.ipv6.sock, nb->data, nb->len,
			 0, addr, sizeof(*addr));
	mcumgr_buf_free(nb);

	return ret < 0 ? MGMT_ERR_EINVAL : MGMT_ERR_EOK;
}
#endif

static uint16_t smp_udp_get_mtu(const struct net_buf *nb)
{
	ARG_UNUSED(nb);

	return CONFIG_MCUMGR_SMP_UDP_MTU;
}

static int smp_udp_ud_copy(struct net_buf *dst, const struct net_buf *src)
{
	struct sockaddr *src_ud = net_buf_user_data(src);
	struct sockaddr *dst_ud = net_buf_user_data(dst);

	net_ipaddr_copy(dst_ud, src_ud);

	return MGMT_ERR_EOK;
}

static void smp_udp_receive_thread(void *p1, void *p2, void *p3)
{
	struct config *conf = (struct config *)p1;

	ARG_UNUSED(p2);
	ARG_UNUSED(p3);

	LOG_INF("Started (%s)", conf->proto);

	while (1) {
		struct sockaddr addr;
		socklen_t addr_len = sizeof(addr);

		int len = recvfrom(conf->sock, conf->recv_buffer,
				   CONFIG_MCUMGR_SMP_UDP_MTU,
				   0, &addr, &addr_len);

		if (len > 0) {
			struct sockaddr *ud;
			struct net_buf *nb;

			/* store sender address in user data for reply */
			nb = mcumgr_buf_alloc();
			net_buf_add_mem(nb, conf->recv_buffer, len);
			ud = net_buf_user_data(nb);
			net_ipaddr_copy(ud, &addr);

			zephyr_smp_rx_req(&conf->smp_transport, nb);
		} else if (len < 0) {
			LOG_ERR("recvfrom error (%s): %i", conf->proto, errno);
		}
	}
}

static int smp_udp_init(struct device *dev)
{
	ARG_UNUSED(dev);

#if CONFIG_MCUMGR_SMP_UDP_IPV4
	zephyr_smp_transport_init(&configs.ipv4.smp_transport,
				  smp_udp4_tx, smp_udp_get_mtu,
				  smp_udp_ud_copy, NULL);
#endif

#if CONFIG_MCUMGR_SMP_UDP_IPV6
	zephyr_smp_transport_init(&configs.ipv6.smp_transport,
				  smp_udp6_tx, smp_udp_get_mtu,
				  smp_udp_ud_copy, NULL);
#endif

	return MGMT_ERR_EOK;
}

static int create_socket(struct sockaddr *addr, const char *proto)
{
	int sock = socket(addr->sa_family, SOCK_DGRAM, IPPROTO_UDP);

	if (sock < 0) {
		LOG_ERR("Could not open receive socket (%s), err: %i",
			proto, errno);

		return -errno;
	}

	if (bind(sock, addr, sizeof(*addr)) < 0) {
		LOG_ERR("Could not bind to receive socket (%s), err: %i",
			proto, errno);

		return -errno;
	}

	return sock;
}

static void create_thread(struct config *conf, const char *name)
{
	k_thread_create(&(conf->thread), conf->stack,
			K_THREAD_STACK_SIZEOF(conf->stack),
			smp_udp_receive_thread, conf, NULL, NULL,
			CONFIG_MCUMGR_SMP_UDP_THREAD_PRIO, 0, K_FOREVER);

	k_thread_name_set(&(conf->thread), name);
	k_thread_start(&(conf->thread));
}

SYS_INIT(smp_udp_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY);

int smp_udp_open(void)
{
	struct config *conf;

#if CONFIG_MCUMGR_SMP_UDP_IPV4
	struct sockaddr_in addr4;

	memset(&addr4, 0, sizeof(addr4));
	addr4.sin_family = AF_INET;
	addr4.sin_port = htons(CONFIG_MCUMGR_SMP_UDP_PORT);
	inet_pton(AF_INET, INADDR_ANY, &addr4.sin_addr);

	conf = &configs.ipv4;
	conf->sock = create_socket((struct sockaddr *)&addr4, conf->proto);

	if (conf->sock < 0) {
		return -MGMT_ERR_EUNKNOWN;
	}

	create_thread(conf, "smp_udp4");
#endif

#if CONFIG_MCUMGR_SMP_UDP_IPV6
	struct sockaddr_in6 addr6;

	memset(&addr6, 0, sizeof(addr6));
	addr6.sin6_family = AF_INET6;
	addr6.sin6_port = htons(CONFIG_MCUMGR_SMP_UDP_PORT);
	addr6.sin6_addr = in6addr_any;

	conf = &configs.ipv6;
	conf->sock = create_socket((struct sockaddr *)&addr6, conf->proto);

	if (conf->sock < 0) {
		return -MGMT_ERR_EUNKNOWN;
	}

	create_thread(conf, "smp_udp6");
#endif

	return MGMT_ERR_EOK;
}

int smp_udp_close(void)
{
#if CONFIG_MCUMGR_SMP_UDP_IPV4
	k_thread_abort(&(configs.ipv4.thread));
	close(configs.ipv4.sock);
#endif

#if CONFIG_MCUMGR_SMP_UDP_IPV6
	k_thread_abort(&(configs.ipv6.thread));
	close(configs.ipv6.sock);
#endif

	return MGMT_ERR_EOK;
}