diff --git a/drivers/modem/modem_socket.c b/drivers/modem/modem_socket.c index 6d54e05587..aa26402331 100644 --- a/drivers/modem/modem_socket.c +++ b/drivers/modem/modem_socket.c @@ -11,7 +11,6 @@ */ #include -#include #include #include "modem_socket.h" @@ -106,34 +105,6 @@ data_ready: return new_total; } -/* - * VTable OPS - */ - -static ssize_t modem_socket_read_op(void *obj, void *buf, size_t sz) -{ - /* TODO: NOT IMPLEMENTED */ - return -ENOTSUP; -} - -static ssize_t modem_socket_write_op(void *obj, const void *buf, size_t sz) -{ - /* TODO: NOT IMPLEMENTED */ - return -ENOTSUP; -} - -static int modem_socket_ioctl_op(void *obj, unsigned int request, va_list args) -{ - /* TODO: NOT IMPLEMENTED */ - return -ENOTSUP; -} - -static const struct fd_op_vtable modem_sock_fd_vtable = { - .read = modem_socket_read_op, - .write = modem_socket_write_op, - .ioctl = modem_socket_ioctl_op, -}; - /* * Socket Support Functions */ @@ -164,7 +135,8 @@ int modem_socket_get(struct modem_socket_config *cfg, 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, &modem_sock_fd_vtable); + z_finalize_fd(cfg->sockets[i].sock_fd, &cfg->sockets[i], + (const struct fd_op_vtable *)cfg->vtable); return cfg->sockets[i].sock_fd; } @@ -290,7 +262,8 @@ int modem_socket_poll(struct modem_socket_config *cfg, return found_count; } -int modem_socket_init(struct modem_socket_config *cfg) +int modem_socket_init(struct modem_socket_config *cfg, + const struct socket_op_vtable *vtable) { int i; @@ -300,5 +273,7 @@ int modem_socket_init(struct modem_socket_config *cfg) cfg->sockets[i].id = cfg->base_socket_num - 1; } + cfg->vtable = vtable; + return 0; } diff --git a/drivers/modem/modem_socket.h b/drivers/modem/modem_socket.h index 5080367308..2516dbb319 100644 --- a/drivers/modem/modem_socket.h +++ b/drivers/modem/modem_socket.h @@ -15,6 +15,9 @@ #include #include +#include + +#include "sockets_internal.h" #ifdef __cplusplus extern "C" { @@ -50,6 +53,8 @@ struct modem_socket_config { /* beginning socket id (modems can set this to 0 or 1 as needed) */ int base_socket_num; struct k_sem sem_poll; + + const struct socket_op_vtable *vtable; }; /* return total size of remaining packets */ @@ -65,7 +70,8 @@ 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 pollfd *fds, int nfds, int msecs); -int modem_socket_init(struct modem_socket_config *cfg); +int modem_socket_init(struct modem_socket_config *cfg, + const struct socket_op_vtable *vtable); #ifdef __cplusplus } diff --git a/drivers/modem/ublox-sara-r4.c b/drivers/modem/ublox-sara-r4.c index 7b739d3fc1..2b4b87ec58 100644 --- a/drivers/modem/ublox-sara-r4.c +++ b/drivers/modem/ublox-sara-r4.c @@ -18,7 +18,6 @@ LOG_MODULE_REGISTER(modem_ublox_sara_r4, CONFIG_MODEM_LOG_LEVEL); #include #include #include -#include #include "modem_context.h" #include "modem_socket.h" @@ -923,24 +922,19 @@ static int create_socket(struct modem_socket *sock, const struct sockaddr *addr) * Socket Offload OPS */ +static const struct socket_op_vtable offload_socket_fd_op_vtable; + static int offload_socket(int family, int type, int proto) { /* defer modem's socket create call to bind() */ return modem_socket_get(&mdata.socket_config, family, type, proto); } -static int offload_close(int sock_fd) +static int offload_close(struct modem_socket *sock) { - struct modem_socket *sock; char buf[sizeof("AT+USOCL=#\r")]; int ret; - sock = modem_socket_from_fd(&mdata.socket_config, sock_fd); - if (!sock) { - /* socket was already closed? Exit quietly here. */ - return 0; - } - /* make sure we assigned an id */ if (sock->id < mdata.socket_config.base_socket_num) { return 0; @@ -955,20 +949,14 @@ static int offload_close(int sock_fd) LOG_ERR("%s ret:%d", log_strdup(buf), ret); } - modem_socket_put(&mdata.socket_config, sock_fd); + modem_socket_put(&mdata.socket_config, sock->sock_fd); return 0; } -static int offload_bind(int sock_fd, const struct sockaddr *addr, +static int offload_bind(void *obj, const struct sockaddr *addr, socklen_t addrlen) { - struct modem_socket *sock = NULL; - - sock = modem_socket_from_fd(&mdata.socket_config, sock_fd); - if (!sock) { - LOG_ERR("Can't locate socket from fd:%d", sock_fd); - return -EINVAL; - } + struct modem_socket *sock = (struct modem_socket *)obj; /* save bind address information */ memcpy(&sock->src, addr, sizeof(*addr)); @@ -981,10 +969,10 @@ static int offload_bind(int sock_fd, const struct sockaddr *addr, return 0; } -static int offload_connect(int sock_fd, const struct sockaddr *addr, +static int offload_connect(void *obj, const struct sockaddr *addr, socklen_t addrlen) { - struct modem_socket *sock; + struct modem_socket *sock = (struct modem_socket *)obj; int ret; char buf[sizeof("AT+USOCO=#,!###.###.###.###!,#####,#\r")]; u16_t dst_port = 0U; @@ -993,15 +981,9 @@ static int offload_connect(int sock_fd, const struct sockaddr *addr, return -EINVAL; } - sock = modem_socket_from_fd(&mdata.socket_config, sock_fd); - if (!sock) { - LOG_ERR("Can't locate socket from fd:%d", sock_fd); - return -EINVAL; - } - if (sock->id < mdata.socket_config.base_socket_num - 1) { LOG_ERR("Invalid socket_id(%d) from fd:%d", - sock->id, sock_fd); + sock->id, sock->sock_fd); return -EINVAL; } @@ -1042,8 +1024,26 @@ static int offload_connect(int sock_fd, const struct sockaddr *addr, /* support for POLLIN only for now. */ static int offload_poll(struct pollfd *fds, int nfds, int msecs) { - int ret = modem_socket_poll(&mdata.socket_config, fds, nfds, msecs); + int ret, 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; + } + } + + ret = modem_socket_poll(&mdata.socket_config, fds, nfds, msecs); if (ret < 0) { LOG_ERR("ret:%d errno:%d", ret, errno); } @@ -1051,11 +1051,11 @@ static int offload_poll(struct pollfd *fds, int nfds, int msecs) return ret; } -static ssize_t offload_recvfrom(int sock_fd, void *buf, short int len, - short int flags, struct sockaddr *from, +static ssize_t offload_recvfrom(void *obj, void *buf, size_t len, + int flags, struct sockaddr *from, socklen_t *fromlen) { - struct modem_socket *sock; + struct modem_socket *sock = (struct modem_socket *)obj; int ret; struct modem_cmd cmd[] = { MODEM_CMD("+USORF: ", on_cmd_sockreadfrom, 4U, ","), @@ -1068,12 +1068,6 @@ static ssize_t offload_recvfrom(int sock_fd, void *buf, short int len, return -EINVAL; } - sock = modem_socket_from_fd(&mdata.socket_config, sock_fd); - if (!sock) { - LOG_ERR("Can't locate socket from fd:%d", sock_fd); - return -EINVAL; - } - if (flags & MSG_PEEK) { return -ENOTSUP; } else if (flags & MSG_DONTWAIT && !sock->packet_sizes[0]) { @@ -1119,17 +1113,11 @@ exit: return ret; } -static ssize_t offload_recv(int sock_fd, void *buf, size_t max_len, int flags) -{ - return offload_recvfrom(sock_fd, buf, (short int)max_len, flags, - NULL, NULL); -} - -static ssize_t offload_sendto(int sock_fd, const void *buf, size_t len, +static ssize_t offload_sendto(void *obj, const void *buf, size_t len, int flags, const struct sockaddr *to, socklen_t tolen) { - struct modem_socket *sock; + struct modem_socket *sock = (struct modem_socket *)obj; struct modem_cmd cmd[] = { MODEM_CMD("+USOST: ", on_cmd_sockwrite, 2U, ","), MODEM_CMD("+USOWR: ", on_cmd_sockwrite, 2U, ","), @@ -1139,12 +1127,6 @@ static ssize_t offload_sendto(int sock_fd, const void *buf, size_t len, return -EINVAL; } - sock = modem_socket_from_fd(&mdata.socket_config, sock_fd); - if (!sock) { - LOG_ERR("Can't locate socket from fd:%d", sock_fd); - return -EINVAL; - } - if (!to && sock->ip_proto == IPPROTO_UDP) { to = &sock->dst; } @@ -1153,23 +1135,73 @@ static ssize_t offload_sendto(int sock_fd, const void *buf, size_t len, buf, len, MDM_CMD_TIMEOUT); } -static ssize_t offload_send(int sock_fd, const void *buf, size_t len, int flags) +static int offload_ioctl(void *obj, unsigned int request, va_list args) { - return offload_sendto(sock_fd, buf, len, flags, NULL, 0U); + switch (request) { + /* Handle close specifically. */ + case ZFD_IOCTL_CLOSE: + return offload_close((struct modem_socket *)obj); + + case ZFD_IOCTL_POLL_PREPARE: + return -EXDEV; + + case ZFD_IOCTL_POLL_UPDATE: + return -EOPNOTSUPP; + + case ZFD_IOCTL_POLL_OFFLOAD: { + struct zsock_pollfd *fds; + int nfds; + int timeout; + + fds = va_arg(args, struct zsock_pollfd *); + nfds = va_arg(args, int); + timeout = va_arg(args, int); + + return offload_poll(fds, nfds, timeout); + } + + default: + errno = EINVAL; + return -1; + } } -static const struct socket_offload modem_socket_offload = { - .socket = offload_socket, - .close = offload_close, +static ssize_t offload_read(void *obj, void *buffer, size_t count) +{ + return offload_recvfrom(obj, buffer, count, 0, NULL, 0); +} + +static ssize_t offload_write(void *obj, const void *buffer, size_t count) +{ + return offload_sendto(obj, buffer, count, 0, NULL, 0); +} + +static const struct socket_op_vtable offload_socket_fd_op_vtable = { + .fd_vtable = { + .read = offload_read, + .write = offload_write, + .ioctl = offload_ioctl, + }, .bind = offload_bind, .connect = offload_connect, - .poll = offload_poll, - .recv = offload_recv, - .recvfrom = offload_recvfrom, - .send = offload_send, .sendto = offload_sendto, + .recvfrom = offload_recvfrom, + .listen = NULL, + .accept = NULL, + .sendmsg = NULL, + .getsockopt = NULL, + .setsockopt = NULL, }; +static bool offload_is_supported(int family, int type, int proto) +{ + /* TODO offloading always enabled for now. */ + return true; +} + +NET_SOCKET_REGISTER(ublox_sara_r4, AF_UNSPEC, offload_is_supported, + offload_socket); + static int net_offload_dummy_get(sa_family_t family, enum net_sock_type type, enum net_ip_protocol ip_proto, @@ -1225,7 +1257,6 @@ static void modem_net_iface_init(struct net_if *iface) net_if_set_link_addr(iface, modem_get_mac(dev), sizeof(data->mac_addr), NET_LINK_ETHERNET); - socket_offload_register(&modem_socket_offload); data->net_iface = iface; } @@ -1264,7 +1295,8 @@ static int modem_init(struct device *dev) mdata.socket_config.sockets = &mdata.sockets[0]; mdata.socket_config.sockets_len = ARRAY_SIZE(mdata.sockets); mdata.socket_config.base_socket_num = MDM_BASE_SOCKET_NUM; - ret = modem_socket_init(&mdata.socket_config); + ret = modem_socket_init(&mdata.socket_config, + &offload_socket_fd_op_vtable); if (ret < 0) { goto error; }