diff --git a/drivers/modem/modem_socket.c b/drivers/modem/modem_socket.c index c5c649d8e08..799d7f25a4e 100644 --- a/drivers/modem/modem_socket.c +++ b/drivers/modem/modem_socket.c @@ -88,6 +88,7 @@ int modem_socket_packet_size_update(struct modem_socket_config *cfg, struct mode /* reset outstanding value here */ sock->packet_count = 0U; sock->packet_sizes[0] = 0U; + k_poll_signal_reset(&sock->sig_data_ready); k_sem_give(&cfg->sem_lock); return 0; } @@ -129,6 +130,11 @@ int modem_socket_packet_size_update(struct modem_socket_config *cfg, struct mode } data_ready: + if (sock->packet_sizes[0]) { + k_poll_signal_raise(&sock->sig_data_ready, 0); + } else { + k_poll_signal_reset(&sock->sig_data_ready); + } k_sem_give(&cfg->sem_lock); return new_total; } @@ -231,14 +237,13 @@ void modem_socket_put(struct modem_socket_config *cfg, int sock_fd) sock->id = cfg->base_socket_num - 1; sock->sock_fd = -1; sock->is_waiting = false; - sock->is_polled = false; sock->is_connected = false; (void)memset(&sock->src, 0, sizeof(struct sockaddr)); (void)memset(&sock->dst, 0, sizeof(struct sockaddr)); memset(&sock->packet_sizes, 0, sizeof(sock->packet_sizes)); sock->packet_count = 0; k_sem_reset(&sock->sem_data_ready); - k_poll_signal_reset(&sock->sem_poll); + k_poll_signal_reset(&sock->sig_data_ready); k_sem_give(&cfg->sem_lock); } @@ -278,12 +283,8 @@ int modem_socket_poll(struct modem_socket_config *cfg, struct zsock_pollfd *fds, found_count++; break; } else if (fds[i].events & ZSOCK_POLLIN) { - k_sem_take(&cfg->sem_lock, K_FOREVER); - sock->is_polled = true; - k_poll_signal_reset(&sock->sem_poll); k_poll_event_init(&events[eventcount++], K_POLL_TYPE_SIGNAL, - K_POLL_MODE_NOTIFY_ONLY, &sock->sem_poll); - k_sem_give(&cfg->sem_lock); + K_POLL_MODE_NOTIFY_ONLY, &sock->sig_data_ready); if (sock->packet_sizes[0] > 0U) { found_count++; break; @@ -322,8 +323,6 @@ int modem_socket_poll(struct modem_socket_config *cfg, struct zsock_pollfd *fds, fds[i].revents |= ZSOCK_POLLIN; found_count++; } - - sock->is_polled = false; } /* EBUSY, EAGAIN and ETIMEDOUT aren't true errors */ @@ -336,6 +335,57 @@ int modem_socket_poll(struct modem_socket_config *cfg, struct zsock_pollfd *fds, return found_count; } +int modem_socket_poll_prepare(struct modem_socket_config *cfg, struct modem_socket *sock, + struct zsock_pollfd *pfd, struct k_poll_event **pev, + struct k_poll_event *pev_end) +{ + if (pfd->events & ZSOCK_POLLIN) { + if (*pev == pev_end) { + errno = ENOMEM; + return -1; + } + + k_poll_event_init(*pev, K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, + &sock->sig_data_ready); + (*pev)++; + } + + if (pfd->events & ZSOCK_POLLOUT) { + if (*pev == pev_end) { + errno = ENOMEM; + return -1; + } + /* Not Implemented */ + errno = ENOTSUP; + return -1; + } + + return 0; +} + +int modem_socket_poll_update(struct modem_socket *sock, struct zsock_pollfd *pfd, + struct k_poll_event **pev) +{ + ARG_UNUSED(sock); + + if (pfd->events & ZSOCK_POLLIN) { + if ((*pev)->state != K_POLL_STATE_NOT_READY) { + pfd->revents |= ZSOCK_POLLIN; + } + (*pev)++; + } + + if (pfd->events & ZSOCK_POLLOUT) { + /* Not implemented, but the modem socket is always ready to transmit, + * so set the revents + */ + pfd->revents |= ZSOCK_POLLOUT; + (*pev)++; + } + + return 0; +} + void modem_socket_wait_data(struct modem_socket_config *cfg, struct modem_socket *sock) { k_sem_take(&cfg->sem_lock, K_FOREVER); @@ -355,11 +405,6 @@ void modem_socket_data_ready(struct modem_socket_config *cfg, struct modem_socke k_sem_give(&sock->sem_data_ready); } - if (sock->is_polled) { - /* unblock poll() */ - k_poll_signal_raise(&sock->sem_poll, 0); - } - k_sem_give(&cfg->sem_lock); } @@ -370,7 +415,7 @@ int modem_socket_init(struct modem_socket_config *cfg, const struct socket_op_vt k_sem_init(&cfg->sem_lock, 1, 1); for (i = 0; i < cfg->sockets_len; i++) { k_sem_init(&cfg->sockets[i].sem_data_ready, 0, 1); - k_poll_signal_init(&cfg->sockets[i].sem_poll); + k_poll_signal_init(&cfg->sockets[i].sig_data_ready); cfg->sockets[i].id = cfg->base_socket_num - 1; } diff --git a/drivers/modem/modem_socket.h b/drivers/modem/modem_socket.h index f01658ac0e9..d617e59978b 100644 --- a/drivers/modem/modem_socket.h +++ b/drivers/modem/modem_socket.h @@ -39,12 +39,11 @@ __net_socket struct modem_socket { /** data ready semaphore */ struct k_sem sem_data_ready; /** data ready poll signal */ - struct k_poll_signal sem_poll; + struct k_poll_signal sig_data_ready; /** socket state */ bool is_connected; bool is_waiting; - bool is_polled; /** temporary socket data */ void *data; @@ -72,6 +71,11 @@ struct modem_socket *modem_socket_from_newid(struct modem_socket_config *cfg); void modem_socket_put(struct modem_socket_config *cfg, int sock_fd); int modem_socket_poll(struct modem_socket_config *cfg, struct zsock_pollfd *fds, int nfds, int msecs); +int modem_socket_poll_update(struct modem_socket *sock, struct zsock_pollfd *pfd, + struct k_poll_event **pev); +int modem_socket_poll_prepare(struct modem_socket_config *cfg, struct modem_socket *sock, + struct zsock_pollfd *pfd, struct k_poll_event **pev, + struct k_poll_event *pev_end); void modem_socket_wait_data(struct modem_socket_config *cfg, struct modem_socket *sock); void modem_socket_data_ready(struct modem_socket_config *cfg, struct modem_socket *sock); int modem_socket_init(struct modem_socket_config *cfg, const struct socket_op_vtable *vtable); diff --git a/drivers/modem/quectel-bg9x.c b/drivers/modem/quectel-bg9x.c index 48cfd8d5ce5..f38a756bdd8 100644 --- a/drivers/modem/quectel-bg9x.c +++ b/drivers/modem/quectel-bg9x.c @@ -636,55 +636,31 @@ static ssize_t offload_write(void *obj, const void *buffer, size_t count) return offload_sendto(obj, buffer, count, 0, NULL, 0); } -/* Func: offload_poll - * Desc: This function polls on a given socket object. - */ -static int offload_poll(struct zsock_pollfd *fds, int nfds, int msecs) -{ - int i; - void *obj; - - /* Only accept modem sockets. */ - for (i = 0; i < nfds; i++) { - if (fds[i].fd < 0) { - continue; - } - - /* If vtable matches, then it's modem socket. */ - obj = z_get_fd_obj(fds[i].fd, - (const struct fd_op_vtable *) &offload_socket_fd_op_vtable, - EINVAL); - if (obj == NULL) { - return -1; - } - } - - return modem_socket_poll(&mdata.socket_config, fds, nfds, msecs); -} - /* Func: offload_ioctl * Desc: Function call to handle various misc requests. */ static int offload_ioctl(void *obj, unsigned int request, va_list args) { switch (request) { - case ZFD_IOCTL_POLL_PREPARE: - return -EXDEV; + case ZFD_IOCTL_POLL_PREPARE: { + struct zsock_pollfd *pfd; + struct k_poll_event **pev; + struct k_poll_event *pev_end; - case ZFD_IOCTL_POLL_UPDATE: - return -EOPNOTSUPP; + pfd = va_arg(args, struct zsock_pollfd *); + pev = va_arg(args, struct k_poll_event **); + pev_end = va_arg(args, struct k_poll_event *); - case ZFD_IOCTL_POLL_OFFLOAD: - { - /* Poll on the given socket. */ - struct zsock_pollfd *fds; - int nfds, timeout; + return modem_socket_poll_prepare(&mdata.socket_config, obj, pfd, pev, pev_end); + } + case ZFD_IOCTL_POLL_UPDATE: { + struct zsock_pollfd *pfd; + struct k_poll_event **pev; - fds = va_arg(args, struct zsock_pollfd *); - nfds = va_arg(args, int); - timeout = va_arg(args, int); + pfd = va_arg(args, struct zsock_pollfd *); + pev = va_arg(args, struct k_poll_event **); - return offload_poll(fds, nfds, timeout); + return modem_socket_poll_update(obj, pfd, pev); } default: diff --git a/drivers/modem/ublox-sara-r4.c b/drivers/modem/ublox-sara-r4.c index 908fa1ea796..381187442b6 100644 --- a/drivers/modem/ublox-sara-r4.c +++ b/drivers/modem/ublox-sara-r4.c @@ -1607,31 +1607,6 @@ static int offload_connect(void *obj, const struct sockaddr *addr, return 0; } -/* support for POLLIN only for now. */ -static int offload_poll(struct zsock_pollfd *fds, int nfds, int msecs) -{ - int i; - void *obj; - - /* Only accept modem sockets. */ - for (i = 0; i < nfds; i++) { - if (fds[i].fd < 0) { - continue; - } - - /* If vtable matches, then it's modem socket. */ - obj = z_get_fd_obj(fds[i].fd, - (const struct fd_op_vtable *) - &offload_socket_fd_op_vtable, - EINVAL); - if (obj == NULL) { - return -1; - } - } - - return modem_socket_poll(&mdata.socket_config, fds, nfds, msecs); -} - static ssize_t offload_recvfrom(void *obj, void *buf, size_t len, int flags, struct sockaddr *from, socklen_t *fromlen) @@ -1745,22 +1720,25 @@ static ssize_t offload_sendto(void *obj, const void *buf, size_t len, static int offload_ioctl(void *obj, unsigned int request, va_list args) { switch (request) { - case ZFD_IOCTL_POLL_PREPARE: - return -EXDEV; + case ZFD_IOCTL_POLL_PREPARE: { + struct zsock_pollfd *pfd; + struct k_poll_event **pev; + struct k_poll_event *pev_end; - case ZFD_IOCTL_POLL_UPDATE: - return -EOPNOTSUPP; + pfd = va_arg(args, struct zsock_pollfd *); + pev = va_arg(args, struct k_poll_event **); + pev_end = va_arg(args, struct k_poll_event *); - case ZFD_IOCTL_POLL_OFFLOAD: { - struct zsock_pollfd *fds; - int nfds; - int timeout; + return modem_socket_poll_prepare(&mdata.socket_config, obj, pfd, pev, pev_end); + } + case ZFD_IOCTL_POLL_UPDATE: { + struct zsock_pollfd *pfd; + struct k_poll_event **pev; - fds = va_arg(args, struct zsock_pollfd *); - nfds = va_arg(args, int); - timeout = va_arg(args, int); + pfd = va_arg(args, struct zsock_pollfd *); + pev = va_arg(args, struct k_poll_event **); - return offload_poll(fds, nfds, timeout); + return modem_socket_poll_update(obj, pfd, pev); } case F_GETFL: