diff --git a/drivers/modem/modem_socket.c b/drivers/modem/modem_socket.c index 7e992fd5676..b3e44d4de1c 100644 --- a/drivers/modem/modem_socket.c +++ b/drivers/modem/modem_socket.c @@ -154,7 +154,7 @@ int modem_socket_get(struct modem_socket_config *cfg, int family, int type, int 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) { + if (cfg->sockets[i].id < cfg->base_socket_id) { break; } } @@ -173,7 +173,8 @@ int modem_socket_get(struct modem_socket_config *cfg, int family, int type, int cfg->sockets[i].family = family; cfg->sockets[i].type = type; cfg->sockets[i].ip_proto = proto; - cfg->sockets[i].id = i + cfg->base_socket_num; + cfg->sockets[i].id = (cfg->assign_id) ? (i + cfg->base_socket_id) : + (cfg->base_socket_id + cfg->sockets_len); z_finalize_fd(cfg->sockets[i].sock_fd, &cfg->sockets[i], (const struct fd_op_vtable *)cfg->vtable); @@ -203,7 +204,7 @@ struct modem_socket *modem_socket_from_id(struct modem_socket_config *cfg, int i { int i; - if (id < cfg->base_socket_num) { + if (id < cfg->base_socket_id) { return NULL; } @@ -223,7 +224,7 @@ struct modem_socket *modem_socket_from_id(struct modem_socket_config *cfg, int i struct modem_socket *modem_socket_from_newid(struct modem_socket_config *cfg) { - return modem_socket_from_id(cfg, cfg->sockets_len + 1); + return modem_socket_from_id(cfg, cfg->base_socket_id + cfg->sockets_len); } void modem_socket_put(struct modem_socket_config *cfg, int sock_fd) @@ -236,7 +237,7 @@ void modem_socket_put(struct modem_socket_config *cfg, int sock_fd) k_sem_take(&cfg->sem_lock, K_FOREVER); - sock->id = cfg->base_socket_num - 1; + sock->id = cfg->base_socket_id - 1; sock->sock_fd = -1; sock->is_waiting = false; sock->is_connected = false; @@ -410,18 +411,78 @@ void modem_socket_data_ready(struct modem_socket_config *cfg, struct modem_socke k_sem_give(&cfg->sem_lock); } -int modem_socket_init(struct modem_socket_config *cfg, const struct socket_op_vtable *vtable) +int modem_socket_init(struct modem_socket_config *cfg, struct modem_socket *sockets, + size_t sockets_len, int base_socket_id, bool assign_id, + 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; + /* Verify arguments */ + if (cfg == NULL || sockets == NULL || sockets_len < 1 || vtable == NULL) { + return -EINVAL; } + /* Initialize config */ + cfg->sockets = sockets; + cfg->sockets_len = sockets_len; + cfg->base_socket_id = base_socket_id; + cfg->assign_id = assign_id; + k_sem_init(&cfg->sem_lock, 1, 1); cfg->vtable = vtable; + /* Initialize associated sockets */ + for (int i = 0; i < cfg->sockets_len; i++) { + /* Clear entire socket structure */ + memset(&cfg->sockets[i], 0, sizeof(cfg->sockets[i])); + + /* Initialize socket members */ + 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 = -1; + } + return 0; +} + +bool modem_socket_is_allocated(const struct modem_socket_config *cfg, + const struct modem_socket *sock) +{ + /* Socket is allocated with a reserved id value if id is not dynamically assigned */ + if (cfg->assign_id == false && sock->id == (cfg->base_socket_id + cfg->sockets_len)) { + return true; + } + + /* Socket must have been allocated if id is assigned */ + return modem_socket_id_is_assigned(cfg, sock); +} + +bool modem_socket_id_is_assigned(const struct modem_socket_config *cfg, + const struct modem_socket *sock) +{ + /* Verify socket is assigned to a valid value */ + if ((cfg->base_socket_id <= sock->id) && + (sock->id < (cfg->base_socket_id + cfg->sockets_len))) { + return true; + } + return false; +} + +int modem_socket_id_assign(const struct modem_socket_config *cfg, + struct modem_socket *sock, int id) +{ + /* Verify dynamically assigning id is disabled */ + if (cfg->assign_id) { + return -EPERM; + } + + /* Verify id is currently not assigned */ + if (modem_socket_id_is_assigned(cfg, sock)) { + return -EPERM; + } + + /* Verify id is valid */ + if (id < cfg->base_socket_id || (cfg->base_socket_id + cfg->sockets_len) <= id) { + return -EINVAL; + } + + /* Assign id */ + sock->id = id; return 0; } diff --git a/drivers/modem/modem_socket.h b/drivers/modem/modem_socket.h index c330bf1ed5b..1c333f818df 100644 --- a/drivers/modem/modem_socket.h +++ b/drivers/modem/modem_socket.h @@ -58,7 +58,11 @@ struct modem_socket_config { size_t sockets_len; /* beginning socket id (modems can set this to 0 or 1 as needed) */ - int base_socket_num; + int base_socket_id; + + /* dynamically assign id when modem socket is allocated */ + bool assign_id; + struct k_sem sem_lock; const struct socket_op_vtable *vtable; @@ -82,7 +86,72 @@ int modem_socket_poll_prepare(struct modem_socket_config *cfg, struct modem_sock 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); + +/** + * @brief Initialize modem socket config struct and associated modem sockets + * + * @param cfg The config to initialize + * @param sockets The array of sockets associated with the modem socket config + * @param sockets_len The length of the array of sockets associated with the modem socket config + * @param base_socket_id The lowest socket id supported by the modem + * @param assign_id Dynamically assign modem socket id when allocated using modem_socket_get() + * @param vtable Socket API implementation used by this config and associated sockets + * + * @return -EINVAL if any argument is invalid + * @return 0 if successful + */ +int modem_socket_init(struct modem_socket_config *cfg, struct modem_socket *sockets, + size_t sockets_len, int base_socket_id, bool assign_id, + const struct socket_op_vtable *vtable); + +/** + * @brief Check if modem socket has been allocated + * + * @details A modem socket is allocated after a successful invocation of modem_socket_get, and + * released after a successful invocation of modem_socket_put. + * + * @note If socket id is automatically assigned, the socket id will be a value between + * base_socket_id and (base_socket_id + socket_len). + * Otherwise, the socket id will be assigned to (base_socket_id + socket_len) when allocated. + * + * @param cfg The modem socket config which the modem socket belongs to + * @param sock The modem socket which is checked + * + * @return true if the socket has been allocated + * @return false if the socket has not been allocated + */ +bool modem_socket_is_allocated(const struct modem_socket_config *cfg, + const struct modem_socket *sock); + +/** + * @brief Check if modem socket id has been assigned + * + * @note An assigned modem socket will have an id between base_socket_id and + * (base_socket_id + socket_len). + * + * @param cfg The modem socket config which the modem socket belongs to + * @param sock The modem socket for which the id is checked + * + * @return true if the socket id is been assigned + * @return false if the socket has not been assigned + */ +bool modem_socket_id_is_assigned(const struct modem_socket_config *cfg, + const struct modem_socket *sock); + +/** + * @brief Assign id to modem socket + * + * @param cfg The modem socket config which the modem socket belongs to + * @param sock The modem socket for which the id will be assigned + * @param id The id to assign to the modem socket + * + * @return -EPERM if id has been assigned previously + * @return -EINVAL if id is invalid + * @return 0 if successful + */ +int modem_socket_id_assign(const struct modem_socket_config *cfg, + struct modem_socket *sock, + int id); #ifdef __cplusplus } diff --git a/drivers/modem/quectel-bg9x.c b/drivers/modem/quectel-bg9x.c index 3f30a27ba2c..b18f928d7d5 100644 --- a/drivers/modem/quectel-bg9x.c +++ b/drivers/modem/quectel-bg9x.c @@ -197,7 +197,7 @@ static void socket_close(struct modem_socket *sock) char buf[sizeof("AT+QICLOSE=##")] = {0}; int ret; - snprintk(buf, sizeof(buf), "AT+QICLOSE=%d", sock->sock_fd); + snprintk(buf, sizeof(buf), "AT+QICLOSE=%d", sock->id); /* Tell the modem to close the socket. */ ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler, @@ -452,7 +452,7 @@ static ssize_t send_socket_data(struct modem_socket *sock, /* Create a buffer with the correct params. */ mdata.sock_written = buf_len; - snprintk(send_buf, sizeof(send_buf), "AT+QISEND=%d,%ld", sock->sock_fd, (long) buf_len); + snprintk(send_buf, sizeof(send_buf), "AT+QISEND=%d,%ld", sock->id, (long) buf_len); /* Setup the locks correctly. */ k_sem_take(&mdata.cmd_handler_data.sem_tx_lock, K_FOREVER); @@ -594,7 +594,7 @@ static ssize_t offload_recvfrom(void *obj, void *buf, size_t len, return -1; } - snprintk(sendbuf, sizeof(sendbuf), "AT+QIRD=%d,%zd", sock->sock_fd, len); + snprintk(sendbuf, sizeof(sendbuf), "AT+QIRD=%d,%zd", sock->id, len); /* Socket read settings */ (void) memset(&sock_data, 0, sizeof(sock_data)); @@ -604,7 +604,7 @@ static ssize_t offload_recvfrom(void *obj, void *buf, size_t len, sock->data = &sock_data; mdata.sock_fd = sock->sock_fd; - /* Tell the modem to give us data (AT+QIRD=sock_fd,data_len). */ + /* Tell the modem to give us data (AT+QIRD=id,data_len). */ ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler, data_cmd, ARRAY_SIZE(data_cmd), sendbuf, &mdata.sem_response, MDM_CMD_TIMEOUT); @@ -695,7 +695,8 @@ static int offload_connect(void *obj, const struct sockaddr *addr, int ret; char ip_str[NET_IPV6_ADDR_LEN]; - if (sock->id < mdata.socket_config.base_socket_num - 1) { + /* Verify socket has been allocated */ + if (modem_socket_is_allocated(&mdata.socket_config, sock) == false) { LOG_ERR("Invalid socket_id(%d) from fd:%d", sock->id, sock->sock_fd); errno = EINVAL; @@ -734,8 +735,8 @@ static int offload_connect(void *obj, const struct sockaddr *addr, } /* Formulate the complete string. */ - snprintk(buf, sizeof(buf), "AT+QIOPEN=%d,%d,\"%s\",\"%s\",%d,0,0", 1, sock->sock_fd, protocol, - ip_str, dst_port); + snprintk(buf, sizeof(buf), "AT+QIOPEN=%d,%d,\"%s\",\"%s\",%d,0,0", 1, sock->id, protocol, + ip_str, dst_port); /* Send out the command. */ ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler, @@ -792,8 +793,8 @@ static int offload_close(void *obj) { struct modem_socket *sock = (struct modem_socket *) obj; - /* Make sure we assigned an id */ - if (sock->id < mdata.socket_config.base_socket_num) { + /* Make sure socket is allocated */ + if (modem_socket_is_allocated(&mdata.socket_config, sock) == false) { return 0; } @@ -1145,10 +1146,8 @@ static int modem_init(const struct device *dev) K_PRIO_COOP(7), NULL); /* socket config */ - 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, &offload_socket_fd_op_vtable); + ret = modem_socket_init(&mdata.socket_config, &mdata.sockets[0], ARRAY_SIZE(mdata.sockets), + MDM_BASE_SOCKET_NUM, true, &offload_socket_fd_op_vtable); if (ret < 0) { goto error; } diff --git a/drivers/modem/simcom-sim7080.c b/drivers/modem/simcom-sim7080.c index f2df209170e..b989b2a21b8 100644 --- a/drivers/modem/simcom-sim7080.c +++ b/drivers/modem/simcom-sim7080.c @@ -144,7 +144,7 @@ static int offload_connect(void *obj, const struct sockaddr *addr, socklen_t add return -EAGAIN; } - if (sock->id < mdata.socket_config.base_socket_num - 1) { + if (modem_socket_is_allocated(&mdata.socket_config, sock) == false) { LOG_ERR("Invalid socket id %d from fd %d", sock->id, sock->sock_fd); errno = EINVAL; return -1; @@ -173,7 +173,7 @@ static int offload_connect(void *obj, const struct sockaddr *addr, socklen_t add return -1; } - ret = snprintk(buf, sizeof(buf), "AT+CAOPEN=%d,%d,\"%s\",\"%s\",%d", 0, sock->sock_fd, + ret = snprintk(buf, sizeof(buf), "AT+CAOPEN=%d,%d,\"%s\",\"%s\",%d", 0, sock->id, protocol, ip_str, dst_port); if (ret < 0) { LOG_ERR("Failed to build connect command. ID: %d, FD: %d", sock->id, sock->sock_fd); @@ -245,7 +245,7 @@ static ssize_t offload_sendto(void *obj, const void *buf, size_t len, int flags, len = MDM_MAX_DATA_LENGTH; } - ret = snprintk(send_buf, sizeof(send_buf), "AT+CASEND=%d,%ld", sock->sock_fd, (long)len); + ret = snprintk(send_buf, sizeof(send_buf), "AT+CASEND=%d,%ld", sock->id, (long)len); if (ret < 0) { LOG_ERR("Failed to build send command!!"); errno = ENOMEM; @@ -410,7 +410,7 @@ static ssize_t offload_recvfrom(void *obj, void *buf, size_t max_len, int flags, } max_len = (max_len > MDM_MAX_DATA_LENGTH) ? MDM_MAX_DATA_LENGTH : max_len; - snprintk(sendbuf, sizeof(sendbuf), "AT+CARECV=%d,%zd", sock->sock_fd, max_len); + snprintk(sendbuf, sizeof(sendbuf), "AT+CARECV=%d,%zd", sock->id, max_len); memset(&sock_data, 0, sizeof(sock_data)); sock_data.recv_buf = buf; @@ -502,7 +502,7 @@ static void socket_close(struct modem_socket *sock) char buf[sizeof("AT+CACLOSE=##")]; int ret; - snprintk(buf, sizeof(buf), "AT+CACLOSE=%d", sock->sock_fd); + snprintk(buf, sizeof(buf), "AT+CACLOSE=%d", sock->id); ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler, NULL, 0U, buf, &mdata.sem_response, MDM_CMD_TIMEOUT); @@ -542,8 +542,8 @@ static int offload_close(void *obj) return -EAGAIN; } - /* Make sure we assigned an id */ - if (sock->id < mdata.socket_config.base_socket_num) { + /* Make sure socket is allocated */ + if (modem_socket_is_allocated(&mdata.socket_config, sock) == false) { return 0; } @@ -2338,10 +2338,8 @@ static int modem_init(const struct device *dev) mdata.sms_buffer_pos = 0; /* Socket config. */ - 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, &offload_socket_fd_op_vtable); + ret = modem_socket_init(&mdata.socket_config, &mdata.sockets[0], ARRAY_SIZE(mdata.sockets), + MDM_BASE_SOCKET_NUM, true, &offload_socket_fd_op_vtable); if (ret < 0) { goto error; } diff --git a/drivers/modem/ublox-sara-r4.c b/drivers/modem/ublox-sara-r4.c index 233990569ed..191490686a2 100644 --- a/drivers/modem/ublox-sara-r4.c +++ b/drivers/modem/ublox-sara-r4.c @@ -738,15 +738,15 @@ static const struct setup_cmd query_cellinfo_cmds[] = { MODEM_CMD_DEFINE(on_cmd_sockcreate) { struct modem_socket *sock = NULL; + int id; /* look up new socket by special id */ sock = modem_socket_from_newid(&mdata.socket_config); if (sock) { - sock->id = ATOI(argv[0], - mdata.socket_config.base_socket_num - 1, - "socket_id"); + id = ATOI(argv[0], -1, "socket_id"); + /* on error give up modem socket */ - if (sock->id == mdata.socket_config.base_socket_num - 1) { + if (modem_socket_id_assign(&mdata.socket_config, sock, id) < 0) { modem_socket_put(&mdata.socket_config, sock->sock_fd); } } @@ -1489,8 +1489,8 @@ static int offload_close(void *obj) char buf[sizeof("AT+USOCL=#\r")]; int ret; - /* make sure we assigned an id */ - if (sock->id < mdata.socket_config.base_socket_num) { + /* make sure socket is allocated and assigned an id */ + if (modem_socket_id_is_assigned(&mdata.socket_config, sock) == false) { return 0; } @@ -1518,7 +1518,7 @@ static int offload_bind(void *obj, const struct sockaddr *addr, memcpy(&sock->src, addr, sizeof(*addr)); /* make sure we've created the socket */ - if (sock->id == mdata.socket_config.sockets_len + 1) { + if (modem_socket_is_allocated(&mdata.socket_config, sock) == true) { if (create_socket(sock, addr) < 0) { return -1; } @@ -1541,7 +1541,8 @@ static int offload_connect(void *obj, const struct sockaddr *addr, return -1; } - if (sock->id < mdata.socket_config.base_socket_num - 1) { + /* make sure socket has been allocated */ + if (modem_socket_is_allocated(&mdata.socket_config, sock) == false) { LOG_ERR("Invalid socket_id(%d) from fd:%d", sock->id, sock->sock_fd); errno = EINVAL; @@ -1549,7 +1550,7 @@ static int offload_connect(void *obj, const struct sockaddr *addr, } /* make sure we've created the socket */ - if (sock->id == mdata.socket_config.sockets_len + 1) { + if (modem_socket_id_is_assigned(&mdata.socket_config, sock) == false) { if (create_socket(sock, NULL) < 0) { return -1; } @@ -2139,11 +2140,8 @@ static int modem_init(const struct device *dev) #endif /* socket config */ - 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, - &offload_socket_fd_op_vtable); + ret = modem_socket_init(&mdata.socket_config, &mdata.sockets[0], ARRAY_SIZE(mdata.sockets), + MDM_BASE_SOCKET_NUM, false, &offload_socket_fd_op_vtable); if (ret < 0) { goto error; }