From bf637bf6233f86a83eb3b0ed2de3b2c4fa87932f Mon Sep 17 00:00:00 2001 From: Marcin Niestroj Date: Tue, 16 Apr 2024 17:57:50 +0200 Subject: [PATCH] drivers: net: nsos: implement sendmsg() Implement sendmsg() socket API to have increased compatibility with components like MQTT client. Signed-off-by: Marcin Niestroj --- drivers/net/nsos.h | 16 ++++++++++++ drivers/net/nsos_adapt.c | 43 +++++++++++++++++++++++++++++++ drivers/net/nsos_sockets.c | 53 ++++++++++++++++++++++++++++++++++++-- 3 files changed, 110 insertions(+), 2 deletions(-) diff --git a/drivers/net/nsos.h b/drivers/net/nsos.h index e7ece3ded7..1ba4584a4a 100644 --- a/drivers/net/nsos.h +++ b/drivers/net/nsos.h @@ -89,6 +89,21 @@ struct nsos_mid_addrinfo { struct nsos_mid_addrinfo *ai_next; }; +struct nsos_mid_iovec { + void *iov_base; + size_t iov_len; +}; + +struct nsos_mid_msghdr { + void *msg_name; /* optional socket address, big endian */ + size_t msg_namelen; /* size of socket address */ + struct nsos_mid_iovec *msg_iov; /* scatter/gather array */ + size_t msg_iovlen; /* number of elements in msg_iov */ + void *msg_control; /* ancillary data */ + size_t msg_controllen; /* ancillary data buffer len */ + int msg_flags; /* flags on received message */ +}; + static inline void nsos_socket_flag_convert(int *flags_a, int flag_a, int *flags_b, int flag_b) { @@ -108,6 +123,7 @@ int nsos_adapt_listen(int fd, int backlog); int nsos_adapt_accept(int fd, struct nsos_mid_sockaddr *addr, size_t *addrlen); int nsos_adapt_sendto(int fd, const void *buf, size_t len, int flags, const struct nsos_mid_sockaddr *addr, size_t addrlen); +int nsos_adapt_sendmsg(int fd, const struct nsos_mid_msghdr *msg_mid, int flags); int nsos_adapt_recvfrom(int fd, void *buf, size_t len, int flags, struct nsos_mid_sockaddr *addr, size_t *addrlen); diff --git a/drivers/net/nsos_adapt.c b/drivers/net/nsos_adapt.c index 5f44b332f6..86aaa74368 100644 --- a/drivers/net/nsos_adapt.c +++ b/drivers/net/nsos_adapt.c @@ -445,6 +445,49 @@ int nsos_adapt_sendto(int fd, const void *buf, size_t len, int flags, return ret; } +int nsos_adapt_sendmsg(int fd, const struct nsos_mid_msghdr *msg_mid, int flags) +{ + struct sockaddr_storage addr_storage; + struct sockaddr *addr = (struct sockaddr *)&addr_storage; + struct msghdr msg; + struct iovec *msg_iov; + socklen_t addrlen; + int ret; + + ret = sockaddr_from_nsos_mid(&addr, &addrlen, msg_mid->msg_name, msg_mid->msg_namelen); + if (ret < 0) { + return ret; + } + + msg_iov = calloc(msg_mid->msg_iovlen, sizeof(*msg_iov)); + if (!msg_iov) { + ret = -ENOMEM; + return ret; + } + + for (size_t i = 0; i < msg_mid->msg_iovlen; i++) { + msg_iov[i].iov_base = msg_mid->msg_iov[i].iov_base; + msg_iov[i].iov_len = msg_mid->msg_iov[i].iov_len; + } + + msg.msg_name = addr; + msg.msg_namelen = addrlen; + msg.msg_iov = msg_iov; + msg.msg_iovlen = msg_mid->msg_iovlen; + msg.msg_control = NULL; + msg.msg_controllen = 0; + msg.msg_flags = 0; + + ret = sendmsg(fd, &msg, socket_flags_from_nsos_mid(flags) | MSG_NOSIGNAL); + if (ret < 0) { + ret = -errno_to_nsos_mid(errno); + } + + free(msg_iov); + + return ret; +} + int nsos_adapt_recvfrom(int fd, void *buf, size_t len, int flags, struct nsos_mid_sockaddr *addr_mid, size_t *addrlen_mid) { diff --git a/drivers/net/nsos_sockets.c b/drivers/net/nsos_sockets.c index e59d940b4d..c177342599 100644 --- a/drivers/net/nsos_sockets.c +++ b/drivers/net/nsos_sockets.c @@ -672,8 +672,57 @@ return_ret: static ssize_t nsos_sendmsg(void *obj, const struct msghdr *msg, int flags) { - errno = ENOTSUP; - return -1; + struct nsos_socket *sock = obj; + struct nsos_mid_sockaddr_storage addr_storage_mid; + struct nsos_mid_sockaddr *addr_mid = (struct nsos_mid_sockaddr *)&addr_storage_mid; + size_t addrlen_mid = sizeof(addr_storage_mid); + struct nsos_mid_msghdr msg_mid; + struct nsos_mid_iovec *msg_iov; + int flags_mid; + int ret; + + ret = socket_flags_to_nsos_mid(flags); + if (ret < 0) { + goto return_ret; + } + + flags_mid = ret; + + ret = sockaddr_to_nsos_mid(msg->msg_name, msg->msg_namelen, &addr_mid, &addrlen_mid); + if (ret < 0) { + goto return_ret; + } + + msg_iov = k_calloc(msg->msg_iovlen, sizeof(*msg_iov)); + if (!msg_iov) { + ret = -ENOMEM; + goto return_ret; + } + + for (size_t i = 0; i < msg->msg_iovlen; i++) { + msg_iov[i].iov_base = msg->msg_iov[i].iov_base; + msg_iov[i].iov_len = msg->msg_iov[i].iov_len; + } + + msg_mid.msg_name = addr_mid; + msg_mid.msg_namelen = addrlen_mid; + msg_mid.msg_iov = msg_iov; + msg_mid.msg_iovlen = msg->msg_iovlen; + msg_mid.msg_control = NULL; + msg_mid.msg_controllen = 0; + msg_mid.msg_flags = 0; + + ret = nsos_adapt_sendmsg(sock->pollfd.fd, &msg_mid, flags_mid); + + k_free(msg_iov); + +return_ret: + if (ret < 0) { + errno = errno_from_nsos_mid(-ret); + return -1; + } + + return ret; } static int nsos_recvfrom_with_poll(struct nsos_socket *sock, void *buf, size_t len, int flags,