blob: 4fb57b3d7ee73fcd1c4bb7c77e2185e481f9edea [file] [log] [blame]
/** @file
* @brief Modem socket / packet size handler
*
* Generic modem socket and packet size implementation for modem context
*/
/*
* Copyright (c) 2019-2020 Foundries.io
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/kernel.h>
#include <zephyr/sys/fdtable.h>
#include "modem_socket.h"
/*
* Packet Size Support Functions
*/
uint16_t modem_socket_next_packet_size(struct modem_socket_config *cfg, struct modem_socket *sock)
{
uint16_t total = 0U;
k_sem_take(&cfg->sem_lock, K_FOREVER);
if (!sock || !sock->packet_count) {
goto exit;
}
total = sock->packet_sizes[0];
exit:
k_sem_give(&cfg->sem_lock);
return total;
}
static uint16_t modem_socket_packet_get_total(struct modem_socket *sock)
{
int i;
uint16_t total = 0U;
if (!sock || !sock->packet_count) {
return 0U;
}
for (i = 0; i < sock->packet_count; i++) {
total += sock->packet_sizes[i];
}
return total;
}
static int modem_socket_packet_drop_first(struct modem_socket *sock)
{
int i;
if (!sock || !sock->packet_count) {
return -EINVAL;
}
sock->packet_count--;
for (i = 0; i < sock->packet_count; i++) {
sock->packet_sizes[i] = sock->packet_sizes[i + 1];
}
sock->packet_sizes[sock->packet_count] = 0U;
return 0;
}
int modem_socket_packet_size_update(struct modem_socket_config *cfg, struct modem_socket *sock,
int new_total)
{
uint16_t old_total = 0U;
if (!sock) {
return -EINVAL;
}
k_sem_take(&cfg->sem_lock, K_FOREVER);
if (new_total < 0) {
new_total += modem_socket_packet_get_total(sock);
}
if (new_total <= 0) {
/* 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;
}
old_total = modem_socket_packet_get_total(sock);
if (new_total == old_total) {
goto data_ready;
}
/* remove sent packets */
if (new_total < old_total) {
/* remove packets that are not included in new_size */
while (old_total > new_total && sock->packet_count > 0) {
/* handle partial read */
if (old_total - new_total < sock->packet_sizes[0]) {
sock->packet_sizes[0] -= old_total - new_total;
break;
}
old_total -= sock->packet_sizes[0];
modem_socket_packet_drop_first(sock);
}
goto data_ready;
}
/* new packet to add */
if (sock->packet_count >= CONFIG_MODEM_SOCKET_PACKET_COUNT) {
k_sem_give(&cfg->sem_lock);
return -ENOMEM;
}
if (new_total - old_total > 0) {
sock->packet_sizes[sock->packet_count] = new_total - old_total;
sock->packet_count++;
} else {
k_sem_give(&cfg->sem_lock);
return -EINVAL;
}
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;
}
/*
* Socket Support Functions
*/
int modem_socket_get(struct modem_socket_config *cfg, int family, int type, int proto)
{
int i;
k_sem_take(&cfg->sem_lock, K_FOREVER);
for (i = 0; i < cfg->sockets_len; i++) {
if (cfg->sockets[i].id < cfg->base_socket_num) {
break;
}
}
if (i >= cfg->sockets_len) {
k_sem_give(&cfg->sem_lock);
return -ENOMEM;
}
/* FIXME: 4 fds max now due to POSIX_OS conflict */
cfg->sockets[i].sock_fd = z_reserve_fd();
if (cfg->sockets[i].sock_fd < 0) {
k_sem_give(&cfg->sem_lock);
return -errno;
}
cfg->sockets[i].family = family;
cfg->sockets[i].type = type;
cfg->sockets[i].ip_proto = proto;
/* socket # needs assigning */
cfg->sockets[i].id = cfg->sockets_len + 1;
z_finalize_fd(cfg->sockets[i].sock_fd, &cfg->sockets[i],
(const struct fd_op_vtable *)cfg->vtable);
k_sem_give(&cfg->sem_lock);
return cfg->sockets[i].sock_fd;
}
struct modem_socket *modem_socket_from_fd(struct modem_socket_config *cfg, int sock_fd)
{
int i;
k_sem_take(&cfg->sem_lock, K_FOREVER);
for (i = 0; i < cfg->sockets_len; i++) {
if (cfg->sockets[i].sock_fd == sock_fd) {
k_sem_give(&cfg->sem_lock);
return &cfg->sockets[i];
}
}
k_sem_give(&cfg->sem_lock);
return NULL;
}
struct modem_socket *modem_socket_from_id(struct modem_socket_config *cfg, int id)
{
int i;
if (id < cfg->base_socket_num) {
return NULL;
}
k_sem_take(&cfg->sem_lock, K_FOREVER);
for (i = 0; i < cfg->sockets_len; i++) {
if (cfg->sockets[i].id == id) {
k_sem_give(&cfg->sem_lock);
return &cfg->sockets[i];
}
}
k_sem_give(&cfg->sem_lock);
return NULL;
}
struct modem_socket *modem_socket_from_newid(struct modem_socket_config *cfg)
{
return modem_socket_from_id(cfg, cfg->sockets_len + 1);
}
void modem_socket_put(struct modem_socket_config *cfg, int sock_fd)
{
struct modem_socket *sock = modem_socket_from_fd(cfg, sock_fd);
if (!sock) {
return;
}
k_sem_take(&cfg->sem_lock, K_FOREVER);
sock->id = cfg->base_socket_num - 1;
sock->sock_fd = -1;
sock->is_waiting = 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->sig_data_ready);
k_sem_give(&cfg->sem_lock);
}
/*
* Generic Poll Function
*/
/*
* FIXME: The design here makes the poll function non-reentrant for same sockets.
* If two different threads poll on two identical sockets we'll end up with unexpected
* behavior - the higher priority thread will be unblocked, regardless on which
* socket it polled. I think we could live with such limitation though in the
* initial implementation, but this should be improved in the future.
*/
int modem_socket_poll(struct modem_socket_config *cfg, struct zsock_pollfd *fds, int nfds,
int msecs)
{
struct modem_socket *sock;
int ret, i;
uint8_t found_count = 0;
if (!cfg || nfds > CONFIG_NET_SOCKETS_POLL_MAX) {
return -EINVAL;
}
struct k_poll_event events[nfds];
int eventcount = 0;
for (i = 0; i < nfds; i++) {
sock = modem_socket_from_fd(cfg, fds[i].fd);
if (sock) {
/*
* Handle user check for POLLOUT events:
* we consider the socket to always be writable.
*/
if (fds[i].events & ZSOCK_POLLOUT) {
found_count++;
break;
} else if (fds[i].events & ZSOCK_POLLIN) {
k_poll_event_init(&events[eventcount++], K_POLL_TYPE_SIGNAL,
K_POLL_MODE_NOTIFY_ONLY, &sock->sig_data_ready);
if (sock->packet_sizes[0] > 0U) {
found_count++;
break;
}
}
}
}
/* Avoid waiting on semaphore if we have already found an event */
ret = 0;
if (!found_count) {
k_timeout_t timeout = K_FOREVER;
if (msecs >= 0) {
timeout = K_MSEC(msecs);
}
ret = k_poll(events, eventcount, timeout);
}
/* Reset counter as we reiterate on all polled sockets */
found_count = 0;
for (i = 0; i < nfds; i++) {
sock = modem_socket_from_fd(cfg, fds[i].fd);
if (!sock) {
continue;
}
/*
* Handle user check for ZSOCK_POLLOUT events:
* we consider the socket to always be writable.
*/
if (fds[i].events & ZSOCK_POLLOUT) {
fds[i].revents |= ZSOCK_POLLOUT;
found_count++;
} else if ((fds[i].events & ZSOCK_POLLIN) && (sock->packet_sizes[0] > 0U)) {
fds[i].revents |= ZSOCK_POLLIN;
found_count++;
}
}
/* EBUSY, EAGAIN and ETIMEDOUT aren't true errors */
if (ret < 0 && ret != -EBUSY && ret != -EAGAIN && ret != -ETIMEDOUT) {
errno = ret;
return -1;
}
errno = 0;
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);
sock->is_waiting = true;
k_sem_give(&cfg->sem_lock);
k_sem_take(&sock->sem_data_ready, K_FOREVER);
}
void modem_socket_data_ready(struct modem_socket_config *cfg, struct modem_socket *sock)
{
k_sem_take(&cfg->sem_lock, K_FOREVER);
if (sock->is_waiting) {
/* unblock sockets waiting on recv() */
sock->is_waiting = false;
k_sem_give(&sock->sem_data_ready);
}
k_sem_give(&cfg->sem_lock);
}
int modem_socket_init(struct modem_socket_config *cfg, const struct socket_op_vtable *vtable)
{
int i;
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].sig_data_ready);
cfg->sockets[i].id = cfg->base_socket_num - 1;
}
cfg->vtable = vtable;
return 0;
}