From 7a196417bbe038e2d100cc93c595f90b5665a9ed Mon Sep 17 00:00:00 2001 From: Alexander Couzens Date: Sun, 1 Oct 2023 17:38:49 +0200 Subject: [PATCH] add uqmid: a daemonic version of uqmi It is configured via ubus to handle QMI based modem and keep the state of the modem in difference to uqmi as command utility. Further it also allows to react on events (indications) of the modem. See uqmid/examples/ for more usage. Signed-off-by: Alexander Couzens --- CMakeLists.txt | 15 + uqmid/CMakeLists.txt | 14 + uqmid/ctrl.c | 93 +++ uqmid/ctrl.h | 32 + uqmid/ddev.c | 224 ++++++ uqmid/examples/1_add.sh | 3 + uqmid/examples/2_configure.sh | 4 + uqmid/examples/3_remove.sh | 3 + uqmid/examples/x_dump.sh | 3 + uqmid/logging.h | 19 + uqmid/modem.c | 295 ++++++++ uqmid/modem.h | 123 ++++ uqmid/modem_fsm.c | 1265 +++++++++++++++++++++++++++++++++ uqmid/modem_fsm.h | 71 ++ uqmid/modem_tx.c | 180 +++++ uqmid/modem_tx.h | 20 + uqmid/osmocom/CMakeLists.txt | 2 + uqmid/osmocom/README.md | 6 + uqmid/osmocom/fsm.c | 1046 +++++++++++++++++++++++++++ uqmid/osmocom/fsm.h | 340 +++++++++ uqmid/osmocom/linuxlist.h | 650 +++++++++++++++++ uqmid/osmocom/logging.h | 48 ++ uqmid/osmocom/timer.c | 75 ++ uqmid/osmocom/timer.h | 43 ++ uqmid/osmocom/utils.c | 285 ++++++++ uqmid/osmocom/utils.h | 77 ++ uqmid/services.c | 244 +++++++ uqmid/services.h | 73 ++ uqmid/sim.c | 59 ++ uqmid/sim.h | 38 + uqmid/ubus.c | 459 ++++++++++++ uqmid/ubus.h | 27 + uqmid/uqmid.c | 79 ++ uqmid/uqmid.h | 95 +++ uqmid/uqmid.init.sh | 12 + 35 files changed, 6022 insertions(+) create mode 100644 uqmid/CMakeLists.txt create mode 100644 uqmid/ctrl.c create mode 100644 uqmid/ctrl.h create mode 100644 uqmid/ddev.c create mode 100644 uqmid/examples/1_add.sh create mode 100644 uqmid/examples/2_configure.sh create mode 100644 uqmid/examples/3_remove.sh create mode 100644 uqmid/examples/x_dump.sh create mode 100644 uqmid/logging.h create mode 100644 uqmid/modem.c create mode 100644 uqmid/modem.h create mode 100644 uqmid/modem_fsm.c create mode 100644 uqmid/modem_fsm.h create mode 100644 uqmid/modem_tx.c create mode 100644 uqmid/modem_tx.h create mode 100644 uqmid/osmocom/CMakeLists.txt create mode 100644 uqmid/osmocom/README.md create mode 100644 uqmid/osmocom/fsm.c create mode 100644 uqmid/osmocom/fsm.h create mode 100644 uqmid/osmocom/linuxlist.h create mode 100644 uqmid/osmocom/logging.h create mode 100644 uqmid/osmocom/timer.c create mode 100644 uqmid/osmocom/timer.h create mode 100644 uqmid/osmocom/utils.c create mode 100644 uqmid/osmocom/utils.h create mode 100644 uqmid/services.c create mode 100644 uqmid/services.h create mode 100644 uqmid/sim.c create mode 100644 uqmid/sim.h create mode 100644 uqmid/ubus.c create mode 100644 uqmid/ubus.h create mode 100644 uqmid/uqmid.c create mode 100644 uqmid/uqmid.h create mode 100755 uqmid/uqmid.init.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 60e9c3e..f42b9cb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 3.10) PROJECT(uqmi C) OPTION(BUILD_STATIC OFF) +OPTION(BUILD_UQMID OFF) ADD_DEFINITIONS(-Os -ggdb -Wall -Werror --std=gnu99 -Wmissing-declarations -Wno-enum-conversion -Wno-dangling-pointer) @@ -67,3 +68,17 @@ ADD_DEPENDENCIES(qmigen gen-headers gen-errors) ADD_SUBDIRECTORY(common) ADD_SUBDIRECTORY(uqmi) + +IF(BUILD_UQMID) + FIND_PATH(talloc_include_dir talloc.h) + FIND_PATH(ubus_include_dir libubus.h) + IF(BUILD_STATIC) + FIND_LIBRARY(talloc_library NAMES libtalloc.a) + FIND_LIBRARY(ubus_library NAMES libubus.a) + ELSE(BUILD_STATIC) + FIND_LIBRARY(talloc_library NAMES talloc) + FIND_LIBRARY(ubus_library NAMES ubus) + ENDIF(BUILD_STATIC) + + ADD_SUBDIRECTORY(uqmid) +ENDIF(BUILD_UQMID) diff --git a/uqmid/CMakeLists.txt b/uqmid/CMakeLists.txt new file mode 100644 index 0000000..d53096b --- /dev/null +++ b/uqmid/CMakeLists.txt @@ -0,0 +1,14 @@ + +SET(UQMID_LIBS ${talloc_library} ${ubus_library}) + +SET(UQMID uqmid.c ddev.c ubus.c modem.c modem_fsm.c modem_tx.c services.c sim.c ctrl.c) + +ADD_SUBDIRECTORY(osmocom) +ADD_EXECUTABLE(uqmid ${UQMID}) +ADD_DEPENDENCIES(uqmid gen-headers gen-errors) + +TARGET_LINK_LIBRARIES(uqmid osmofsm ${UQMID_LIBS} ${LIBS} common qmigen) +TARGET_INCLUDE_DIRECTORIES(uqmid PRIVATE ${ubus_include_dir} ${ubox_include_dir} ${blobmsg_json_include_dir} ${json_include_dir} ${talloc_include_dir} ${CMAKE_SOURCE_DIR}) +INSTALL(TARGETS uqmid + RUNTIME DESTINATION sbin +) diff --git a/uqmid/ctrl.c b/uqmid/ctrl.c new file mode 100644 index 0000000..6747cc9 --- /dev/null +++ b/uqmid/ctrl.c @@ -0,0 +1,93 @@ + +#include +#include + +#include "services.h" +#include "uqmid.h" + +#include "ctrl.h" +#include "qmi-message.h" + +static void uqmi_ctrl_request_clientid_cb(struct qmi_service *ctrl, struct qmi_request *req, struct qmi_msg *msg) +{ + struct qmi_ctl_allocate_cid_response res; + struct qmi_service *service; + if (!msg) + return; + + qmi_parse_ctl_allocate_cid_response(msg, &res); + service = uqmi_service_find(ctrl->qmi, res.data.allocation_info.service); + if (!service) { + /* FIXME: error log("Can't find the service for the allocated CID") */ + return; + } + + uqmi_service_get_client_id_cb(service, res.data.allocation_info.cid); +} + +int uqmi_ctrl_request_clientid(struct qmi_service *service) +{ + struct qmi_service *ctrl = service->qmi->ctrl; + struct qmi_request *req = talloc_zero(ctrl, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 128); + + + struct qmi_ctl_allocate_cid_request creq = { + QMI_INIT(service, service->service) + }; + qmi_set_ctl_allocate_cid_request(msg, &creq); + req->cb = uqmi_ctrl_request_clientid_cb; + req->msg = msg; + + return uqmi_service_send_msg(ctrl, req); +} + +static void uqmi_ctrl_release_clientid_cb(struct qmi_service *ctrl, struct qmi_request *req, struct qmi_msg *msg) +{ + struct qmi_ctl_release_cid_response res; + struct qmi_service *service; + + if (!msg) + return; + + if (qmi_parse_ctl_release_cid_response(msg, &res)) { + /* error_log("Couldn't parse release cid response") */ + return; + } + + if (!res.set.release_info) + return; + + service = uqmi_service_find(ctrl->qmi, res.data.release_info.service); + if (service && service->service) + uqmi_service_close_cb(service); +} + +int uqmi_ctrl_release_clientid(struct qmi_service *service) +{ + struct qmi_service *ctrl = service->qmi->ctrl; + struct qmi_request *req = talloc_zero(ctrl, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 128); + + struct qmi_ctl_release_cid_request creq = { + QMI_INIT_SEQUENCE(release_info, + .service = service->service, + .cid = service->client_id, + ) + }; + qmi_set_ctl_release_cid_request(msg, &creq); + req->msg = msg; + req->cb = uqmi_ctrl_release_clientid_cb; + + return uqmi_service_send_msg(ctrl, req); +} + +struct qmi_service *uqmi_ctrl_generate(struct qmi_dev *qmi) +{ + qmi->ctrl = uqmi_service_create(qmi, 0); + qmi->ctrl->client_id = 0; + qmi->ctrl->state = SERVICE_READY; + + return qmi->ctrl; +} + diff --git a/uqmid/ctrl.h b/uqmid/ctrl.h new file mode 100644 index 0000000..5d5b7de --- /dev/null +++ b/uqmid/ctrl.h @@ -0,0 +1,32 @@ +/* + * uqmi -- tiny QMI support implementation + * + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#ifndef __UQMID_CTRL_H +#define __UQMID_CTRL_H + +struct qmi_dev; +struct qmi_service; + +int uqmi_ctrl_request_clientid(struct qmi_service *service); +int uqmi_ctrl_release_clientid(struct qmi_service *service); +struct qmi_service *uqmi_ctrl_generate(struct qmi_dev *qmi); + +#endif /* __UQMID_CTRL_H */ diff --git a/uqmid/ddev.c b/uqmid/ddev.c new file mode 100644 index 0000000..5bfc3c0 --- /dev/null +++ b/uqmid/ddev.c @@ -0,0 +1,224 @@ + +#include +#include +#include + +#include +#include + +#include "qmi-enums.h" +#include "qmi-enums-private.h" +#include "qmi-message.h" +#include "qmi-struct.h" + +#include "ctrl.h" +#include "uqmid.h" +#include "logging.h" +#include "services.h" +#include "modem.h" + +/* FIXME: decide dump_packet */ +#define dump_packet(str, buf, len) + +static void +__qmi_request_complete(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + void *tlv_buf; + int tlv_len; + + if (!req->pending) + return; + + req->pending = false; + req->complete = true; + list_del(&req->list); + + if (msg) { + tlv_buf = qmi_msg_get_tlv_buf(msg, &tlv_len); + req->ret = qmi_check_message_status(tlv_buf, tlv_len); + } else { + req->ret = QMI_ERROR_CANCELLED; + } + + if (req->cb && msg) + req->cb(service, req, msg); + + talloc_free(req); + /* frees msg as well because of tree */ +} + +static void +qmi_process_msg(struct qmi_dev *qmi, struct qmi_msg *msg) +{ + struct qmi_service *service; + struct qmi_request *req; + uint16_t tid; + + /* FIXME: indications? */ + + if (msg->qmux.service == QMI_SERVICE_CTL) + modem_log(qmi->modem, LOGL_DEBUG, "Process message from srv %d msg %04x flag: %02x tid: %02x", + msg->qmux.service, le16_to_cpu(msg->ctl.message), msg->flags, msg->ctl.transaction); + else + modem_log(qmi->modem, LOGL_DEBUG, "Process message from srv %d msg %04x flag: %02x tid: %04x", + msg->qmux.service, le16_to_cpu(msg->svc.message), msg->flags, msg->svc.transaction); + + if (msg->flags != QMI_CTL_FLAG_RESPONSE && msg->flags != QMI_SERVICE_FLAG_RESPONSE) + return; + + if (msg->qmux.service == QMI_SERVICE_CTL) + tid = msg->ctl.transaction; + else + tid = le16_to_cpu(msg->svc.transaction); + + service = uqmi_service_find(qmi, msg->qmux.service); + if (!service) { + /* error_log("Couldn't find a service for incoming message") */ + return; + } + + list_for_each_entry(req, &service->reqs, list) { + if (req->tid != tid) + continue; + + __qmi_request_complete(service, req, msg); + return; + } + + /* error_log("Couldn't find a tid for incoming message") */ +} + +static void qmi_notify_read(struct ustream *us, int bytes) +{ + struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream); + struct qmi_msg *msg; + char *buf; + int len, msg_len; + + + while (1) { + buf = ustream_get_read_buf(us, &len); + if (!buf || !len) + return; + + /* FIXME: check partial reads */ + /* FIXME: implement mbim */ + + dump_packet("Received packet", buf, len); + if (len < offsetof(struct qmi_msg, flags)) + return; + msg = (struct qmi_msg *) buf; + msg_len = le16_to_cpu(msg->qmux.len) + 1; + + if (len < msg_len) + return; + + qmi_process_msg(qmi, msg); + ustream_consume(us, msg_len); + } +} + +static void qmi_notify_state(struct ustream *us) +{ + struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream); + + if (us->eof || us->write_error) { + modem_log(qmi->modem, LOGL_ERROR, "Modem connection died! Closing modem."); + } else { + modem_log(qmi->modem, LOGL_ERROR, "Unknown modem fd state change eof: %d write_error: %d. Closing modem anyways", + us->eof, us->write_error); + } + + /* errors! */ + if (qmi->state != QMI_STOPPING) + qmi->state = QMI_ERROR; + + if (qmi->error_cb) + qmi->error_cb(qmi, qmi->error_cb_data); +} + +struct qmi_dev *qmi_device_open(struct modem *modem, const char *path) +{ + struct qmi_dev *qmi; + struct ustream *us; + int fd; + + /* assert(qmi) */ + + fd = open(path, O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY); + if (fd < 0) + return NULL; + + qmi = talloc_zero(modem, struct qmi_dev); + us = &qmi->sf.stream; + + us->notify_state = qmi_notify_state; + us->notify_read = qmi_notify_read; + ustream_fd_init(&qmi->sf, fd); + INIT_LIST_HEAD(&qmi->services); + qmi->modem = modem; + + uqmi_ctrl_generate(qmi); + + return qmi; +} + +/* timer callback to give service the time to shut down */ +static void qmi_device_close_cb(struct uloop_timeout *timeout) +{ + struct qmi_service *service, *tmp; + struct qmi_dev *qmi = container_of(timeout, struct qmi_dev, shutdown); + + modem_log(qmi->modem, LOGL_INFO, "Closing qmi device"); + uqmi_service_close(qmi->ctrl); + list_for_each_entry_safe(service, tmp, &qmi->services, list) { + list_del(&service->list); + talloc_free(service); + } + qmi->ctrl = NULL; + + ustream_free(&qmi->sf.stream); + close(qmi->sf.fd.fd); + + if (qmi->closing_cb) + qmi->closing_cb(qmi, qmi->closing_cb_data); +} + +/* called by the service when the QMI modem release the client id */ +void qmi_device_service_closed(struct qmi_dev *qmi) +{ + if (qmi->state != QMI_STOPPING) + return; + /* only ctrl left, use schedule to decouple it from req and break a free(req) loop */ + if (qmi->services.next == qmi->services.prev && qmi->services.prev == &qmi->ctrl->list) + uloop_timeout_set(&qmi->shutdown, 0); +} + +void qmi_device_close(struct qmi_dev *qmi, int timeout_ms) +{ + struct qmi_service *service, *tmp; + bool error = qmi->state == QMI_ERROR; + + if (qmi->state == QMI_STOPPING) + return; + + qmi->state = QMI_STOPPING; + if (!error) { + list_for_each_entry_safe(service, tmp, &qmi->services, list) { + /* CTL service is required to close the others. The pending request will be cleared in _cb */ + if (service->service == QMI_SERVICE_CTL) + continue; + + uqmi_service_close(service); + } + } + + /* should we allow to close all services at once or should we close it slowly? one-by-one? */ + if (timeout_ms <= 0 || error) { + qmi_device_close_cb(&qmi->shutdown); + } else { + qmi->shutdown.cb = qmi_device_close_cb; + uloop_timeout_set(&qmi->shutdown, timeout_ms); + } +} + diff --git a/uqmid/examples/1_add.sh b/uqmid/examples/1_add.sh new file mode 100644 index 0000000..e4e52f7 --- /dev/null +++ b/uqmid/examples/1_add.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +ubus call uqmid add_modem '{ "name": "modem1", "device": "/dev/cdc-wdm0"}' diff --git a/uqmid/examples/2_configure.sh b/uqmid/examples/2_configure.sh new file mode 100644 index 0000000..75079de --- /dev/null +++ b/uqmid/examples/2_configure.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +CONFIG='{"apn":"internet.telekom","roaming":"true"}' +ubus call uqmid.modem.modem1 configure "$CONFIG" diff --git a/uqmid/examples/3_remove.sh b/uqmid/examples/3_remove.sh new file mode 100644 index 0000000..3735f4f --- /dev/null +++ b/uqmid/examples/3_remove.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +ubus call uqmid remove_modem '{ "name": "modem1" }' diff --git a/uqmid/examples/x_dump.sh b/uqmid/examples/x_dump.sh new file mode 100644 index 0000000..b8d44c8 --- /dev/null +++ b/uqmid/examples/x_dump.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +ubus call uqmid.modem.modem1 dump diff --git a/uqmid/logging.h b/uqmid/logging.h new file mode 100644 index 0000000..3efac54 --- /dev/null +++ b/uqmid/logging.h @@ -0,0 +1,19 @@ + +#ifndef _UQMID_LOGGING_H__ +#define _UQMID_LOGGING_H__ + +#include "osmocom/logging.h" + +/* TOOD: implement logging */ + +/* TODO: improve modem_log() */ +#define modem_log(modem, level, fmt, args...) fprintf(stderr, "%s:%d " fmt "\n", modem->name, level, ## args) +#define service_log(service, level, fmt, args...) \ + fprintf(stderr, "%s:svc/%d:%d " fmt "\n", service->qmi->modem->name, service->service, level, ## args); + + +#define LOG_ERROR(fmt, args...) fprintf(stderr, fmt, ## args) +#define LOG_INFO(fmt, args...) LOG_ERROR(fmt, ## args) +#define LOG_DEBUG(fmt, args...) LOG_ERROR(fmt, ## args) + +#endif /* _UQMID_LOGGING_H__ */ diff --git a/uqmid/modem.c b/uqmid/modem.c new file mode 100644 index 0000000..71927b8 --- /dev/null +++ b/uqmid/modem.c @@ -0,0 +1,295 @@ +/* + * uqmi -- tiny QMI support implementation + * + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +/* Used by uqmid to contain the modem state */ + +#include +#include +#include +#include +#include +#include + +#include "qmi-message.h" + +#include "qmi-enums-dms.h" +#include "qmi-enums-wds.h" +#include "qmi-enums.h" +#include "uqmid.h" +#include "qmi-errors.h" +#include "qmi-errors.c" + +#include "mbim.h" +#include "services.h" + +#include "modem.h" +#include "modem_fsm.h" +#include "modem_tx.h" +#include "osmocom/fsm.h" +#include "ubus.h" +#include "logging.h" +#include "utils.h" + +LIST_HEAD(uqmid_modems); + +struct modem * +uqmid_modem_find_by_device(const char *device) +{ + struct modem *modem; + + list_for_each_entry(modem, &uqmid_modems, list) { + if (!strcmp(device, modem->device)) + return modem; + } + + return NULL; +} + +struct modem * +uqmid_modem_find_by_name(const char *name) +{ + struct modem *modem; + list_for_each_entry(modem, &uqmid_modems, list) { + if (!strcmp(name, modem->name)) + return modem; + } + + return NULL; +} + +static void +modem_error_cb(struct qmi_dev *dev, void *data) +{ + struct modem *modem = data; + + osmo_fsm_inst_term(modem->fi, OSMO_FSM_TERM_ERROR, modem); + uqmid_ubus_modem_destroy(modem); + qmi_device_close(modem->qmi, 0); + + list_del(&modem->list); + talloc_free(modem); +} + +/** + * + * @param name an unique name also used by ubus + * @param device path to the device. /dev/cdc-wdm0 + * @param qmi_over_mbim true if qmi needs to be tunneled over mbim + * @return 0 on success + */ +int +uqmid_modem_add(const char *name, const char *device, bool qmi_over_mbim) +{ + struct modem *modem = uqmid_modem_find_by_name(name); + int ret; + if (modem) + return -EALREADY; + + modem = talloc_zero(NULL, struct modem); + modem->name = talloc_strdup(modem, name); + modem->device = talloc_strdup(modem, device); + modem->qmi = qmi_device_open(modem, device); + if (!modem->qmi) + goto err; + + ret = uqmid_ubus_modem_add(modem); + if (ret) + goto err; + + modem->qmi->error_cb = modem_error_cb; + modem->qmi->error_cb_data = modem; + + modem->fi = modem_fsm_alloc(modem); + list_add(&modem->list, &uqmid_modems); + modem_fsm_start(modem); + + return 0; + +err: + if (modem->qmi) + qmi_device_close(modem->qmi, 0); + + talloc_free(modem); + return -1; +} + + +static void +modem_remove_cb(struct qmi_dev *dev, void *data) +{ + struct modem *modem = data; + + osmo_fsm_inst_term(modem->fi, OSMO_FSM_TERM_REGULAR, modem); + uqmid_ubus_modem_destroy(modem); + + list_del(&modem->list); + talloc_free(modem); +} + +int +uqmid_modem_remove(struct modem *modem) +{ + if (!modem) + return -EINVAL; + + if (!modem->fi) + return 0; + + if (modem->fi->state == MODEM_ST_DESTROY) + return 0; + + if (modem->fi->proc.terminating) + return 0; + + /* TODO: move the assignment of closing_cb */ + modem->qmi->closing_cb = modem_remove_cb; + modem->qmi->closing_cb_data = modem; + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_REQ_DESTROY, modem); + + return 0; +} + +int +uqmid_modem_configured(struct modem *modem) +{ + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_REQ_CONFIGURED, NULL); + return 0; +} + +static void +wds_get_packet_status_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0; + struct qmi_wds_get_packet_service_status_response res = {}; + + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Status %d/%s.", req->ret, qmi_get_error_str(req->ret)); + /* FIXME: send ubus failed */ + return; + } + + ret = qmi_parse_wds_get_packet_service_status_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get packet service status. Failed to parse message"); + /* FIXME: send ubus failed */ + return; + } + + if (!res.set.connection_status) { + modem_log(modem, LOGL_INFO, "Failed to get packet service status. No connection status"); + } + + modem_log(modem, LOGL_INFO, "Conn Status is %d", res.data.connection_status); + + /* Fix send back */ + return; +} + +struct modem_opmode_data { + struct modem *modem; + void *cb_data; + uqmid_modem_get_opmode_cb cb; +}; + +static void +modem_get_opmode_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem_opmode_data *data= req->cb_data; + struct modem *modem = data->modem; + int ret = 0; + struct qmi_dms_get_operating_mode_response res = {}; + + if (!data) { + return; + } + + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Status %d/%s.", req->ret, qmi_get_error_str(req->ret)); + goto out; + } + + ret = qmi_parse_dms_get_operating_mode_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Failed to parse message"); + goto out; + } + + if (!res.set.mode) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Failed to parse message"); + } + + modem_log(modem, LOGL_INFO, "Opmode is %d", res.data.mode); + data->cb(data->cb_data, 0, res.data.mode); +out: + free(data); + return; +} + +int +uqmid_modem_get_opmode(struct modem *modem, uqmid_modem_get_opmode_cb cb, void *cb_data) +{ + struct qmi_service *dms = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + if (!dms) { + cb(cb_data, -ENOENT, 0); + modem_log(modem, LOGL_ERROR, "Ubus call requested, but dms not available."); + return -1; + } + + struct modem_opmode_data *data = calloc(1, sizeof(*data)); + if (!data) { + cb(cb_data, -ENOMEM, 0); + return -1; + } + + data->cb = cb; + data->cb_data = cb_data; + data->modem = modem; + + uqmi_service_send_simple(dms, qmi_set_dms_get_operating_mode_request, modem_get_opmode_cb, data); + return 0; +} + +int uqmid_modem_networkstatus(struct modem *modem) +{ + struct qmi_service *wds = uqmi_service_find(modem->qmi, QMI_SERVICE_WDS); + if (!wds) { + modem_log(modem, LOGL_ERROR, "Ubus call requested, but wds not available."); + return -1; + } + + uqmi_service_send_simple(wds, qmi_set_wds_get_packet_service_status_request, wds_get_packet_status_cb, modem); + return 0; +} + +void uqmid_modem_set_error(struct modem *modem, const char *error) +{ + if (modem->state.error) { + talloc_free(modem->state.error); + } + + if (error) { + modem_log(modem, LOGL_ERROR, "failed with %s", error); + modem->state.error = talloc_strdup(modem, error); + } else { + modem->state.error = NULL; + } +} diff --git a/uqmid/modem.h b/uqmid/modem.h new file mode 100644 index 0000000..c31c966 --- /dev/null +++ b/uqmid/modem.h @@ -0,0 +1,123 @@ +/* + * uqmi -- tiny QMI support implementation + * + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#ifndef __UQMID_MODEM_H +#define __UQMID_MODEM_H + +#include "sim.h" +#include +#include + +#define IMEI_LEN 15 +#define PATH_LEN 128 + + +// try to get osmocom fsm into here? +struct modem_config { + bool configured; + char *apn; + char *pin; + bool roaming; + uint8_t pdp_type; +}; + +struct modem { + char *name; + char *device; + char imei[IMEI_LEN]; + char path[PATH_LEN]; + + char *manuf; + char *model; + char *rev; + + /* unsure if iccid should be here */ + char *iccid; + char *imsi; + + struct modem_config config; + struct { + /* Radio Access Technology */ + int rat; + /* Mobile Country Code */ + int mcc; + /* Mobile Network Code */ + int mnc; + /* len can be 2 or 3 */ + int mnc_len; + /* Operator name, can be from Sim or network. */ + char *operator_name; + /* attached to Circuit Switch/Voice */ + bool cs; + /* attached to Packet Switch/Data */ + bool ps; + /* if an error happened and the modem should stay off */ + char *error; + } state; + + /* TODO: add multiple bearer support later */ + struct { + /* The QMI internal handle */ + uint32_t packet_data_handle; + /* either ipv4, ipv6, ipv46 */ + int pdp_type; + /* valid v4_addr and ipv46 */ + struct sockaddr_in v4_addr; + struct sockaddr_in v4_netmask; + struct sockaddr_in v4_gateway; + /* valid v6 and ipv46 */ + struct sockaddr_in6 v6; + struct sockaddr_storage dns1; + struct sockaddr_storage dns2; + } brearer; + + struct { + /* Do we found a valid simcard */ + bool init; + /* Certain modems (and maybe simcards) support either unlocking the sim via pin1 or upin. */ + enum uqmi_sim_state state; + bool use_upin; + bool requires_unlock; + int pin_retries; + int puk_retries; + } sim; + + struct qmi_dev *qmi; + + struct list_head list; + struct ubus_object ubus; + + struct osmo_fsm_inst *fi; +}; + +int uqmid_modem_add(const char *name, const char *device, bool qmi_over_mbim); +int uqmid_modem_remove(struct modem *modem); +int uqmid_modem_start(struct modem *modem); +int uqmid_modem_configured(struct modem *modem); +void uqmid_modem_set_error(struct modem *modem, const char *error); + +typedef void (*uqmid_modem_get_opmode_cb)(void *data, int opmode_err, int opmode); +int uqmid_modem_get_opmode(struct modem *modem, uqmid_modem_get_opmode_cb cb, void *cb_data); +int uqmid_modem_networkstatus(struct modem *modem); +struct modem *uqmid_modem_find_by_device(const char *device); +struct modem *uqmid_modem_find_by_name(const char *name); + +#endif /* __UQMID_MODEM_H */ diff --git a/uqmid/modem_fsm.c b/uqmid/modem_fsm.c new file mode 100644 index 0000000..f86dae4 --- /dev/null +++ b/uqmid/modem_fsm.c @@ -0,0 +1,1265 @@ + +#include "osmocom/fsm.h" +#include "osmocom/utils.h" + +#include "qmi-enums-dms.h" +#include "qmi-errors.h" +#include "talloc.h" + +#include "qmi-struct.h" +#include "qmi-enums.h" +#include "qmi-message.h" +#include "qmi-enums-uim.h" + +#include "uqmid.h" +#include "logging.h" +#include "utils.h" + +#include "modem.h" +#include "services.h" +#include "modem_fsm.h" +#include "modem_tx.h" + +#define S(x) (1 << (x)) + +/* FIXME: if a service "vanished", abort/re-sync with backoff */ +/* Timeouts periods in seconds */ +#define T_GET_VERSION_S 5 +#define T_SYNC_S 3 + +/* Timeout number */ + +enum { + N_RESEND = 1, + N_FAILURE, + N_DESTROY, +}; + +static const struct value_string modem_event_names[] = { + { MODEM_EV_REQ_START, "REQ_START" }, + { MODEM_EV_REQ_DESTROY, "REQ_DESTROY" }, + { MODEM_EV_RX_SYNC, "RX_SYNC" }, + { MODEM_EV_RX_VERSION, "RX_VERSION" }, + { MODEM_EV_RX_MODEL, "RX_MODEL" }, + { MODEM_EV_RX_MANUFACTURER, "RX_MANUFACTURER" }, + { MODEM_EV_RX_REVISION, "RX_REVISION" }, + + { MODEM_EV_RX_IMSI, "RX_IMSI" }, + { MODEM_EV_RX_UIM_FAILED, "RX_UIM_FAILED" }, + { MODEM_EV_RX_UIM_GET_SLOT_FAILED, "RX_UIM_GET_SLOT_FAILED" }, + { MODEM_EV_RX_UIM_VALID_ICCID, "RX_UIM_VALID_ICCID" }, + { MODEM_EV_RX_UIM_NO_UIM_FOUND, "RX_UIM_NO_UIM_FOUND" }, + { MODEM_EV_RX_IMSI_DMS_FAILED, "RX_IMSI_DMS_FAILED" }, + + { MODEM_EV_RX_POWEROFF, "RX_POWEROFF" }, + { MODEM_EV_RX_POWERON, "RX_POWERON" }, + { MODEM_EV_RX_POWERSET, "RX_POWERSET" }, + + { MODEM_EV_RX_UNLOCKED_PIN, "RX_UNLOCKED_PIN" }, + { MODEM_EV_RX_UIM_PUK_REQUIRED, "RX_UIM_PUK_REQUIRED" }, + { MODEM_EV_RX_UIM_PIN_REQUIRED, "RX_UIM_PIN_REQUIRED" }, + { MODEM_EV_RX_UIM_READY, "RX_UIM_READY" }, + + { MODEM_EV_RX_GET_PROFILE_LIST, "RX_GET_PROFILE_LIST" }, + { MODEM_EV_RX_MODIFIED_PROFILE, "RX_MODIFIED_PROFILE" }, + { MODEM_EV_RX_CONFIGURED, "RX_CONFIGURED" }, + + { MODEM_EV_RX_SEARCHING, "RX_SEARCHING" }, + { MODEM_EV_RX_UNREGISTERED, "RX_UNREGISTERED" }, + { MODEM_EV_RX_REGISTERED, "RX_REGISTERED" }, + + { MODEM_EV_RX_SUBSCRIBED, "RX_SUBSCRIBED" }, + { MODEM_EV_RX_SUBSCRIBE_FAILED, "RX_SUBSCRIBE_FAILED" }, + + { MODEM_EV_RX_FAILED, "RX_FAILED" }, + { MODEM_EV_RX_SUCCEED, "RX_SUCCEED" }, + { 0, NULL } +}; + +#define NAS_SERVICE_POLL_TIMEOUT_S 5 + +void modem_fsm_start(struct modem *modem) +{ + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_REQ_START, NULL); +} + +static void modem_st_unconfigured(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case MODEM_EV_REQ_START: + osmo_fsm_inst_state_chg(fi, MODEM_ST_RESYNC, T_SYNC_S, N_FAILURE); + break; + } +} + +static void ctl_sync_request_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0; + + /* transaction aborted or timedout */ + if (!msg) { + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + ret = qmi_parse_ctl_sync_response(msg); + if (ret != QMI_PROTOCOL_ERROR_NONE) { + modem_log(modem, LOGL_ERROR, "Sync Request returned error %d", ret); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_SYNC, NULL); +} + +static void modem_st_resync_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *ctl = uqmi_service_find(modem->qmi, QMI_SERVICE_CTL); + + uqmi_service_send_simple(ctl, qmi_set_ctl_sync_request, ctl_sync_request_cb, modem); +} + +static void modem_st_resync(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + osmo_fsm_inst_state_chg(fi, MODEM_ST_GET_VERSION, T_GET_VERSION_S, 0); +} + +static void uqmid_modem_version_cb(struct qmi_service *ctl_service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + struct qmi_service *service; + uint8_t service_id; + uint16_t major, minor; + + struct qmi_ctl_get_version_info_response res = {}; + qmi_parse_ctl_get_version_info_response(msg, &res); + + for (int i = 0; i < res.data.service_list_n; i++) { + service_id = res.data.service_list[i].service; + major = res.data.service_list[i].major_version; + minor = res.data.service_list[i].minor_version; + + service = uqmi_service_find_or_init(modem->qmi, service_id); + if (!service) + continue; + + service->major = major; + service->minor = minor; + } + + /* FIXME: create a list of required services and validate them */ + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_VERSION, NULL); +} + +static void modem_st_get_version_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_request *req = talloc_zero(modem->qmi->ctrl, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + int ret = qmi_set_ctl_get_version_info_request(msg); + if (ret) { + LOG_ERROR("Failed to encode get version info"); + return; + } + + req->msg = msg; + req->cb = uqmid_modem_version_cb; + req->cb_data = modem; + uqmi_service_send_msg(modem->qmi->ctrl, req); +} + +static void modem_st_get_version(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case MODEM_EV_RX_VERSION: + osmo_fsm_inst_state_chg(fi, MODEM_ST_GET_MODEL, 0, 0); + break; + } +} + +static void get_manuf_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0; + + struct qmi_dms_get_manufacturer_response res = {}; + ret = qmi_parse_dms_get_manufacturer_response(msg, &res); + + if (ret) { + /* FIXME: No manufacturer. Ignoring */ + modem_log(modem, LOGL_ERROR, "Failed to get a manufacturer."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MANUFACTURER, NULL); + return; + } + + if (!res.data.manufacturer) { + /* FIXME: No manufacturer */ + modem_log(modem, LOGL_ERROR, "Got empty manufacturer."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MANUFACTURER, NULL); + return; + } + + if (modem->manuf) + talloc_free(modem->manuf); + + modem->manuf = talloc_strdup(modem, res.data.manufacturer); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MANUFACTURER, NULL); +} + +static void get_model_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0; + + struct qmi_dms_get_model_response res = {}; + ret = qmi_parse_dms_get_model_response(msg, &res); + + if (ret) { + /* FIXME: No model. Ignoring */ + modem_log(modem, LOGL_ERROR, "Failed to get a model."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MODEL, NULL); + return; + } + + if (!res.data.model) { + /* FIXME: No model */ + modem_log(modem, LOGL_ERROR, "Got empty model."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MODEL, NULL); + return; + } + + if (modem->model) + talloc_free(modem->model); + + modem->model = talloc_strdup(modem, res.data.model); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MODEL, NULL); +} + +static void get_revision_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0; + + struct qmi_dms_get_revision_response res = {}; + ret = qmi_parse_dms_get_revision_response(msg, &res); + + if (ret) { + /* FIXME: No revision. Ignoring */ + modem_log(modem, LOGL_ERROR, "Failed to get a revision."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_REVISION, NULL); + return; + } + + if (!res.data.revision) { + /* FIXME: No revision */ + modem_log(modem, LOGL_ERROR, "Got empty revision."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_REVISION, NULL); + return; + } + + if (modem->rev) + talloc_free(modem->rev); + + modem->rev = talloc_strdup(modem, res.data.revision); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_REVISION, NULL); +} + +static void modem_st_get_model_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *service = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + + if (!service) { + modem_log(modem, LOGL_ERROR, "DMS service unavailable"); + /* FIXME: fail to perm failure */ + } + + uqmi_service_send_simple(service, qmi_set_dms_get_model_request, get_model_cb, modem); +} + +static void modem_st_get_model(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct modem *modem = fi->priv; + struct qmi_service *service = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + + if (!service) { + modem_log(modem, LOGL_ERROR, "DMS service unavailable"); + /* FIXME: fail to perm failure */ + } + + /* FIXME: enable timeout and check if enough information has been gathered */ + + switch (event) { + case MODEM_EV_RX_MODEL: + uqmi_service_send_simple(service, qmi_set_dms_get_manufacturer_request, get_manuf_cb, modem); + break; + case MODEM_EV_RX_MANUFACTURER: + uqmi_service_send_simple(service, qmi_set_dms_get_revision_request, get_revision_cb, modem); + break; + case MODEM_EV_RX_REVISION: + osmo_fsm_inst_state_chg(fi, MODEM_ST_GET_IMSI, 0, 0); + break; + } +} + +static void uim_get_slot_status_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0, i = 0; + char *iccid_str; + + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to get slot status. Returned %d/%s. Trying next card status", + req->ret, qmi_get_error_str(req->ret)); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_GET_SLOT_FAILED, NULL); + return; + } + + struct qmi_uim_get_slot_status_response res = {}; + ret = qmi_parse_uim_get_slot_status_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get slot status."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_FAILED, NULL); + return; + } + + // res->data.physical_slot_status_n + // struct { + // uint32_t physical_card_status; + // uint32_t physical_slot_status; + // uint8_t logical_slot; + // unsigned int iccid_n; + // uint8_t *iccid; + // } *physical_slot_status; + + for (i = 0; i < res.data.physical_slot_status_n; ++i) { + uint32_t physical_card_status = res.data.physical_slot_status[i].physical_card_status; + uint32_t physical_slot_status = res.data.physical_slot_status[i].physical_slot_status; + /* ignoring uint8_t logical_slot */ + unsigned int iccid_n = res.data.physical_slot_status[i].iccid_n; + uint8_t *iccid_bcd = res.data.physical_slot_status[i].iccid; + + if (physical_card_status != QMI_UIM_PHYSICAL_CARD_STATE_PRESENT) + continue; + + if (physical_slot_status != QMI_UIM_SLOT_STATE_ACTIVE) + continue; + + if (iccid_n == 0) { + modem_log(modem, LOGL_INFO, "slot %d: Empty ICCID. UIM not yet ready? Or an EUICC?", i); + continue; + } + + /* FIXME: check for uneven number .. */ + int iccid_str_len = iccid_n * 2; + iccid_str = talloc_zero_size(modem, iccid_n * 2); + ret = osmo_bcd2str(iccid_str, iccid_str_len, iccid_bcd, 0, iccid_n, 1); + + if (ret) { + modem_log(modem, LOGL_INFO, "Couldn't decode ICCID of slot %d", i); + talloc_free(iccid_str); + continue; + } + + if (modem->iccid) { + modem_log(modem, LOGL_INFO, "Modem already contains an ICCID. Replacing old entry %s with %s", + modem->iccid, iccid_str); + talloc_free(modem->iccid); + } + modem->iccid = iccid_str; + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_VALID_ICCID, NULL); + } + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_NO_UIM_FOUND, NULL); +} + +static void uim_get_card_status_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0, i = 0, found = 0; + + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to get card status %d/%s.", req->ret, qmi_get_error_str(req->ret)); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_FAILED, NULL); + return; + } + + struct qmi_uim_get_card_status_response res = {}; + ret = qmi_parse_uim_get_card_status_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get card status. Decoder failed."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_FAILED, NULL); + return; + } + + for (i = 0; i < res.data.card_status.cards_n; ++i) { + if (res.data.card_status.cards[i].card_state != QMI_UIM_CARD_STATE_PRESENT) + continue; + + for (int j = 0; j < res.data.card_status.cards[i].applications_n; ++j) { + /* TODO: We should prioritize the USIM application. If not found, fallback to SIM -> ISIM -> CSIM */ + if (res.data.card_status.cards[i].applications[j].type == QMI_UIM_CARD_APPLICATION_TYPE_UNKNOWN) + continue; + + modem_log(modem, LOGL_INFO, "Found valid card application type %x", + res.data.card_status.cards[i].applications[j].type); + modem->sim.use_upin = res.data.card_status.cards[i].applications[i].upin_replaces_pin1; + + /* check if pin1 or upin is the correct method */ + if (modem->sim.use_upin) { + modem->sim.state = uim_pin_to_uqmi_state(res.data.card_status.cards[i].upin_state); + modem->sim.pin_retries = res.data.card_status.cards[i].upin_retries; + modem->sim.puk_retries = res.data.card_status.cards[i].upuk_retries; + } else { + modem->sim.state = + uim_pin_to_uqmi_state(res.data.card_status.cards[i].applications[i].pin1_state); + modem->sim.pin_retries = res.data.card_status.cards[i].applications[i].pin1_retries; + modem->sim.puk_retries = res.data.card_status.cards[i].applications[i].puk1_retries; + } + + found = 1; + break; /* handle first application only for now */ + } + + if (found) { + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_VALID_ICCID, NULL); + return; + } + } + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_NO_UIM_FOUND, NULL); +} + +static void dms_get_imsi_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0; + + struct qmi_dms_uim_get_imsi_response res = {}; + ret = qmi_parse_dms_uim_get_imsi_response(msg, &res); + + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get imsi via dms. Failed to parse message"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_IMSI_DMS_FAILED, NULL); + return; + } + + if (!res.data.imsi || strlen(res.data.imsi)) { + modem_log(modem, LOGL_INFO, "Received empty IMSI"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_IMSI_DMS_FAILED, (void *)1); + return; + } + + modem->imsi = talloc_strdup(modem, res.data.imsi); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_IMSI, NULL); +} + +static void modem_st_get_imsi_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *uim = uqmi_service_find(modem->qmi, QMI_SERVICE_UIM); + + /* FIXME: register for indication of removed uims + * - register_physical_slot_status_events + * - Physical Slot Status? + */ + if (uim) { + modem_log(modem, LOGL_INFO, "Trying to query UIM for slot status."); + uqmi_service_send_simple(uim, qmi_set_uim_get_slot_status_request, uim_get_slot_status_cb, modem); + return; + } + + /* Retrieve it via DMS */ + struct qmi_service *dms = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + if (dms) { + modem_log(modem, LOGL_INFO, "Trying to query UIM for IMSI."); + uqmi_service_send_simple(dms, qmi_set_dms_uim_get_imsi_request, dms_get_imsi_cb, modem); + return; + } +} + +static void modem_st_get_imsi(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct modem *modem = fi->priv; + struct qmi_service *uim = uqmi_service_find(modem->qmi, QMI_SERVICE_UIM); + struct qmi_service *dms = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + + /* TODO: figure out when DMS: Get IMSI is available, same for Get UIM State. + * Are those legacy calls? + */ + + switch (event) { + case MODEM_EV_RX_UIM_VALID_ICCID: + /* FIXME: ICCID must be enough for now. + * It seems only Get Record or Get Transparent file for UIM would be the only + * available solution to retrieve the IMSI. Ignoring IMSI for now. + */ + modem_log(modem, LOGL_INFO, "Found valid & usable USIM."); + osmo_fsm_inst_state_chg(fi, MODEM_ST_POWEROFF, 0, 0); + break; + case MODEM_EV_RX_UIM_GET_SLOT_FAILED: + if (uim) + uqmi_service_send_simple(uim, qmi_set_uim_get_card_status_request, uim_get_card_status_cb, + modem); + else + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UIM_FAILED, NULL); + break; + case MODEM_EV_RX_UIM_NO_UIM_FOUND: + modem_log(modem, LOGL_INFO, "No valid UIM found. Waiting for timeout to retry."); + break; + case MODEM_EV_RX_UIM_FAILED: + if (dms) + uqmi_service_send_simple(dms, qmi_set_dms_uim_get_imsi_request, dms_get_imsi_cb, modem); + break; + case MODEM_EV_RX_IMSI: + osmo_fsm_inst_state_chg(fi, MODEM_ST_POWEROFF, 0, 0); + break; + case MODEM_EV_RX_IMSI_DMS_FAILED: + modem_log(modem, LOGL_INFO, "DMS failed to retrieve the IMSI. Err %p", data); + break; + } +} + +static void dms_get_operating_mode_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + int ret = 0; + struct qmi_dms_get_operating_mode_response res = {}; + + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Status %d/%s.", req->ret, + qmi_get_error_str(req->ret)); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + ret = qmi_parse_dms_get_operating_mode_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Failed to parse message"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + if (!res.set.mode) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Failed to parse message"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + } + + modem_log(modem, LOGL_INFO, "Current in operating mode %d", res.data.mode); + switch (res.data.mode) { + case QMI_DMS_OPERATING_MODE_ONLINE: + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_POWERON, (void *)(long)res.data.mode); + break; + case QMI_DMS_OPERATING_MODE_LOW_POWER: + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_POWEROFF, (void *)(long)res.data.mode); + break; + default: + modem_log(modem, LOGL_ERROR, "Unhandled power mode! (%d) Trying to power off", res.data.mode); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_POWERON, (void *)(long)res.data.mode); + } +} + +static void dms_set_operating_mode_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to set operating mode. Status %d/%s.", req->ret, + qmi_get_error_str(req->ret)); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + /* TODO: we should request the state again to validate the modem is following our request. */ + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_POWERSET, 0); +} + +static void modem_st_poweroff_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *dms = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + + /* FIXME: abort when DMS doesn't exist */ + if (dms) + uqmi_service_send_simple(dms, qmi_set_dms_get_operating_mode_request, dms_get_operating_mode_cb, modem); +} +static void modem_st_poweroff(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct modem *modem = fi->priv; + struct qmi_service *dms = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + + switch (event) { + case MODEM_EV_RX_POWERON: + tx_dms_set_operating_mode(modem, dms, QMI_DMS_OPERATING_MODE_LOW_POWER, dms_set_operating_mode_cb); + break; + case MODEM_EV_RX_POWERSET: + uqmi_service_send_simple(dms, qmi_set_dms_get_operating_mode_request, dms_get_operating_mode_cb, modem); + break; + case MODEM_EV_RX_POWEROFF: + if (modem->config.configured && !modem->state.error) + osmo_fsm_inst_state_chg(fi, MODEM_ST_UNLOCK_PIN, 0, 0); + else /* stop timer and wait for the user to continue */ + osmo_timer_del(&fi->timer); + break; + case MODEM_EV_REQ_CONFIGURED: /* when the modem reach this state, but isn't yet configured, wait for the config */ + osmo_fsm_inst_state_chg(fi, MODEM_ST_UNLOCK_PIN, 0, 0); + break; + } +} + +static void modem_st_unlock_pin_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + osmo_fsm_inst_state_chg(fi, MODEM_ST_CONFIGURE_MODEM, 0, 0); +} +static void modem_st_unlock_pin(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ +} + +static void wds_modify_profile_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + struct qmi_wds_modify_profile_response res = {}; + long err = 1; + int ret; + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to modify profile list. Status %d/%s.", req->ret, + qmi_get_error_str(req->ret)); + /* FIXME: add extended profile error */ + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MODIFIED_PROFILE, (void *)err); + return; + } + + ret = qmi_parse_wds_modify_profile_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to modify profile list. Failed to parse message"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MODIFIED_PROFILE, (void *)err); + return; + } + + err = 0; + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_MODIFIED_PROFILE, (void *)err); +} + +static void wds_get_profile_list_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + struct qmi_wds_get_profile_list_response res = {}; + long err = 1; + int ret; + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to get profile list. Status %d/%s.", req->ret, + qmi_get_error_str(req->ret)); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + ret = qmi_parse_wds_get_profile_list_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Failed to parse message"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + for (int i = 0; i < res.data.profile_list_n; i++) { + uint8_t type = res.data.profile_list[i].profile_type; + uint8_t idx = res.data.profile_list[i].profile_index; + /* const char *name = res.data.profile_list[i].profile_name; */ + if (type != QMI_WDS_PROFILE_TYPE_3GPP) + continue; + + err = 0; + /* we're taking the first one and ignoring the rest for now */ + modem_log(modem, LOGL_INFO, "Found profile id %d", idx); + modem->qmi->wds.profile_id = idx; + modem->qmi->wds.valid = 1; + } + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_GET_PROFILE_LIST, (void *)err); +} + +static void modem_st_configure_modem_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *wds = uqmi_service_find(modem->qmi, QMI_SERVICE_WDS); + + switch (modem->config.pdp_type) { + case QMI_WDS_PDP_TYPE_IPV4: + modem->qmi->wds.ip_family = QMI_WDS_IP_FAMILY_IPV4; + break; + /* FIXME: add support for IPV4/IPV6 dual stack */ + case QMI_WDS_PDP_TYPE_IPV4_OR_IPV6: + case QMI_WDS_PDP_TYPE_IPV6: + modem->qmi->wds.ip_family = QMI_WDS_IP_FAMILY_IPV6; + break; + default: + modem->qmi->wds.ip_family = QMI_WDS_IP_FAMILY_UNSPECIFIED; + break; + } + + tx_wds_get_profile_list(modem, wds, wds_get_profile_list_cb); +} + +static void modem_st_configure_modem(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct modem *modem = fi->priv; + struct qmi_service *wds = uqmi_service_find(modem->qmi, QMI_SERVICE_WDS); + + /* get Profile list, if no Profile, + * create one () + * modify profile () */ + + switch (event) { + case MODEM_EV_RX_GET_PROFILE_LIST: + /* err */ + if (data) { + /* failed to get profile list/generate a new profile */ + } else { + tx_wds_modify_profile(modem, wds, wds_modify_profile_cb, modem->qmi->wds.profile_id, + modem->config.apn, modem->config.pdp_type); + } + break; + case MODEM_EV_RX_MODIFIED_PROFILE: + osmo_fsm_inst_state_chg(fi, MODEM_ST_POWERON, 0, 0); + break; + } + /* TODO: check if this is the default profile! */ +} + +static void modem_st_poweron_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *dms = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + + /* FIXME: abort when DMS doesn't exist */ + if (dms) + uqmi_service_send_simple(dms, qmi_set_dms_get_operating_mode_request, dms_get_operating_mode_cb, modem); +} +static void modem_st_poweron(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct modem *modem = fi->priv; + struct qmi_service *dms = uqmi_service_find(modem->qmi, QMI_SERVICE_DMS); + + switch (event) { + case MODEM_EV_RX_POWEROFF: + tx_dms_set_operating_mode(modem, dms, QMI_DMS_OPERATING_MODE_ONLINE, dms_set_operating_mode_cb); + break; + case MODEM_EV_RX_POWERSET: + case MODEM_EV_RX_POWERON: + osmo_fsm_inst_state_chg(fi, MODEM_ST_NETSEARCH, NAS_SERVICE_POLL_TIMEOUT_S, 0); + break; + } +} + +static void subscribe_result_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + if (req->ret) { + modem_log(modem, LOGL_INFO, "Service %d: Subscribe check failed with %d/%s", service->service, req->ret, + qmi_get_error_str(req->ret)); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_SUBSCRIBE_FAILED, + (void *)(long)le32_to_cpu(msg->svc.message)); + return; + } + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_SUBSCRIBED, (void *)(long)le32_to_cpu(msg->svc.message)); +} + +static void get_serving_system_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + struct qmi_nas_get_serving_system_response res = {}; + + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to get system info. Status %d/%s", req->ret, + qmi_get_error_str(req->ret)); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)(long)1); + return; + } + + int ret = qmi_parse_nas_get_serving_system_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_ERROR, "Failed to decode serving system response"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)(long)1); + return; + } + + if (!res.set.serving_system) { + modem_log(modem, LOGL_ERROR, "No serving system in get serving system"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)(long)1); + return; + } + + modem_log(modem, LOGL_INFO, "Network registration state %d", res.data.serving_system.registration_state); + + switch (res.data.serving_system.registration_state) { + case QMI_NAS_REGISTRATION_STATE_REGISTERED: + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_REGISTERED, NULL); + return; + case QMI_NAS_REGISTRATION_STATE_NOT_REGISTERED: + case QMI_NAS_REGISTRATION_STATE_UNKNOWN: + case QMI_NAS_REGISTRATION_STATE_REGISTRATION_DENIED: + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_UNREGISTERED, NULL); + return; + case QMI_NAS_REGISTRATION_STATE_NOT_REGISTERED_SEARCHING: + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_SEARCHING, NULL); + return; + } +} + +static void nas_force_search_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + if (req->ret) { + modem_log(modem, LOGL_INFO, "Failed to force a network search. Status %d/%s", req->ret, + qmi_get_error_str(req->ret)); + /* FIXME: charge timer */ + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } +} + +static void modem_st_netsearch_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *nas = uqmi_service_find(modem->qmi, QMI_SERVICE_NAS); + tx_nas_subscribe_nas_events(modem, nas, 1, subscribe_result_cb); +} + +static void modem_st_netsearch(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + /* TODO: register to internal indications */ + struct modem *modem = fi->priv; + struct qmi_service *nas = uqmi_service_find(modem->qmi, QMI_SERVICE_NAS); + + switch (event) { + case MODEM_EV_RX_REGISTERED: + osmo_fsm_inst_state_chg(fi, MODEM_ST_REGISTERED, 0, 0); + break; + case MODEM_EV_RX_FAILED: + osmo_timer_schedule(&fi->timer, 5, 0); + break; + case MODEM_EV_RX_SUBSCRIBED: + case MODEM_EV_RX_SUBSCRIBE_FAILED: + uqmi_service_send_simple(nas, qmi_set_nas_get_serving_system_request, get_serving_system_cb, modem); + break; + case MODEM_EV_RX_UNREGISTERED: + modem_log(modem, LOGL_INFO, "Start network search."); + uqmi_service_send_simple(nas, qmi_set_nas_force_network_search_request, nas_force_search_cb, modem); + break; + } +} + +static void modem_st_registered_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + osmo_fsm_inst_state_chg(fi, MODEM_ST_START_IFACE, 5, 0); +} +static void modem_st_registered(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ +} + +static void wds_start_network_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + struct qmi_wds_start_network_response res = {}; + long err = 1; + int ret; + + ret = qmi_parse_wds_start_network_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get operating mode. Failed to parse message"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + /* to get error causes */ + err = ret = req->ret; + if (ret && ret == QMI_PROTOCOL_ERROR_NO_EFFECT) { + modem_log(modem, LOGL_INFO, "Start network return NO_EFFECT. Trying to continue"); + } else if (ret) { + modem_log(modem, LOGL_INFO, "Failed start network. QMI Status %d/%s.", req->ret, + qmi_get_error_str(req->ret)); + if (res.set.call_end_reason) + modem_log(modem, LOGL_INFO, "Call End Reason %x", res.data.call_end_reason); + + if (res.set.verbose_call_end_reason) { + modem_log(modem, LOGL_INFO, "Verbose End Reason type %x/reason %x", + res.data.verbose_call_end_reason.type, res.data.verbose_call_end_reason.reason); + } + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)(long)req->ret); + return; + } + + if (res.set.packet_data_handle) { + modem->qmi->wds.packet_handle = res.data.packet_data_handle; + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_SUCCEED, (void *)err); + } else { + modem_log(modem, LOGL_INFO, "No packet data handle. Suspicious..."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)err); + } +} + +static void modem_st_start_iface_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *wds = uqmi_service_find(modem->qmi, QMI_SERVICE_WDS); + + tx_wds_start_network(modem, wds, wds_start_network_cb, modem->qmi->wds.profile_id, modem->qmi->wds.ip_family); +} +static void modem_st_start_iface(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct modem *modem = fi->priv; + long reason; + + switch (event) { + case MODEM_EV_RX_FAILED: + reason = (long)data; + if (reason == QMI_PROTOCOL_ERROR_CALL_FAILED) { + fi->T = N_RESEND; + } else { + uqmid_modem_set_error(modem, "Start Iface/Network failed!"); + osmo_fsm_inst_state_chg(fi, MODEM_ST_POWEROFF, 0, 0); + } + break; + case MODEM_EV_RX_SUCCEED: + osmo_fsm_inst_state_chg(fi, MODEM_ST_LIVE, 0, 0); + break; + } +} + +static void ipv4_to_sockaddr(const uint32_t wds_ip, struct sockaddr_in *addr) +{ + memset(addr, 0, sizeof(*addr)); + addr->sin_family = AF_INET; + addr->sin_addr.s_addr = htonl(wds_ip); +} + +static void ipv6_to_sockaddr(const uint16_t *wds_ip6, struct sockaddr_in6 *addr) +{ + memset(addr, 0, sizeof(*addr)); + addr->sin6_family = AF_INET6; + memcpy(&addr->sin6_addr, wds_ip6, sizeof(struct in6_addr)); +} + +static void wds_get_current_settings_cb(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg) +{ + struct modem *modem = req->cb_data; + struct qmi_wds_get_current_settings_response res = {}; + int ret; + + ret = qmi_parse_wds_get_current_settings_response(msg, &res); + if (ret) { + modem_log(modem, LOGL_INFO, "Failed to get current settings. Failed to parse message"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, NULL); + return; + } + + if (!res.set.ip_family) { + modem_log(modem, LOGL_ERROR, "Modem didn't include ip family"); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)1); + } + + memset(&modem->brearer.dns1, 0, sizeof(modem->brearer.dns1)); + memset(&modem->brearer.dns2, 0, sizeof(modem->brearer.dns2)); + + switch (res.data.ip_family) { + case QMI_WDS_IP_FAMILY_IPV4: + memset(&modem->brearer.v4_addr, 0, sizeof(modem->brearer.v4_addr)); + memset(&modem->brearer.v4_netmask, 0, sizeof(modem->brearer.v4_netmask)); + memset(&modem->brearer.v4_gateway, 0, sizeof(modem->brearer.v4_gateway)); + + if (!res.set.ipv4_address) { + modem_log(modem, LOGL_ERROR, "Modem didn't include IPv4 Address."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)2); + } + ipv4_to_sockaddr(res.data.ipv4_address, &modem->brearer.v4_addr); + + /* Still unsure why there is a this subnet mask. maybe natt'ed by the modem? */ + if (res.set.ipv4_gateway_subnet_mask) { + ipv4_to_sockaddr(res.data.ipv4_gateway_subnet_mask, + (struct sockaddr_in *)&modem->brearer.v4_netmask); + } + + if (res.set.ipv4_gateway_address) { + ipv4_to_sockaddr(res.data.ipv4_gateway_address, + (struct sockaddr_in *)&modem->brearer.v4_gateway); + } + + if (res.set.primary_ipv4_dns_address) { + ipv4_to_sockaddr(res.data.primary_ipv4_dns_address, (struct sockaddr_in *)&modem->brearer.dns1); + } + + if (res.set.secondary_ipv4_dns_address) { + ipv4_to_sockaddr(res.data.secondary_ipv4_dns_address, + (struct sockaddr_in *)&modem->brearer.dns2); + } + break; + case QMI_WDS_IP_FAMILY_IPV6: + if (!res.set.ipv6_address) { + modem_log(modem, LOGL_ERROR, "Modem didn't include IPv6 Address."); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)3); + } + + if (res.set.ipv6_primary_dns_address) { + ipv6_to_sockaddr(res.data.ipv6_primary_dns_address, + (struct sockaddr_in6 *)&modem->brearer.dns1); + } + + if (res.set.ipv6_secondary_dns_address) { + ipv6_to_sockaddr(res.data.ipv6_secondary_dns_address, + (struct sockaddr_in6 *)&modem->brearer.dns2); + } + + break; + default: + modem_log(modem, LOGL_ERROR, "Modem reported an unknown ip_family %d.", res.data.ip_family); + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_FAILED, (void *)4); + break; + } + + osmo_fsm_inst_dispatch(modem->fi, MODEM_EV_RX_SUCCEED, NULL); +} + +static void modem_st_live_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + struct qmi_service *wds = uqmi_service_find(modem->qmi, QMI_SERVICE_WDS); + + tx_wds_get_current_settings(modem, wds, wds_get_current_settings_cb); + /* TODO: register to indications */ +} + +static void modem_st_live(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case MODEM_EV_RX_FAILED: + break; + case MODEM_EV_RX_SUCCEED: + break; + } +} + +static void modem_st_failed(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ +} + +static void modem_st_destroy_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct modem *modem = fi->priv; + + /* FIXME: try to close the network, poweroff the modem ? */ + qmi_device_close(modem->qmi, 5000); +} +static void modem_st_destroy(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ +} + +static int modem_fsm_timer_cb(struct osmo_fsm_inst *fi) +{ + struct modem *modem = fi->priv; + struct qmi_service *service; + + switch (fi->state) { + case MODEM_ST_NETSEARCH: + service = uqmi_service_find(modem->qmi, QMI_SERVICE_NAS); + if (!service) { + modem_log(modem, LOGL_ERROR, "NAS service doesn't exist"); + return 1; + } + uqmi_service_send_simple(service, qmi_set_nas_get_serving_system_request, get_serving_system_cb, modem); + osmo_timer_schedule(&fi->timer, NAS_SERVICE_POLL_TIMEOUT_S, 0); + break; + case MODEM_ST_START_IFACE: + switch (fi->T) { + case N_RESEND: + /* resend the packet */ + service = uqmi_service_find(modem->qmi, QMI_SERVICE_WDS); + if (!service) { + modem_log(modem, LOGL_ERROR, "WDS service doesn't exist"); + return 1; + } + tx_wds_start_network(modem, service, wds_start_network_cb, modem->qmi->wds.profile_id, + modem->qmi->wds.ip_family); + osmo_timer_schedule(&fi->timer, 5, 0); + break; + default: + /* we timedout ! No answer? */ + modem_log(modem, LOGL_ERROR, "WDS: Couldn't start the interface!"); + /* TODO: enter failure state */ + break; + } + break; + default: + switch (fi->T) { + case N_FAILURE: + modem_log(modem, LOGL_ERROR, "State timedout. Entering failure state"); + osmo_fsm_inst_state_chg(fi, MODEM_ST_FAILED, 0, 0); + break; + } + + break; + } + + return 0; +} + +static void modem_fsm_allstate_action(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case MODEM_EV_REQ_DESTROY: + osmo_fsm_inst_state_chg(fi, MODEM_ST_DESTROY, 0, 0); + break; + } +} + +static const struct osmo_fsm_state modem_states[] = { + [MODEM_ST_IDLE] = { + .in_event_mask = S(MODEM_EV_REQ_START), + .out_state_mask = S(MODEM_ST_RESYNC) | S(MODEM_ST_DESTROY), + .name = "UNCONFIGURED", + .action = modem_st_unconfigured, + }, + [MODEM_ST_RESYNC] = { + .in_event_mask = S(MODEM_EV_RX_SYNC), + .out_state_mask = S(MODEM_ST_GET_VERSION) + | S(MODEM_ST_FAILED) + | S(MODEM_ST_DESTROY), + .name = "RESYNC", + .action = modem_st_resync, + .onenter = modem_st_resync_onenter, + }, + [MODEM_ST_GET_VERSION] = { + .in_event_mask = S(MODEM_EV_RX_VERSION), + .out_state_mask = S(MODEM_ST_GET_MODEL) | S(MODEM_ST_DESTROY), + .name = "GET_VERSION", + .action = modem_st_get_version, + .onenter = modem_st_get_version_onenter, + }, + [MODEM_ST_GET_MODEL] = { + .in_event_mask = S(MODEM_EV_RX_MODEL) | S(MODEM_EV_RX_MANUFACTURER) | S(MODEM_EV_RX_REVISION), + .out_state_mask = S(MODEM_ST_GET_IMSI) | S(MODEM_ST_DESTROY), + .name = "GET_MODEL", + .action = modem_st_get_model, + .onenter = modem_st_get_model_onenter, + }, + [MODEM_ST_GET_IMSI] = { + .in_event_mask = S(MODEM_EV_RX_IMSI) + | S(MODEM_EV_RX_UIM_FAILED) + | S(MODEM_EV_RX_UIM_GET_SLOT_FAILED) + | S(MODEM_EV_RX_UIM_VALID_ICCID) + | S(MODEM_EV_RX_UIM_NO_UIM_FOUND) + | S(MODEM_EV_RX_IMSI_DMS_FAILED), + .out_state_mask = S(MODEM_ST_POWEROFF) | S(MODEM_ST_DESTROY), + .name = "GET_IMSI", + .action = modem_st_get_imsi, + .onenter = modem_st_get_imsi_onenter, + }, + [MODEM_ST_POWEROFF] = { + .in_event_mask = S(MODEM_EV_RX_POWEROFF) + | S(MODEM_EV_RX_POWERON) + | S(MODEM_EV_RX_POWERSET) + | S(MODEM_EV_REQ_CONFIGURED), + .out_state_mask = S(MODEM_ST_UNLOCK_PIN) | S(MODEM_ST_DESTROY), + .name = "POWEROFF", + .action = modem_st_poweroff, + .onenter = modem_st_poweroff_onenter, + }, + [MODEM_ST_UNLOCK_PIN] = { + .in_event_mask = S(MODEM_EV_RX_UNLOCKED_PIN), + .out_state_mask = S(MODEM_ST_CONFIGURE_MODEM) | S(MODEM_ST_DESTROY), + .name = "UNLOCK_PIN", + .action = modem_st_unlock_pin, + .onenter = modem_st_unlock_pin_onenter, + }, + [MODEM_ST_CONFIGURE_MODEM] = { + .in_event_mask = S(MODEM_EV_RX_CONFIGURED) + | S(MODEM_EV_RX_GET_PROFILE_LIST) + | S(MODEM_EV_RX_MODIFIED_PROFILE), + .out_state_mask = S(MODEM_ST_POWERON) | S(MODEM_ST_DESTROY), + .name = "CONFIGURE_MODEM", + .action = modem_st_configure_modem, + .onenter = modem_st_configure_modem_onenter, + }, + [MODEM_ST_POWERON] = { + .in_event_mask = S(MODEM_EV_RX_POWEROFF) + | S(MODEM_EV_RX_POWERON) + | S(MODEM_EV_RX_POWERSET), + .out_state_mask = S(MODEM_ST_NETSEARCH) | S(MODEM_ST_DESTROY), + .name = "POWERON", + .action = modem_st_poweron, + .onenter = modem_st_poweron_onenter, + }, + [MODEM_ST_NETSEARCH] = { + .in_event_mask = S(MODEM_EV_RX_REGISTERED) + | S(MODEM_EV_RX_UNREGISTERED) + | S(MODEM_EV_RX_SEARCHING) + | S(MODEM_EV_RX_FAILED) + | S(MODEM_EV_RX_SUBSCRIBED) + | S(MODEM_EV_RX_SUBSCRIBE_FAILED), + .out_state_mask = S(MODEM_ST_REGISTERED) | S(MODEM_ST_DESTROY), + .name = "NETSEARCH", + .action = modem_st_netsearch, + .onenter = modem_st_netsearch_onenter, + }, + [MODEM_ST_REGISTERED] = { + .in_event_mask = 0, + .out_state_mask = S(MODEM_ST_START_IFACE) | S(MODEM_ST_DESTROY), + .name = "REGISTERED", + .action = modem_st_registered, + .onenter = modem_st_registered_onenter, + }, + [MODEM_ST_START_IFACE] = { + .in_event_mask = S(MODEM_EV_RX_SUCCEED) + | S(MODEM_EV_RX_FAILED), + .out_state_mask = S(MODEM_ST_LIVE) | S(MODEM_ST_DESTROY) | S(MODEM_ST_POWEROFF), + .name = "START_IFACE", + .action = modem_st_start_iface, + .onenter = modem_st_start_iface_onenter, + }, + [MODEM_ST_LIVE] = { + .in_event_mask = S(MODEM_EV_RX_SUCCEED) | S(MODEM_EV_RX_FAILED), + .out_state_mask = S(MODEM_ST_DESTROY), + .name = "LIVE", + .action = modem_st_live, + .onenter = modem_st_live_onenter, + }, + [MODEM_ST_FAILED] = { + .in_event_mask = 0, + .out_state_mask = S(MODEM_ST_DESTROY), + .name = "FAILED", + .action = modem_st_failed, + // .onenter = modem_st_live_onenter,s + }, + [MODEM_ST_DESTROY] = { + .in_event_mask = 0, + .out_state_mask = 0, + .name = "DESTROY", + .action = modem_st_destroy, + .onenter = modem_st_destroy_onenter, + }, +}; + +static struct osmo_fsm modem_fsm = { + .name = "MODEM", + .states = modem_states, + .num_states = ARRAY_SIZE(modem_states), + .allstate_event_mask = S(MODEM_EV_REQ_DESTROY), + .allstate_action = modem_fsm_allstate_action, + // .cleanup = modem_fsm_cleanup, + .timer_cb = modem_fsm_timer_cb, + .event_names = modem_event_names, + .pre_term = NULL, +}; + +struct osmo_fsm_inst *modem_fsm_alloc(struct modem *modem) +{ + return osmo_fsm_inst_alloc(&modem_fsm, modem, modem, LOGL_DEBUG, modem->name); +} + +static __attribute__((constructor)) void on_dso_load_ctx(void) +{ + OSMO_ASSERT(osmo_fsm_register(&modem_fsm) == 0); +} diff --git a/uqmid/modem_fsm.h b/uqmid/modem_fsm.h new file mode 100644 index 0000000..c91ea4d --- /dev/null +++ b/uqmid/modem_fsm.h @@ -0,0 +1,71 @@ +#ifndef __UQMID_MODEM_FSM_H +#define __UQMID_MODEM_FSM_H + +enum modem_fsm_state { + MODEM_ST_IDLE, + MODEM_ST_RESYNC, + MODEM_ST_GET_VERSION, + MODEM_ST_GET_MODEL, + MODEM_ST_GET_IMSI, + MODEM_ST_POWEROFF, + MODEM_ST_UNLOCK_PIN, + MODEM_ST_CONFIGURE_MODEM, + MODEM_ST_POWERON, + MODEM_ST_NETSEARCH, + MODEM_ST_REGISTERED, + MODEM_ST_START_IFACE, + MODEM_ST_LIVE, + MODEM_ST_FAILED, + MODEM_ST_DESTROY, +}; + +enum modem_fsm_event { + MODEM_EV_REQ_START, + MODEM_EV_REQ_CONFIGURED, + + MODEM_EV_RX_SYNC, + MODEM_EV_RX_VERSION, + + MODEM_EV_RX_MODEL, + MODEM_EV_RX_MANUFACTURER, + MODEM_EV_RX_REVISION, + + MODEM_EV_RX_IMSI, + MODEM_EV_RX_UIM_FAILED, + MODEM_EV_RX_UIM_GET_SLOT_FAILED, + MODEM_EV_RX_UIM_VALID_ICCID, + MODEM_EV_RX_UIM_NO_UIM_FOUND, + MODEM_EV_RX_IMSI_DMS_FAILED, + + MODEM_EV_RX_POWEROFF, + MODEM_EV_RX_POWERON, + MODEM_EV_RX_POWERSET, + + MODEM_EV_RX_UNLOCKED_PIN, + MODEM_EV_RX_UIM_PUK_REQUIRED, + MODEM_EV_RX_UIM_PIN_REQUIRED, + MODEM_EV_RX_UIM_READY, + + MODEM_EV_RX_GET_PROFILE_LIST, + MODEM_EV_RX_MODIFIED_PROFILE, + MODEM_EV_RX_CONFIGURED, + + MODEM_EV_RX_GET_SERVING_SYSTEM, + MODEM_EV_RX_REGISTERED, + MODEM_EV_RX_UNREGISTERED, + MODEM_EV_RX_SEARCHING, + + MODEM_EV_RX_SUBSCRIBED, + MODEM_EV_RX_SUBSCRIBE_FAILED, + + MODEM_EV_RX_FAILED, + MODEM_EV_RX_SUCCEED, /* a generic callback succeeded */ + MODEM_EV_REQ_DESTROY, +}; + + +struct modem; +void modem_fsm_start(struct modem *modem); +struct osmo_fsm_inst *modem_fsm_alloc(struct modem *modem); + +#endif /* __UQMID_MODEM_FSM_H */ diff --git a/uqmid/modem_tx.c b/uqmid/modem_tx.c new file mode 100644 index 0000000..f57a86a --- /dev/null +++ b/uqmid/modem_tx.c @@ -0,0 +1,180 @@ +/* + * uqmi -- tiny QMI support implementation + * + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +/* Used by uqmid to contain the modem state */ + +#include +#include + +#include "qmi-struct.h" +#include "qmi-enums.h" +#include "qmi-message.h" +#include "qmi-enums-uim.h" + +#include "logging.h" +#include "utils.h" + +#include "modem.h" +#include "services.h" +#include "modem_fsm.h" + +#include "modem_tx.h" + +int tx_dms_set_operating_mode(struct modem *modem, struct qmi_service *dms, uint8_t operating_mode, request_cb cb) +{ + struct qmi_request *req = talloc_zero(dms, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + struct qmi_dms_set_operating_mode_request set_op_mode_req = { QMI_INIT(mode, operating_mode) }; + + int ret = qmi_set_dms_set_operating_mode_request(msg, &set_op_mode_req); + if (ret) { + LOG_ERROR("Failed to encode get version info"); + return 1; + } + + req->msg = msg; + req->cb = cb; + req->cb_data = modem; + return uqmi_service_send_msg(dms, req); +} + +int tx_nas_subscribe_nas_events(struct modem *modem, struct qmi_service *nas, bool action, request_cb cb) +{ + struct qmi_request *req = talloc_zero(nas, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + struct qmi_nas_register_indications_request register_req = {}; + qmi_set(®ister_req, serving_system_events, action); + qmi_set(®ister_req, subscription_info, action); + qmi_set(®ister_req, system_info, action); + qmi_set(®ister_req, signal_info, action); + register_req.set.network_reject_information = 1; + register_req.data.network_reject_information.enable_network_reject_indications = action; + + int ret = qmi_set_nas_register_indications_request(msg, ®ister_req); + if (ret) { + LOG_ERROR("Failed to encode get version info"); + return 1; + } + + req->msg = msg; + req->cb = cb; + req->cb_data = modem; + return uqmi_service_send_msg(nas, req); +} + +int tx_wds_get_profile_list(struct modem *modem, struct qmi_service *wds, request_cb cb) +{ + struct qmi_request *req = talloc_zero(wds, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + struct qmi_wds_get_profile_list_request profile_req = { QMI_INIT(profile_type, QMI_WDS_PROFILE_TYPE_3GPP) }; + + int ret = qmi_set_wds_get_profile_list_request(msg, &profile_req); + if (ret) { + LOG_ERROR("Failed to encode get profile list"); + return 1; + } + + req->msg = msg; + req->cb = cb; + req->cb_data = modem; + return uqmi_service_send_msg(wds, req); +} + +int tx_wds_modify_profile(struct modem *modem, struct qmi_service *wds, request_cb cb, uint8_t profile, const char *apn, + uint8_t pdp_type) +{ + struct qmi_request *req = talloc_zero(wds, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + struct qmi_wds_modify_profile_request profile_req = {}; + + profile_req.set.profile_identifier = 1; + profile_req.data.profile_identifier.profile_type = QMI_WDS_PROFILE_TYPE_3GPP; + profile_req.data.profile_identifier.profile_index = profile; + qmi_set(&profile_req, pdp_type, pdp_type); + + if (apn) + profile_req.data.apn_name = (char *) apn; + + int ret = qmi_set_wds_modify_profile_request(msg, &profile_req); + if (ret) { + LOG_ERROR("Failed to encode get profile list"); + return 1; + } + + req->msg = msg; + req->cb = cb; + req->cb_data = modem; + return uqmi_service_send_msg(wds, req); +} + +int tx_wds_start_network(struct modem *modem, struct qmi_service *wds, request_cb cb, uint8_t profile_idx, + uint8_t ip_family) +{ + struct qmi_request *req = talloc_zero(wds, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + struct qmi_wds_start_network_request start_req = {}; + qmi_set(&start_req, profile_index_3gpp, profile_idx); + qmi_set(&start_req, ip_family_preference, ip_family); + + int ret = qmi_set_wds_start_network_request(msg, &start_req); + if (ret) { + LOG_ERROR("Failed to encode start network request"); + return 1; + } + + req->msg = msg; + req->cb = cb; + req->cb_data = modem; + return uqmi_service_send_msg(wds, req); +} + +int tx_wds_get_current_settings(struct modem *modem, struct qmi_service *wds, request_cb cb) +{ + struct qmi_request *req = talloc_zero(wds, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + struct qmi_wds_get_current_settings_request get_settings_req = {}; + qmi_set(&get_settings_req, requested_settings, + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_PDP_TYPE | + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_DNS_ADDRESS | + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_GRANTED_QOS | + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_IP_ADDRESS | + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_GATEWAY_INFO | + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_MTU | + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_DOMAIN_NAME_LIST | + QMI_WDS_GET_CURRENT_SETTINGS_REQUESTED_SETTINGS_IP_FAMILY); + + int ret = qmi_set_wds_get_current_settings_request(msg, &get_settings_req); + if (ret) { + LOG_ERROR("Failed to encode start network request"); + return 1; + } + + req->msg = msg; + req->cb = cb; + req->cb_data = modem; + return uqmi_service_send_msg(wds, req); +} diff --git a/uqmid/modem_tx.h b/uqmid/modem_tx.h new file mode 100644 index 0000000..7b1b8fb --- /dev/null +++ b/uqmid/modem_tx.h @@ -0,0 +1,20 @@ +#ifndef __UQMID_MODEM_TX_H +#define __UQMID_MODEM_TX_H + +#include "uqmid.h" + +#include + +struct modem; +struct qmi_service; + +int tx_dms_set_operating_mode(struct modem *modem, struct qmi_service *dms, uint8_t operating_mode, request_cb cb); +int tx_nas_subscribe_nas_events(struct modem *modem, struct qmi_service *nas, bool action, request_cb cb); +int tx_wds_get_profile_list(struct modem *modem, struct qmi_service *wds, request_cb cb); +int tx_wds_modify_profile(struct modem *modem, struct qmi_service *wds, request_cb cb, uint8_t profile, const char *apn, + uint8_t pdp_type); +int tx_wds_start_network(struct modem *modem, struct qmi_service *wds, request_cb cb, uint8_t profile_idx, + uint8_t ip_family); +int tx_wds_get_current_settings(struct modem *modem, struct qmi_service *wds, request_cb cb); + +#endif /* __UQMID_MODEM_TX_H */ diff --git a/uqmid/osmocom/CMakeLists.txt b/uqmid/osmocom/CMakeLists.txt new file mode 100644 index 0000000..41bde17 --- /dev/null +++ b/uqmid/osmocom/CMakeLists.txt @@ -0,0 +1,2 @@ + +ADD_LIBRARY(osmofsm fsm.c utils.c timer.c) diff --git a/uqmid/osmocom/README.md b/uqmid/osmocom/README.md new file mode 100644 index 0000000..1f66614 --- /dev/null +++ b/uqmid/osmocom/README.md @@ -0,0 +1,6 @@ +# osmocom imports + +The files fsm.c/fsm.h has been imported from libosmocore v1.9.0 +under the GPLv2+. + +It has been modified to work with libubox as underlying event loop abstraction. diff --git a/uqmid/osmocom/fsm.c b/uqmid/osmocom/fsm.c new file mode 100644 index 0000000..250ff24 --- /dev/null +++ b/uqmid/osmocom/fsm.c @@ -0,0 +1,1046 @@ +/*! \file fsm.c + * Osmocom generic Finite State Machine implementation. */ +/* + * (C) 2016-2019 by Harald Welte + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include +#include +#include +#include +#include + +#include "fsm.h" +#include "logging.h" +#include "utils.h" + +/*! \addtogroup fsm + * @{ + * Finite State Machine abstraction + * + * This is a generic C-language abstraction for implementing finite + * state machines within the Osmocom framework. It is intended to + * replace existing hand-coded or even only implicitly existing FSMs + * all over the existing code base. + * + * An libosmocore FSM is described by its \ref osmo_fsm description, + * which in turn refers to an array of \ref osmo_fsm_state descriptor, + * each describing a single state in the FSM. + * + * The general idea is that all actions performed within one state are + * located at one position in the code (the state's action function), + * as opposed to the 'message-centric' view of e.g. the existing + * state machines of the LAPD(m) core, where there is one message for + * each possible event (primitive), and the function then needs to + * concern itself on how to handle that event over all possible states. + * + * For each state, there is a bit-mask of permitted input events for + * this state, as well as a bit-mask of permitted new output states to + * which the state can change. Furthermore, there is a function + * pointer implementing the actual handling of the input events + * occurring whilst in that state. + * + * Furthermore, each state offers a function pointer that can be + * executed just before leaving a state, and another one just after + * entering a state. + * + * When transitioning into a new state, an optional timer number and + * time-out can be passed along. The timer is started just after + * entering the new state, and will call the \ref osmo_fsm timer_cb + * function once it expires. This is intended to be used in telecom + * state machines where a given timer (identified by a certain number) + * is started to terminate the fsm or terminate the fsm once expected + * events are not happening before timeout expiration. + * + * As there can often be many concurrent FSMs of one given class, we + * introduce the concept of \ref osmo_fsm_inst, i.e. an FSM instance. + * The instance keeps the actual state, while the \ref osmo_fsm + * descriptor contains the static/const descriptor of the FSM's states + * and possible transitions. + * + * osmo_fsm are integrated with the libosmocore logging system. The + * logging sub-system is determined by the FSM descriptor, as we assume + * one FSM (let's say one related to a location update procedure) is + * inevitably always tied to a sub-system. The logging level however + * is configurable for each FSM instance, to ensure that e.g. DEBUG + * logging can be used for the LU procedure of one subscriber, while + * NOTICE level is used for all other subscribers. + * + * In order to attach private state to the \ref osmo_fsm_inst, it + * offers an opaque private pointer. + * + * \file fsm.c */ + +LIST_HEAD(osmo_g_fsms); +static bool fsm_log_addr = true; +static bool fsm_log_timeouts = false; +/*! See osmo_fsm_term_safely(). */ +static bool fsm_term_safely_enabled = false; + +/*! Internal state for FSM instance termination cascades. */ +static __thread struct { + /*! The first FSM instance that invoked osmo_fsm_inst_term() in the current cascade. */ + struct osmo_fsm_inst *root_fi; + /*! 2 if a secondary FSM terminates, 3 if a secondary FSM causes a tertiary FSM to terminate, and so on. */ + unsigned int depth; + /*! Talloc context to collect all deferred deallocations (FSM instances, and talloc objects if any). */ + void *collect_ctx; + /*! See osmo_fsm_set_dealloc_ctx() */ + void *fsm_dealloc_ctx; +} fsm_term_safely; + +/*! Internal call to free an FSM instance, which redirects to the context set by osmo_fsm_set_dealloc_ctx() if any. + */ +static void fsm_free_or_steal(void *talloc_object) +{ + if (fsm_term_safely.fsm_dealloc_ctx) + talloc_steal(fsm_term_safely.fsm_dealloc_ctx, talloc_object); + else + talloc_free(talloc_object); +} + +/*! specify if FSM instance addresses should be logged or not + * + * By default, the FSM name includes the pointer address of the \ref + * osmo_fsm_inst. This behavior can be disabled (and re-enabled) + * using this function. + * + * \param[in] log_addr Indicate if FSM instance address shall be logged + */ +void osmo_fsm_log_addr(bool log_addr) +{ + fsm_log_addr = log_addr; +} + +/*! Enable or disable logging of timeout values for FSM instance state changes. + * + * By default, state changes are logged by state name only, omitting the timeout. When passing true, each state change + * will also log the T number (or Osmocom-specific X number) and the chosen timeout in seconds. + * osmo_fsm_inst_state_chg_keep_timer() will log remaining timeout in millisecond precision. + * + * The default for this is false to reflect legacy behavior. Since various C tests that verify logging output already + * existed prior to this option, keeping timeout logging off makes sure that they continue to pass. Particularly, + * osmo_fsm_inst_state_chg_keep_timer() may cause non-deterministic logging of remaining timeout values. + * + * For any program that does not explicitly require deterministic logging output, i.e. anything besides regression tests + * involving FSM instances, it is recommended to call osmo_fsm_log_timeouts(true). + * + * \param[in] log_timeouts Pass true to log timeouts on state transitions, false to omit timeouts. + */ +void osmo_fsm_log_timeouts(bool log_timeouts) +{ + fsm_log_timeouts = log_timeouts; +} + +/*! Enable safer way to deallocate cascades of terminating FSM instances. + * + * Note, using osmo_fsm_set_dealloc_ctx() is a more general solution to this same problem. + * Particularly, in a program using osmo_select_main_ctx(), the simplest solution to avoid most use-after-free problems + * from FSM instance deallocation is using osmo_fsm_set_dealloc_ctx(OTC_SELECT). + * + * When enabled, an FSM instance termination detects whether another FSM instance is already terminating, and instead of + * deallocating immediately, collects all terminating FSM instances in a talloc context, to be bulk deallocated once all + * event handling and termination cascades are done. + * + * For example, if an FSM's cleanup() sends an event to some "other" FSM, which in turn causes the FSM's parent to + * deallocate, then the parent would talloc_free() the child's memory, causing a use-after-free. There are infinite + * constellations like this, which all are trivially solved with this feature enabled. + * + * For illustration, see fsm_dealloc_test.c. + * + * When enabled, this feature changes the order of logging, which may break legacy unit test expectations, and changes + * the order of deallocation to after the parent term event is dispatched. + * + * \param[in] term_safely Pass true to switch to safer FSM instance termination behavior. + */ +void osmo_fsm_term_safely(bool term_safely) +{ + fsm_term_safely_enabled = term_safely; +} + +/*! Instead of deallocating FSM instances, move them to the given talloc context. + * + * It is the caller's responsibility to clear this context to actually free the memory of terminated FSM instances. + * Make sure to not talloc_free(ctx) itself before setting a different osmo_fsm_set_dealloc_ctx(). To clear a ctx + * without the need to call osmo_fsm_set_dealloc_ctx() again, rather use talloc_free_children(ctx). + * + * For example, to defer deallocation to the next osmo_select_main_ctx() iteration, set this to OTC_SELECT. + * + * Deferring deallocation is the simplest solution to avoid most use-after-free problems from FSM instance deallocation. + * This is a simpler and more general solution than osmo_fsm_term_safely(). + * + * To disable the feature again, pass NULL as ctx. + * + * Both osmo_fsm_term_safely() and osmo_fsm_set_dealloc_ctx() can be enabled at the same time, which will result in + * first collecting deallocated FSM instances in fsm_term_safely.collect_ctx, and finally reparenting that to the ctx + * passed here. However, in practice, it does not really make sense to enable both at the same time. + * + * \param ctx[in] Instead of talloc_free()int, talloc_steal() all future deallocated osmo_fsm_inst instances to this + * ctx. If NULL, go back to talloc_free() as usual. + */ +void osmo_fsm_set_dealloc_ctx(void *ctx) +{ + fsm_term_safely.fsm_dealloc_ctx = ctx; +} + +/*! talloc_free() the given object immediately, or once ongoing FSM terminations are done. + * + * If an FSM deallocation cascade is ongoing, talloc_steal() the given talloc_object into the talloc context that is + * freed once the cascade is done. If no FSM deallocation cascade is ongoing, or if osmo_fsm_term_safely() is disabled, + * immediately talloc_free the object. + * + * This can be useful if some higher order talloc object, which is the talloc parent for FSM instances or their priv + * objects, is not itself tied to an FSM instance. This function allows safely freeing it without affecting ongoing FSM + * termination cascades. + * + * Once passed to this function, the talloc_object should be considered as already freed. Only FSM instance pre_term() + * and cleanup() functions as well as event handling caused by these may safely assume that it is still valid memory. + * + * The talloc_object should not have multiple parents. + * + * (This function may some day move to public API, which might be redundant if we introduce a select-loop volatile + * context mechanism to defer deallocation instead.) + * + * \param[in] talloc_object Object pointer to free. + */ +static void osmo_fsm_defer_free(void *talloc_object) +{ + if (!fsm_term_safely.depth) { + fsm_free_or_steal(talloc_object); + return; + } + + if (!fsm_term_safely.collect_ctx) { + /* This is actually the first other object / FSM instance besides the root terminating inst. Create the + * ctx to collect this and possibly more objects to free. Avoid talloc parent loops: don't make this ctx + * the child of the root inst or anything like that. */ + fsm_term_safely.collect_ctx = talloc_named_const(NULL, 0, "fsm_term_safely.collect_ctx"); + OSMO_ASSERT(fsm_term_safely.collect_ctx); + } + talloc_steal(fsm_term_safely.collect_ctx, talloc_object); +} + +struct osmo_fsm *osmo_fsm_find_by_name(const char *name) +{ + struct osmo_fsm *fsm; + list_for_each_entry(fsm, &osmo_g_fsms, list) { + if (!strcmp(name, fsm->name)) + return fsm; + } + return NULL; +} + +struct osmo_fsm_inst *osmo_fsm_inst_find_by_name(const struct osmo_fsm *fsm, + const char *name) +{ + struct osmo_fsm_inst *fi; + + if (!name) + return NULL; + + list_for_each_entry(fi, &fsm->instances, list) { + if (!fi->name) + continue; + if (!strcmp(name, fi->name)) + return fi; + } + return NULL; +} + +struct osmo_fsm_inst *osmo_fsm_inst_find_by_id(const struct osmo_fsm *fsm, + const char *id) +{ + struct osmo_fsm_inst *fi; + + list_for_each_entry(fi, &fsm->instances, list) { + if (!strcmp(id, fi->id)) + return fi; + } + return NULL; +} + +/*! register a FSM with the core + * + * A FSM descriptor needs to be registered with the core before any + * instances can be created for it. + * + * \param[in] fsm Descriptor of Finite State Machine to be registered + * \returns 0 on success; negative on error + */ +int osmo_fsm_register(struct osmo_fsm *fsm) +{ + if (!osmo_identifier_valid(fsm->name)) { + LOGP(DLGLOBAL, LOGL_ERROR, "Attempting to register FSM with illegal identifier '%s'\n", fsm->name); + return -EINVAL; + } + if (osmo_fsm_find_by_name(fsm->name)) + return -EEXIST; + if (fsm->event_names == NULL) + LOGP(DLGLOBAL, LOGL_ERROR, "FSM '%s' has no event names! Please fix!\n", fsm->name); + list_add_tail(&fsm->list, &osmo_g_fsms); + INIT_LIST_HEAD(&fsm->instances); + + return 0; +} + +/*! unregister a FSM from the core + * + * Once the FSM descriptor is unregistered, active instances can still + * use it, but no new instances may be created for it. + * + * \param[in] fsm Descriptor of Finite State Machine to be removed + */ +void osmo_fsm_unregister(struct osmo_fsm *fsm) +{ + list_del(&fsm->list); +} + +/* small wrapper function around timer expiration (for logging) */ +static void fsm_tmr_cb(void *data) +{ + struct osmo_fsm_inst *fi = data; + struct osmo_fsm *fsm = fi->fsm; + int32_t T = fi->T; + + LOGPFSM(fi, "Timeout of " OSMO_T_FMT "\n", OSMO_T_FMT_ARGS(fi->T)); + + if (fsm->timer_cb) { + int rc = fsm->timer_cb(fi); + if (rc != 1) + /* We don't actually know whether fi exists anymore. + * Make sure to not access it and return right away. */ + return; + /* The timer_cb told us to terminate, so we can safely assume + * that fi still exists. */ + LOGPFSM(fi, "timer_cb requested termination\n"); + } else + LOGPFSM(fi, "No timer_cb, automatic termination\n"); + + /* if timer_cb returns 1 or there is no timer_cb */ + osmo_fsm_inst_term(fi, OSMO_FSM_TERM_TIMEOUT, &T); +} + +/*! Change id of the FSM instance + * \param[in] fi FSM instance + * \param[in] id new ID + * \returns 0 if the ID was updated, otherwise -EINVAL + */ +int osmo_fsm_inst_update_id(struct osmo_fsm_inst *fi, const char *id) +{ + if (!id) + return osmo_fsm_inst_update_id_f(fi, NULL); + else + return osmo_fsm_inst_update_id_f(fi, "%s", id); +} + +static void update_name(struct osmo_fsm_inst *fi) +{ + if (fi->name) + talloc_free((char*)fi->name); + + if (!fsm_log_addr) { + if (fi->id) + fi->name = talloc_asprintf(fi, "%s(%s)", fi->fsm->name, fi->id); + else + fi->name = talloc_asprintf(fi, "%s", fi->fsm->name); + } else { + if (fi->id) + fi->name = talloc_asprintf(fi, "%s(%s)[%p]", fi->fsm->name, fi->id, fi); + else + fi->name = talloc_asprintf(fi, "%s[%p]", fi->fsm->name, fi); + } +} + +/*! Change id of the FSM instance using a string format. + * \param[in] fi FSM instance. + * \param[in] fmt format string to compose new ID. + * \param[in] ... variable argument list for format string. + * \returns 0 if the ID was updated, otherwise -EINVAL. + */ +int osmo_fsm_inst_update_id_f(struct osmo_fsm_inst *fi, const char *fmt, ...) +{ + char *id = NULL; + + if (fmt) { + va_list ap; + + va_start(ap, fmt); + id = talloc_vasprintf(fi, fmt, ap); + va_end(ap); + + if (!osmo_identifier_valid(id)) { + LOGP(DLGLOBAL, LOGL_ERROR, + "Attempting to set illegal id for FSM instance of type '%s': %s\n", + fi->fsm->name, id); + talloc_free(id); + return -EINVAL; + } + } + + if (fi->id) + talloc_free((char*)fi->id); + fi->id = id; + + update_name(fi); + return 0; +} + +/*! Change id of the FSM instance using a string format, and ensuring a valid id. + * Replace any characters that are not permitted as FSM identifier with replace_with. + * \param[in] fi FSM instance. + * \param[in] replace_with Character to use instead of non-permitted FSM id characters. + * Make sure to choose a legal character, e.g. '-'. + * \param[in] fmt format string to compose new ID. + * \param[in] ... variable argument list for format string. + * \returns 0 if the ID was updated, otherwise -EINVAL. + */ +int osmo_fsm_inst_update_id_f_sanitize(struct osmo_fsm_inst *fi, char replace_with, const char *fmt, ...) +{ + char *id = NULL; + va_list ap; + int rc; + + if (!fmt) + return osmo_fsm_inst_update_id(fi, NULL); + + va_start(ap, fmt); + id = talloc_vasprintf(fi, fmt, ap); + va_end(ap); + + osmo_identifier_sanitize_buf(id, NULL, replace_with); + + rc = osmo_fsm_inst_update_id(fi, id); + talloc_free(id); + return rc; +} + +/*! allocate a new instance of a specified FSM + * \param[in] fsm Descriptor of the FSM + * \param[in] ctx talloc context from which to allocate memory + * \param[in] priv private data reference store in fsm instance + * \param[in] log_level The log level for events of this FSM + * \param[in] id The name/ID of the FSM instance + * \returns newly-allocated, initialized and registered FSM instance + */ +struct osmo_fsm_inst *osmo_fsm_inst_alloc(struct osmo_fsm *fsm, void *ctx, void *priv, + int log_level, const char *id) +{ + struct osmo_fsm_inst *fi = talloc_zero(ctx, struct osmo_fsm_inst); + + fi->fsm = fsm; + fi->priv = priv; + fi->log_level = log_level; + osmo_timer_setup(&fi->timer, fsm_tmr_cb, fi); + + if (osmo_fsm_inst_update_id(fi, id) < 0) { + fsm_free_or_steal(fi); + return NULL; + } + + INIT_LIST_HEAD(&fi->proc.children); + INIT_LIST_HEAD(&fi->proc.child); + list_add(&fi->list, &fsm->instances); + + LOGPFSM(fi, "Allocated\n"); + + return fi; +} + +/*! allocate a new instance of a specified FSM as child of + * other FSM instance + * + * This is like \ref osmo_fsm_inst_alloc but using the parent FSM as + * talloc context, and inheriting the log level of the parent. + * + * \param[in] fsm Descriptor of the to-be-allocated FSM + * \param[in] parent Parent FSM instance + * \param[in] parent_term_event Event to be sent to parent when terminating + * \returns newly-allocated, initialized and registered FSM instance + */ +struct osmo_fsm_inst *osmo_fsm_inst_alloc_child(struct osmo_fsm *fsm, + struct osmo_fsm_inst *parent, + uint32_t parent_term_event) +{ + struct osmo_fsm_inst *fi; + + fi = osmo_fsm_inst_alloc(fsm, parent, NULL, parent->log_level, + parent->id); + if (!fi) { + /* indicate immediate termination to caller */ + osmo_fsm_inst_dispatch(parent, parent_term_event, NULL); + return NULL; + } + + LOGPFSM(fi, "is child of %s\n", osmo_fsm_inst_name(parent)); + + osmo_fsm_inst_change_parent(fi, parent, parent_term_event); + + return fi; +} + +/*! unlink child FSM from its parent FSM. + * \param[in] fi Descriptor of the child FSM to unlink. + * \param[in] ctx New talloc context + * + * Never call this function from the cleanup callback, because at that time + * the child FSMs will already be terminated. If unlinking should be performed + * on FSM termination, use the grace callback instead. */ +void osmo_fsm_inst_unlink_parent(struct osmo_fsm_inst *fi, void *ctx) +{ + if (fi->proc.parent) { + talloc_steal(ctx, fi); + fi->proc.parent = NULL; + fi->proc.parent_term_event = 0; + list_del(&fi->proc.child); + } +} + +/*! change parent instance of an FSM. + * \param[in] fi Descriptor of the to-be-allocated FSM. + * \param[in] new_parent New parent FSM instance. + * \param[in] new_parent_term_event Event to be sent to parent when terminating. + * + * Never call this function from the cleanup callback! + * (see also osmo_fsm_inst_unlink_parent()).*/ +void osmo_fsm_inst_change_parent(struct osmo_fsm_inst *fi, + struct osmo_fsm_inst *new_parent, + uint32_t new_parent_term_event) +{ + /* Make sure a possibly existing old parent is unlinked first + * (new_parent can be NULL) */ + osmo_fsm_inst_unlink_parent(fi, new_parent); + + /* Add new parent */ + if (new_parent) { + fi->proc.parent = new_parent; + fi->proc.parent_term_event = new_parent_term_event; + list_add(&fi->proc.child, &new_parent->proc.children); + } +} + +/*! delete a given instance of a FSM + * \param[in] fi FSM instance to be un-registered and deleted + */ +void osmo_fsm_inst_free(struct osmo_fsm_inst *fi) +{ + osmo_timer_del(&fi->timer); + list_del(&fi->list); + + if (fsm_term_safely.depth) { + /* Another FSM instance has caused this one to free and is still busy with its termination. Don't free + * yet, until the other FSM instance is done. */ + osmo_fsm_defer_free(fi); + /* The root_fi can't go missing really, but to be safe... */ + if (fsm_term_safely.root_fi) + LOGPFSM(fi, "Deferring: will deallocate with %s\n", fsm_term_safely.root_fi->name); + else + LOGPFSM(fi, "Deferring deallocation\n"); + + /* Don't free anything yet. Exit. */ + return; + } + + /* fsm_term_safely.depth == 0. + * - If fsm_term_safely is enabled, this is the original FSM instance that started terminating first. Free this + * and along with it all other collected terminated FSM instances. + * - If fsm_term_safely is disabled, this is just any FSM instance deallocating. */ + + if (fsm_term_safely.collect_ctx) { + /* The fi may be a child of any other FSM instances or objects collected in the collect_ctx. Don't + * deallocate separately to avoid use-after-free errors, put it in there and deallocate all at once. */ + LOGPFSM(fi, "Deallocated, including all deferred deallocations\n"); + osmo_fsm_defer_free(fi); + fsm_free_or_steal(fsm_term_safely.collect_ctx); + fsm_term_safely.collect_ctx = NULL; + } else { + LOGPFSM(fi, "Deallocated\n"); + fsm_free_or_steal(fi); + } + fsm_term_safely.root_fi = NULL; +} + +/*! get human-readable name of FSM event + * \param[in] fsm FSM descriptor of event + * \param[in] event Event integer value + * \returns string rendering of the event + */ +const char *osmo_fsm_event_name(const struct osmo_fsm *fsm, uint32_t event) +{ + static __thread char buf[32]; + if (!fsm->event_names) { + snprintf(buf, sizeof(buf), "%"PRIu32, event); + return buf; + } else + return get_value_string(fsm->event_names, event); +} + +/*! get human-readable name of FSM instance + * \param[in] fi FSM instance + * \returns string rendering of the FSM identity + */ +const char *osmo_fsm_inst_name(const struct osmo_fsm_inst *fi) +{ + if (!fi) + return "NULL"; + + if (fi->name) + return fi->name; + else + return fi->fsm->name; +} + +/*! get human-readable name of FSM state + * \param[in] fsm FSM descriptor + * \param[in] state FSM state number + * \returns string rendering of the FSM state + */ +const char *osmo_fsm_state_name(const struct osmo_fsm *fsm, uint32_t state) +{ + static __thread char buf[32]; + if (state >= fsm->num_states) { + snprintf(buf, sizeof(buf), "unknown %"PRIu32, state); + return buf; + } else + return fsm->states[state].name; +} + +static int state_chg(struct osmo_fsm_inst *fi, uint32_t new_state, + bool keep_timer, unsigned long timeout_ms, int T, + const char *file, int line) +{ + struct osmo_fsm *fsm = fi->fsm; + uint32_t old_state = fi->state; + const struct osmo_fsm_state *st = &fsm->states[fi->state]; + struct timeval remaining; + + if (fi->proc.terminating) { + LOGPFSMSRC(fi, file, line, + "FSM instance already terminating, not changing state to %s\n", + osmo_fsm_state_name(fsm, new_state)); + return -EINVAL; + } + + /* validate if new_state is a valid state */ + if (!(st->out_state_mask & (1 << new_state))) { + LOGPFSMLSRC(fi, LOGL_ERROR, file, line, + "transition to state %s not permitted!\n", + osmo_fsm_state_name(fsm, new_state)); + return -EPERM; + } + + if (!keep_timer) { + /* delete the old timer */ + osmo_timer_del(&fi->timer); + } + + if (st->onleave) + st->onleave(fi, new_state); + + if (fsm_log_timeouts) { + char trailer[64]; + trailer[0] = '\0'; + if (keep_timer && osmo_timer_pending(&fi->timer)) { + /* This should always give us a timeout, but just in case the return value indicates error, omit + * logging the remaining time. */ + if (osmo_timer_remaining(&fi->timer, NULL, &remaining)) + snprintf(trailer, sizeof(trailer), "(keeping " OSMO_T_FMT ")", + OSMO_T_FMT_ARGS(fi->T)); + else + snprintf(trailer, sizeof(trailer), "(keeping " OSMO_T_FMT + ", %ld.%03lds remaining)", OSMO_T_FMT_ARGS(fi->T), + (long) remaining.tv_sec, remaining.tv_usec / 1000); + } else if (timeout_ms) { + if (timeout_ms % 1000 == 0) + /* keep log output legacy compatible to avoid autotest failures */ + snprintf(trailer, sizeof(trailer), "(" OSMO_T_FMT ", %lus)", + OSMO_T_FMT_ARGS(T), timeout_ms/1000); + else + snprintf(trailer, sizeof(trailer), "(" OSMO_T_FMT ", %lums)", + OSMO_T_FMT_ARGS(T), timeout_ms); + } else + snprintf(trailer, sizeof(trailer), "(no timeout)"); + + LOGPFSMSRC(fi, file, line, "State change to %s %s\n", + osmo_fsm_state_name(fsm, new_state), trailer); + } else { + LOGPFSMSRC(fi, file, line, "state_chg to %s\n", + osmo_fsm_state_name(fsm, new_state)); + } + + fi->state = new_state; + st = &fsm->states[new_state]; + + if (!keep_timer + || (keep_timer && !osmo_timer_pending(&fi->timer))) { + fi->T = T; + if (timeout_ms) { + osmo_timer_schedule(&fi->timer, + /* seconds */ (timeout_ms / 1000), + /* microseconds */ (timeout_ms % 1000) * 1000); + } + } + + /* Call 'onenter' last, user might terminate FSM from there */ + if (st->onenter) + st->onenter(fi, old_state); + + return 0; +} + +/*! perform a state change of the given FSM instance + * + * Best invoke via the osmo_fsm_inst_state_chg() macro which logs the source + * file where the state change was effected. Alternatively, you may pass \a + * file as NULL to use the normal file/line indication instead. + * + * All changes to the FSM instance state must be made via an osmo_fsm_inst_state_chg_* + * function. It verifies that the existing state actually permits a + * transition to new_state. + * + * If timeout_secs is 0, stay in the new state indefinitely, without a timeout + * (stop the FSM instance's timer if it was runnning). + * + * If timeout_secs > 0, start or reset the FSM instance's timer with this + * timeout. On expiry, invoke the FSM instance's timer_cb -- if no timer_cb is + * set, an expired timer immediately terminates the FSM instance with + * OSMO_FSM_TERM_TIMEOUT. + * + * The value of T is stored in fi->T and is then available for query in + * timer_cb. If passing timeout_secs == 0, it is recommended to also pass T == + * 0, so that fi->T is reset to 0 when no timeout is invoked. + * + * Positive values for T are considered to be 3GPP spec compliant and appear in + * logging and VTY as "T1234", while negative values are considered to be + * Osmocom specific timers, represented in logging and VTY as "X1234". + * + * See also osmo_tdef_fsm_inst_state_chg() from the osmo_tdef API, which + * provides a unified way to configure and apply GSM style Tnnnn timers to FSM + * state transitions. + * + * \param[in] fi FSM instance whose state is to change + * \param[in] new_state The new state into which we should change + * \param[in] timeout_secs Timeout in seconds (if !=0), maximum-clamped to 2147483647 seconds. + * \param[in] T Timer number, where positive numbers are considered to be 3GPP spec compliant timer numbers and are + * logged as "T1234", while negative numbers are considered Osmocom specific timer numbers logged as + * "X1234". + * \param[in] file Calling source file (from osmo_fsm_inst_state_chg macro) + * \param[in] line Calling source line (from osmo_fsm_inst_state_chg macro) + * \returns 0 on success; negative on error + */ +int _osmo_fsm_inst_state_chg(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_secs, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, false, timeout_secs*1000, T, file, line); +} +int _osmo_fsm_inst_state_chg_ms(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_ms, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, false, timeout_ms, T, file, line); +} + +/*! perform a state change while keeping the current timer running. + * + * This is useful to keep a timeout across several states (without having to round the + * remaining time to seconds). + * + * Best invoke via the osmo_fsm_inst_state_chg_keep_timer() macro which logs the source + * file where the state change was effected. Alternatively, you may pass \a + * file as NULL to use the normal file/line indication instead. + * + * All changes to the FSM instance state must be made via an osmo_fsm_inst_state_chg_* + * function. It verifies that the existing state actually permits a + * transition to new_state. + * + * \param[in] fi FSM instance whose state is to change + * \param[in] new_state The new state into which we should change + * \param[in] file Calling source file (from osmo_fsm_inst_state_chg macro) + * \param[in] line Calling source line (from osmo_fsm_inst_state_chg macro) + * \returns 0 on success; negative on error + */ +int _osmo_fsm_inst_state_chg_keep_timer(struct osmo_fsm_inst *fi, uint32_t new_state, + const char *file, int line) +{ + return state_chg(fi, new_state, true, 0, 0, file, line); +} + +/*! perform a state change while keeping the current timer if running, or starting a timer otherwise. + * + * This is useful to keep a timeout across several states, but to make sure that some timeout is actually running. + * + * Best invoke via the osmo_fsm_inst_state_chg_keep_or_start_timer() macro which logs the source file where the state + * change was effected. Alternatively, you may pass file as NULL to use the normal file/line indication instead. + * + * All changes to the FSM instance state must be made via an osmo_fsm_inst_state_chg_* + * function. It verifies that the existing state actually permits a + * transition to new_state. + * + * \param[in] fi FSM instance whose state is to change + * \param[in] new_state The new state into which we should change + * \param[in] timeout_secs If no timer is running yet, set this timeout in seconds (if !=0), maximum-clamped to + * 2147483647 seconds. + * \param[in] T Timer number, where positive numbers are considered to be 3GPP spec compliant timer numbers and are + * logged as "T1234", while negative numbers are considered Osmocom specific timer numbers logged as + * "X1234". + * \param[in] file Calling source file (from osmo_fsm_inst_state_chg macro) + * \param[in] line Calling source line (from osmo_fsm_inst_state_chg macro) + * \returns 0 on success; negative on error + */ +int _osmo_fsm_inst_state_chg_keep_or_start_timer(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_secs, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, true, timeout_secs*1000, T, file, line); +} +int _osmo_fsm_inst_state_chg_keep_or_start_timer_ms(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_ms, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, true, timeout_ms, T, file, line); +} + + +/*! dispatch an event to an osmocom finite state machine instance + * + * Best invoke via the osmo_fsm_inst_dispatch() macro which logs the source + * file where the event was effected. Alternatively, you may pass \a file as + * NULL to use the normal file/line indication instead. + * + * Any incoming events to \ref osmo_fsm instances must be dispatched to + * them via this function. It verifies, whether the event is permitted + * based on the current state of the FSM. If not, -1 is returned. + * + * \param[in] fi FSM instance + * \param[in] event Event to send to FSM instance + * \param[in] data Data to pass along with the event + * \param[in] file Calling source file (from osmo_fsm_inst_dispatch macro) + * \param[in] line Calling source line (from osmo_fsm_inst_dispatch macro) + * \returns 0 in case of success; negative on error + */ +int _osmo_fsm_inst_dispatch(struct osmo_fsm_inst *fi, uint32_t event, void *data, + const char *file, int line) +{ + struct osmo_fsm *fsm; + const struct osmo_fsm_state *fs; + + if (!fi) { + LOGPSRC(DLGLOBAL, LOGL_ERROR, file, line, + "Trying to dispatch event %"PRIu32" to non-existent" + " FSM instance!\n", event); + osmo_log_backtrace(DLGLOBAL, LOGL_ERROR); + return -ENODEV; + } + + fsm = fi->fsm; + + if (fi->proc.terminating) { + LOGPFSMSRC(fi, file, line, + "FSM instance already terminating, not dispatching event %s\n", + osmo_fsm_event_name(fsm, event)); + return -EINVAL; + } + + OSMO_ASSERT(fi->state < fsm->num_states); + fs = &fi->fsm->states[fi->state]; + + LOGPFSMSRC(fi, file, line, + "Received Event %s\n", osmo_fsm_event_name(fsm, event)); + + if (((1 << event) & fsm->allstate_event_mask) && fsm->allstate_action) { + fsm->allstate_action(fi, event, data); + return 0; + } + + if (!((1 << event) & fs->in_event_mask)) { + LOGPFSMLSRC(fi, LOGL_ERROR, file, line, + "Event %s not permitted\n", + osmo_fsm_event_name(fsm, event)); + return -1; + } + + if (fs->action) + fs->action(fi, event, data); + + return 0; +} + +/*! Terminate FSM instance with given cause + * + * This safely terminates the given FSM instance by first iterating + * over all children and sending them a termination event. Next, it + * calls the FSM descriptors cleanup function (if any), followed by + * releasing any memory associated with the FSM instance. + * + * Finally, the parent FSM instance (if any) is notified using the + * parent termination event configured at time of FSM instance start. + * + * \param[in] fi FSM instance to be terminated + * \param[in] cause Cause / reason for termination + * \param[in] data Opaque event data to be passed with the parent term event + * \param[in] file Calling source file (from osmo_fsm_inst_term macro) + * \param[in] line Calling source line (from osmo_fsm_inst_term macro) + */ +void _osmo_fsm_inst_term(struct osmo_fsm_inst *fi, + enum osmo_fsm_term_cause cause, void *data, + const char *file, int line) +{ + struct osmo_fsm_inst *parent; + uint32_t parent_term_event = fi->proc.parent_term_event; + + if (fi->proc.terminating) { + LOGPFSMSRC(fi, file, line, "Ignoring trigger to terminate: already terminating\n"); + return; + } + fi->proc.terminating = true; + + /* Start termination cascade handling only if the feature is enabled. Also check the current depth: though + * unlikely, theoretically the fsm_term_safely_enabled flag could be toggled in the middle of a cascaded + * termination, so make sure to continue if it already started. */ + if (fsm_term_safely_enabled || fsm_term_safely.depth) { + fsm_term_safely.depth++; + /* root_fi is just for logging, so no need to be extra careful about it. */ + if (!fsm_term_safely.root_fi) + fsm_term_safely.root_fi = fi; + } + + if (fsm_term_safely.depth > 1) { + /* fsm_term_safely is enabled and this is a secondary FSM instance terminated, caused by the root_fi. */ + LOGPFSMSRC(fi, file, line, "Terminating in cascade, depth %d (cause = %s, caused by: %s)\n", + fsm_term_safely.depth, osmo_fsm_term_cause_name(cause), + fsm_term_safely.root_fi ? fsm_term_safely.root_fi->name : "unknown"); + /* The root_fi can't go missing really, but to be safe, log "unknown" in that case. */ + } else { + /* fsm_term_safely is disabled, or this is the root_fi. */ + LOGPFSMSRC(fi, file, line, "Terminating (cause = %s)\n", osmo_fsm_term_cause_name(cause)); + } + + /* graceful exit (optional) */ + if (fi->fsm->pre_term) + fi->fsm->pre_term(fi, cause); + + _osmo_fsm_inst_term_children(fi, OSMO_FSM_TERM_PARENT, NULL, + file, line); + + /* delete ourselves from the parent */ + parent = fi->proc.parent; + if (parent) { + LOGPFSMSRC(fi, file, line, "Removing from parent %s\n", + osmo_fsm_inst_name(parent)); + list_del(&fi->proc.child); + } + + /* call destructor / clean-up function */ + if (fi->fsm->cleanup) + fi->fsm->cleanup(fi, cause); + + /* Fetch parent again in case it has changed. */ + parent = fi->proc.parent; + + /* Legacy behavior if fsm_term_safely is disabled: free before dispatching parent event. (If fsm_term_safely is + * enabled, depth will *always* be > 0 here.) Pivot on depth instead of the enabled flag in case the enabled + * flag is toggled in the middle of an FSM term. */ + if (!fsm_term_safely.depth) { + LOGPFSMSRC(fi, file, line, "Freeing instance\n"); + osmo_fsm_inst_free(fi); + } + + /* indicate our termination to the parent */ + if (parent && cause != OSMO_FSM_TERM_PARENT) + _osmo_fsm_inst_dispatch(parent, parent_term_event, data, + file, line); + + /* Newer, safe deallocation: free only after the parent_term_event was dispatched, to catch all termination + * cascades, and free all FSM instances at once. (If fsm_term_safely is enabled, depth will *always* be > 0 + * here.) osmo_fsm_inst_free() will do the defer magic depending on the fsm_term_safely.depth. */ + if (fsm_term_safely.depth) { + fsm_term_safely.depth--; + osmo_fsm_inst_free(fi); + } +} + +/*! Terminate all child FSM instances of an FSM instance. + * + * Iterate over all children and send them a termination event, with the given + * cause. Pass OSMO_FSM_TERM_PARENT to avoid dispatching events from the + * terminated child FSMs. + * + * \param[in] fi FSM instance that should be cleared of child FSMs + * \param[in] cause Cause / reason for termination (OSMO_FSM_TERM_PARENT) + * \param[in] data Opaque event data to be passed with the parent term events + * \param[in] file Calling source file (from osmo_fsm_inst_term_children macro) + * \param[in] line Calling source line (from osmo_fsm_inst_term_children macro) + */ +void _osmo_fsm_inst_term_children(struct osmo_fsm_inst *fi, + enum osmo_fsm_term_cause cause, + void *data, + const char *file, int line) +{ + struct osmo_fsm_inst *first_child, *last_seen_first_child; + + /* iterate over all children, starting from the beginning every time: + * terminating an FSM may emit events that cause other FSMs to also + * terminate and remove themselves from this list. */ + last_seen_first_child = NULL; + while (!list_empty(&fi->proc.children)) { + first_child = list_entry(fi->proc.children.next, + typeof(*first_child), + proc.child); + + /* paranoia: do not loop forever */ + if (first_child == last_seen_first_child) { + LOGPFSMLSRC(fi, LOGL_ERROR, file, line, + "Internal error while terminating child" + " FSMs: a child FSM is stuck\n"); + break; + } + last_seen_first_child = first_child; + + /* terminate child */ + _osmo_fsm_inst_term(first_child, cause, data, + file, line); + } +} + +/*! Broadcast an event to all the FSMs children. + * + * Iterate over all children and send them the specified event. + * + * \param[in] fi FSM instance of the parent + * \param[in] event Event to send to children of FSM instance + * \param[in] data Data to pass along with the event + * \param[in] file Calling source file (from osmo_fsm_inst_dispatch macro) + * \param[in] line Calling source line (from osmo_fsm_inst_dispatch macro) + */ +void _osmo_fsm_inst_broadcast_children(struct osmo_fsm_inst *fi, + uint32_t event, void *data, + const char *file, int line) +{ + struct osmo_fsm_inst *child, *tmp; + list_for_each_entry_safe(child, tmp, &fi->proc.children, proc.child) { + _osmo_fsm_inst_dispatch(child, event, data, file, line); + } +} + +const struct value_string osmo_fsm_term_cause_names[] = { + OSMO_VALUE_STRING(OSMO_FSM_TERM_PARENT), + OSMO_VALUE_STRING(OSMO_FSM_TERM_REQUEST), + OSMO_VALUE_STRING(OSMO_FSM_TERM_REGULAR), + OSMO_VALUE_STRING(OSMO_FSM_TERM_ERROR), + OSMO_VALUE_STRING(OSMO_FSM_TERM_TIMEOUT), + { 0, NULL } +}; + +/*! @} */ diff --git a/uqmid/osmocom/fsm.h b/uqmid/osmocom/fsm.h new file mode 100644 index 0000000..a4ca562 --- /dev/null +++ b/uqmid/osmocom/fsm.h @@ -0,0 +1,340 @@ +/*! \file fsm.h + * Finite State Machine + */ + +#pragma once + +#include +#include + +// #include "linuxlist.h" +#include "timer.h" +#include "utils.h" +#include "logging.h" + +/*! \defgroup fsm Finite State Machine abstraction + * @{ + * \file fsm.h */ + +struct osmo_fsm_inst; + +enum osmo_fsm_term_cause { + /*! terminate because parent terminated */ + OSMO_FSM_TERM_PARENT, + /*! terminate on explicit user request */ + OSMO_FSM_TERM_REQUEST, + /*! regular termination of process */ + OSMO_FSM_TERM_REGULAR, + /*! erroneous termination of process */ + OSMO_FSM_TERM_ERROR, + /*! termination due to time-out */ + OSMO_FSM_TERM_TIMEOUT, +}; + +extern const struct value_string osmo_fsm_term_cause_names[]; +static inline const char *osmo_fsm_term_cause_name(enum osmo_fsm_term_cause cause) +{ + return get_value_string(osmo_fsm_term_cause_names, cause); +} + + +/*! description of a rule in the FSM */ +struct osmo_fsm_state { + /*! bit-mask of permitted input events for this state */ + uint32_t in_event_mask; + /*! bit-mask to which other states this state may transiton */ + uint32_t out_state_mask; + /*! human-readable name of this state */ + const char *name; + /*! function to be called for events arriving in this state */ + void (*action)(struct osmo_fsm_inst *fi, uint32_t event, void *data); + /*! function to be called just after entering the state */ + void (*onenter)(struct osmo_fsm_inst *fi, uint32_t prev_state); + /*! function to be called just before leaving the state */ + void (*onleave)(struct osmo_fsm_inst *fi, uint32_t next_state); +}; + +/*! a description of an osmocom finite state machine */ +struct osmo_fsm { + /*! global list */ + struct list_head list; + /*! list of instances of this FSM */ + struct list_head instances; + /*! human readable name */ + const char *name; + /*! table of state transition rules */ + const struct osmo_fsm_state *states; + /*! number of entries in \ref states */ + unsigned int num_states; + /*! bit-mask of events permitted in all states */ + uint32_t allstate_event_mask; + /*! function pointer to be called for allstate events */ + void (*allstate_action)(struct osmo_fsm_inst *fi, uint32_t event, void *data); + /*! clean-up function, called during termination */ + void (*cleanup)(struct osmo_fsm_inst *fi, enum osmo_fsm_term_cause cause); + /*! timer call-back for states with time-out. + * \returns 1 to request termination, 0 to keep running. */ + int (*timer_cb)(struct osmo_fsm_inst *fi); + /*! logging sub-system for this FSM */ + int log_subsys; + /*! human-readable names of events */ + const struct value_string *event_names; + /*! graceful exit function, called at the beginning of termination */ + void (*pre_term)(struct osmo_fsm_inst *fi, enum osmo_fsm_term_cause cause); +}; + +/*! a single instanceof an osmocom finite state machine */ +struct osmo_fsm_inst { + /*! member in the fsm->instances list */ + struct list_head list; + /*! back-pointer to the FSM of which we are an instance */ + struct osmo_fsm *fsm; + /*! human readable identifier */ + const char *id; + /*! human readable fully-qualified name */ + const char *name; + /*! some private data of this instance */ + void *priv; + /*! logging level for this FSM */ + int log_level; + /*! current state of the FSM */ + uint32_t state; + + /*! timer number for states with time-out */ + int T; + /*! timer back-end for states with time-out */ + struct osmo_timer_list timer; + + /*! support for fsm-based procedures */ + struct { + /*! the parent FSM that has created us */ + struct osmo_fsm_inst *parent; + /*! the event we should send upon termination */ + uint32_t parent_term_event; + /*! a list of children processes */ + struct list_head children; + /*! \ref list_head linked to parent->proc.children */ + struct list_head child; + /*! Indicator whether osmo_fsm_inst_term() was already invoked on this instance. */ + bool terminating; + } proc; +}; + +void osmo_fsm_log_addr(bool log_addr); +void osmo_fsm_log_timeouts(bool log_timeouts); +void osmo_fsm_term_safely(bool term_safely); +void osmo_fsm_set_dealloc_ctx(void *ctx); + +/*! Log using FSM instance's context, on explicit logging subsystem and level. + * \param fi An osmo_fsm_inst. + * \param subsys A logging subsystem, e.g. DLGLOBAL. + * \param level A logging level, e.g. LOGL_INFO. + * \param fmt printf-like format string. + * \param args Format string arguments. + */ +#define LOGPFSMSL(fi, subsys, level, fmt, args...) \ + LOGPFSMSLSRC(fi, subsys, level, __FILE__, __LINE__, fmt, ## args) + +/*! Log using FSM instance's context, on explicit logging subsystem and level, + * and passing explicit source file and line information. + * \param fi An osmo_fsm_inst. + * \param subsys A logging subsystem, e.g. DLGLOBAL. + * \param level A logging level, e.g. LOGL_INFO. + * \param caller_file A string constant containing a source file path, like __FILE__. + * \param caller_line A number constant containing a source file line, like __LINE__. + * \param fmt printf-like format string. + * \param args Format string arguments. + */ +#define LOGPFSMSLSRC(fi, subsys, level, caller_file, caller_line, fmt, args...) \ + LOGPSRC(subsys, level, \ + caller_file, caller_line, \ + "%s{%s}: " fmt, \ + osmo_fsm_inst_name(fi), \ + (fi) ? osmo_fsm_state_name((fi)->fsm, (fi)->state) : "fi=NULL", ## args) + + +/*! Log using FSM instance's context, on explicit logging level. + * \param fi An osmo_fsm_inst. + * \param level A logging level, e.g. LOGL_INFO. + * \param fmt printf-like format string. + * \param args Format string arguments. + */ +#define LOGPFSML(fi, level, fmt, args...) \ + LOGPFSMLSRC(fi, level, __FILE__, __LINE__, fmt, ## args) + +/*! Log using FSM instance's context, on explicit logging level, and with explicit source file and line info. + * The log subsystem to log on is obtained from the underlying FSM definition. + * \param fi An osmo_fsm_inst. + * \param level A logging level, e.g. LOGL_INFO. + * \param caller_file A string constant containing a source file path, like __FILE__. + * \param caller_line A number constant containing a source file line, like __LINE__. + * \param fmt printf-like format string. + * \param args Format string arguments. + */ +#define LOGPFSMLSRC(fi, level, caller_file, caller_line, fmt, args...) \ + LOGPFSMSLSRC(fi, (fi) ? (fi)->fsm->log_subsys : DLGLOBAL, level, \ + caller_file, caller_line, fmt, ## args) + +/*! Log using FSM instance's context. + * The log level to log on is obtained from the FSM instance. + * The log subsystem to log on is obtained from the underlying FSM definition. + * \param fi An osmo_fsm_inst. + * \param fmt printf-like format string. + * \param args Format string arguments. + */ +#define LOGPFSM(fi, fmt, args...) \ + LOGPFSML(fi, (fi) ? (fi)->log_level : LOGL_ERROR, fmt, ## args) + +/*! Log using FSM instance's context, with explicit source file and line info. + * The log level to log on is obtained from the FSM instance. + * The log subsystem to log on is obtained from the underlying FSM definition. + * \param fi An osmo_fsm_inst. + * \param caller_file A string constant containing a source file path, like __FILE__. + * \param caller_line A number constant containing a source file line, like __LINE__. + * \param fmt printf-like format string. + * \param args Format string arguments. + */ +#define LOGPFSMSRC(fi, caller_file, caller_line, fmt, args...) \ + LOGPFSMLSRC(fi, (fi) ? (fi)->log_level : LOGL_ERROR, \ + caller_file, caller_line, \ + fmt, ## args) + +#define OSMO_T_FMT "%c%u" +#define OSMO_T_FMT_ARGS(T) ((T) >= 0 ? 'T' : 'X'), ((T) >= 0 ? T : -T) + +int osmo_fsm_register(struct osmo_fsm *fsm); +void osmo_fsm_unregister(struct osmo_fsm *fsm); +struct osmo_fsm *osmo_fsm_find_by_name(const char *name); +struct osmo_fsm_inst *osmo_fsm_inst_find_by_name(const struct osmo_fsm *fsm, + const char *name); +struct osmo_fsm_inst *osmo_fsm_inst_find_by_id(const struct osmo_fsm *fsm, + const char *id); +struct osmo_fsm_inst *osmo_fsm_inst_alloc(struct osmo_fsm *fsm, void *ctx, void *priv, + int log_level, const char *id); +struct osmo_fsm_inst *osmo_fsm_inst_alloc_child(struct osmo_fsm *fsm, + struct osmo_fsm_inst *parent, + uint32_t parent_term_event); +void osmo_fsm_inst_unlink_parent(struct osmo_fsm_inst *fi, void *ctx); +void osmo_fsm_inst_change_parent(struct osmo_fsm_inst *fi, + struct osmo_fsm_inst *new_parent, + uint32_t new_parent_term_event); +void osmo_fsm_inst_free(struct osmo_fsm_inst *fi); + +int osmo_fsm_inst_update_id(struct osmo_fsm_inst *fi, const char *id); +int osmo_fsm_inst_update_id_f(struct osmo_fsm_inst *fi, const char *fmt, ...); +int osmo_fsm_inst_update_id_f_sanitize(struct osmo_fsm_inst *fi, char replace_with, const char *fmt, ...); + +const char *osmo_fsm_event_name(const struct osmo_fsm *fsm, uint32_t event); +const char *osmo_fsm_inst_name(const struct osmo_fsm_inst *fi); +const char *osmo_fsm_state_name(const struct osmo_fsm *fsm, uint32_t state); + +/*! return the name of the state the FSM instance is currently in. */ +static inline const char *osmo_fsm_inst_state_name(struct osmo_fsm_inst *fi) +{ return fi ? osmo_fsm_state_name(fi->fsm, fi->state) : "NULL"; } + +/*! perform a state change of the given FSM instance + * + * This is a macro that calls _osmo_fsm_inst_state_chg() with the given + * parameters as well as the caller's source file and line number for logging + * purposes. See there for documentation. + */ +#define osmo_fsm_inst_state_chg(fi, new_state, timeout_secs, T) \ + _osmo_fsm_inst_state_chg(fi, new_state, timeout_secs, T, \ + __FILE__, __LINE__) +int _osmo_fsm_inst_state_chg(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_secs, int T, + const char *file, int line); + +#define osmo_fsm_inst_state_chg_ms(fi, new_state, timeout_ms, T) \ + _osmo_fsm_inst_state_chg_ms(fi, new_state, timeout_ms, T, \ + __FILE__, __LINE__) +int _osmo_fsm_inst_state_chg_ms(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_ms, int T, + const char *file, int line); + +/*! perform a state change while keeping the current timer running. + * + * This is useful to keep a timeout across several states (without having to round the + * remaining time to seconds). + * + * This is a macro that calls _osmo_fsm_inst_state_chg_keep_timer() with the given + * parameters as well as the caller's source file and line number for logging + * purposes. See there for documentation. + */ +#define osmo_fsm_inst_state_chg_keep_timer(fi, new_state) \ + _osmo_fsm_inst_state_chg_keep_timer(fi, new_state, \ + __FILE__, __LINE__) +int _osmo_fsm_inst_state_chg_keep_timer(struct osmo_fsm_inst *fi, uint32_t new_state, + const char *file, int line); + +/*! perform a state change while keeping the current timer if running, or starting a timer otherwise. + * + * This is useful to keep a timeout across several states, but to make sure that some timeout is actually running. + * + * This is a macro that calls _osmo_fsm_inst_state_chg_keep_or_start_timer() with the given + * parameters as well as the caller's source file and line number for logging + * purposes. See there for documentation. + */ +#define osmo_fsm_inst_state_chg_keep_or_start_timer(fi, new_state, timeout_secs, T) \ + _osmo_fsm_inst_state_chg_keep_or_start_timer(fi, new_state, timeout_secs, T, \ + __FILE__, __LINE__) +int _osmo_fsm_inst_state_chg_keep_or_start_timer(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_secs, int T, + const char *file, int line); + +#define osmo_fsm_inst_state_chg_keep_or_start_timer_ms(fi, new_state, timeout_ms, T) \ + _osmo_fsm_inst_state_chg_keep_or_start_timer_ms(fi, new_state, timeout_ms, T, \ + __FILE__, __LINE__) +int _osmo_fsm_inst_state_chg_keep_or_start_timer_ms(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_ms, int T, + const char *file, int line); + + +/*! dispatch an event to an osmocom finite state machine instance + * + * This is a macro that calls _osmo_fsm_inst_dispatch() with the given + * parameters as well as the caller's source file and line number for logging + * purposes. See there for documentation. + */ +#define osmo_fsm_inst_dispatch(fi, event, data) \ + _osmo_fsm_inst_dispatch(fi, event, data, __FILE__, __LINE__) +int _osmo_fsm_inst_dispatch(struct osmo_fsm_inst *fi, uint32_t event, void *data, + const char *file, int line); + +/*! Terminate FSM instance with given cause + * + * This is a macro that calls _osmo_fsm_inst_term() with the given parameters + * as well as the caller's source file and line number for logging purposes. + * See there for documentation. + */ +#define osmo_fsm_inst_term(fi, cause, data) \ + _osmo_fsm_inst_term(fi, cause, data, __FILE__, __LINE__) +void _osmo_fsm_inst_term(struct osmo_fsm_inst *fi, + enum osmo_fsm_term_cause cause, void *data, + const char *file, int line); + +/*! Terminate all child FSM instances of an FSM instance. + * + * This is a macro that calls _osmo_fsm_inst_term_children() with the given + * parameters as well as the caller's source file and line number for logging + * purposes. See there for documentation. + */ +#define osmo_fsm_inst_term_children(fi, cause, data) \ + _osmo_fsm_inst_term_children(fi, cause, data, __FILE__, __LINE__) +void _osmo_fsm_inst_term_children(struct osmo_fsm_inst *fi, + enum osmo_fsm_term_cause cause, + void *data, + const char *file, int line); + +/*! dispatch an event to all children of an osmocom finite state machine instance + * + * This is a macro that calls _osmo_fsm_inst_broadcast_children() with the given + * parameters as well as the caller's source file and line number for logging + * purposes. See there for documentation. + */ +#define osmo_fsm_inst_broadcast_children(fi, cause, data) \ + _osmo_fsm_inst_broadcast_children(fi, cause, data, __FILE__, __LINE__) +void _osmo_fsm_inst_broadcast_children(struct osmo_fsm_inst *fi, uint32_t event, + void *data, const char *file, int line); + +/*! @} */ diff --git a/uqmid/osmocom/linuxlist.h b/uqmid/osmocom/linuxlist.h new file mode 100644 index 0000000..2fc3fa7 --- /dev/null +++ b/uqmid/osmocom/linuxlist.h @@ -0,0 +1,650 @@ +/*! \file linuxlist.h + * + * Simple doubly linked list implementation. + * + * Some of the internal functions ("__xxx") are useful when + * manipulating whole llists rather than single entries, as + * sometimes we already know the next/prev entries and we can + * generate better code by using them directly rather than + * using the generic single-entry routines. + */ + +#pragma once + +/*! \defgroup linuxlist Simple doubly linked list implementation + * @{ + * \file linuxlist.h */ + +#include +#include + +#ifndef inline +#define inline __inline__ +#endif + +static inline void prefetch(const void *x) {;} + +/*! Cast a member of a structure out to the containing structure. + * \param[in] ptr the pointer to the member. + * \param[in] type the type of the container struct this is embedded in. + * \param[in] member the name of the member within the struct. + */ +#define container_of(ptr, type, member) ({ \ + const typeof( ((type *)0)->member ) *__mptr = (ptr); \ + (type *)( (char *)__mptr - offsetof(type, member) );}) + + +/*! + * These are non-NULL pointers that will result in page faults + * under normal circumstances, used to verify that nobody uses + * non-initialized llist entries. + */ +#define LLIST_POISON1 ((void *) 0x00100100) +#define LLIST_POISON2 ((void *) 0x00200200) + +/*! (double) linked list header structure */ +struct llist_head { + /*! Pointer to next and previous item */ + struct llist_head *next, *prev; +}; + +/*! Define a new llist_head pointing to a given llist_head. + * \param[in] name another llist_head to be pointed. + */ +#define LLIST_HEAD_INIT(name) { &(name), &(name) } + +/*! Define a statically-initialized variable of type llist_head. + * \param[in] name variable (symbol) name. + */ +#define LLIST_HEAD(name) \ + struct llist_head name = LLIST_HEAD_INIT(name) + +/*! Initialize a llist_head to point back to itself. + * \param[in] ptr llist_head to be initialized. + */ +#define INIT_LLIST_HEAD(ptr) do { \ + (ptr)->next = (ptr); (ptr)->prev = (ptr); \ +} while (0) + +/* + * Insert a new entry between two known consecutive entries. + * + * This is only for internal llist manipulation where we know + * the prev/next entries already! + */ +static inline void __llist_add(struct llist_head *_new, + struct llist_head *prev, + struct llist_head *next) +{ + next->prev = _new; + _new->next = next; + _new->prev = prev; + prev->next = _new; +} + +/*! Add a new entry into a linked list (at head). + * \param _new the entry to be added. + * \param head llist_head to prepend the element to. + * + * Insert a new entry after the specified head. + * This is good for implementing stacks. + */ +static inline void llist_add(struct llist_head *_new, struct llist_head *head) +{ + __llist_add(_new, head, head->next); +} + +/*! Add a new entry into a linked list (at tail). + * \param _new the entry to be added. + * \param head llist_head to append the element to. + * + * Insert a new entry before the specified head. + * This is useful for implementing queues. + */ +static inline void llist_add_tail(struct llist_head *_new, struct llist_head *head) +{ + __llist_add(_new, head->prev, head); +} + +/* + * Delete a llist entry by making the prev/next entries + * point to each other. + * + * This is only for internal llist manipulation where we know + * the prev/next entries already! + */ +static inline void __llist_del(struct llist_head * prev, struct llist_head * next) +{ + next->prev = prev; + prev->next = next; +} + +/*! Delete a single entry from a linked list. + * \param entry the element to delete. + * + * Note: llist_empty on entry does not return true after this, the entry is + * in an undefined state. + */ +static inline void llist_del(struct llist_head *entry) +{ + __llist_del(entry->prev, entry->next); + entry->next = (struct llist_head *)LLIST_POISON1; + entry->prev = (struct llist_head *)LLIST_POISON2; +} + +/*! Delete a single entry from a linked list and reinitialize it. + * \param entry the element to delete and reinitialize. + */ +static inline void llist_del_init(struct llist_head *entry) +{ + __llist_del(entry->prev, entry->next); + INIT_LLIST_HEAD(entry); +} + +/*! Delete from one llist and add as another's head. + * \param llist the entry to move. + * \param head the head that will precede our entry. + */ +static inline void llist_move(struct llist_head *llist, struct llist_head *head) +{ + __llist_del(llist->prev, llist->next); + llist_add(llist, head); +} + +/*! Delete from one llist and add as another's tail. + * \param llist the entry to move. + * \param head the head that will follow our entry. + */ +static inline void llist_move_tail(struct llist_head *llist, + struct llist_head *head) +{ + __llist_del(llist->prev, llist->next); + llist_add_tail(llist, head); +} + +/*! Test whether a linked list is empty. + * \param[in] head the llist to test. + * \returns 1 if the list is empty, 0 otherwise. + */ +static inline int llist_empty(const struct llist_head *head) +{ + return head->next == head; +} + +static inline void __llist_splice(struct llist_head *llist, + struct llist_head *head) +{ + struct llist_head *first = llist->next; + struct llist_head *last = llist->prev; + struct llist_head *at = head->next; + + first->prev = head; + head->next = first; + + last->next = at; + at->prev = last; +} + +/*! Join two linked lists. + * \param llist the new linked list to add. + * \param head the place to add llist within the other list. + */ +static inline void llist_splice(struct llist_head *llist, struct llist_head *head) +{ + if (!llist_empty(llist)) + __llist_splice(llist, head); +} + +/*! Join two llists and reinitialise the emptied llist. + * \param llist the new linked list to add. + * \param head the place to add it within the first llist. + * + * The llist is reinitialised. + */ +static inline void llist_splice_init(struct llist_head *llist, + struct llist_head *head) +{ + if (!llist_empty(llist)) { + __llist_splice(llist, head); + INIT_LLIST_HEAD(llist); + } +} + +/*! Get the struct containing this list entry. + * \param ptr the llist_head pointer. + * \param type the type of the struct this is embedded in. + * \param member the name of the llist_head within the struct. + */ +#define llist_entry(ptr, type, member) \ + container_of(ptr, type, member) + +/*! Get the first element from a linked list. + * \param ptr the list head to take the element from. + * \param type the type of the struct this is embedded in. + * \param member the name of the list_head within the struct. + * + * Note, that list is expected to be not empty. + */ +#define llist_first_entry(ptr, type, member) \ + llist_entry((ptr)->next, type, member) + +/*! Get the last element from a list. + * \param ptr the list head to take the element from. + * \param type the type of the struct this is embedded in. + * \param member the name of the llist_head within the struct. + * + * Note, that list is expected to be not empty. + */ +#define llist_last_entry(ptr, type, member) \ + llist_entry((ptr)->prev, type, member) + +/*! Return the last element of the list. + * \param head the llist head of the list. + * \returns last element of the list, head if the list is empty. + */ +#define llist_last(head) (head)->prev + +/*! Get the first element from a list, or NULL. + * \param ptr the list head to take the element from. + * \param type the type of the struct this is embedded in. + * \param member the name of the list_head within the struct. + * + * Note that if the list is empty, it returns NULL. + */ +#define llist_first_entry_or_null(ptr, type, member) \ + (!llist_empty(ptr) ? llist_first_entry(ptr, type, member) : NULL) + +/*! Iterate over a linked list. + * \param pos the llist_head to use as a loop counter. + * \param head the head of the list over which to iterate. + */ +#define llist_for_each(pos, head) \ + for (pos = (head)->next, prefetch(pos->next); pos != (head); \ + pos = pos->next, prefetch(pos->next)) + +/*! Iterate over a linked list (no prefetch). + * \param pos the llist_head to use as a loop counter. + * \param head the head of the list over which to iterate. + * + * This variant differs from llist_for_each() in that it's the + * simplest possible llist iteration code, no prefetching is done. + * Use this for code that knows the llist to be very short (empty + * or 1 entry) most of the time. + */ +#define __llist_for_each(pos, head) \ + for (pos = (head)->next; pos != (head); pos = pos->next) + +/*! Iterate over a linked list backwards. + * \param pos the llist_head to use as a loop counter. + * \param head the head of the list over which to iterate. + */ +#define llist_for_each_prev(pos, head) \ + for (pos = (head)->prev, prefetch(pos->prev); pos != (head); \ + pos = pos->prev, prefetch(pos->prev)) + +/*! Iterate over a linked list, safe against removal of llist entry. + * \param pos the llist_head to use as a loop counter. + * \param n another llist_head to use as temporary storage. + * \param head the head of the list over which to iterate. + */ +#define llist_for_each_safe(pos, n, head) \ + for (pos = (head)->next, n = pos->next; pos != (head); \ + pos = n, n = pos->next) + +/*! Iterate over a linked list of a given type. + * \param pos the 'type *' to use as a loop counter. + * \param head the head of the list over which to iterate. + * \param member the name of the llist_head within the struct pos. + */ +#define llist_for_each_entry(pos, head, member) \ + for (pos = llist_entry((head)->next, typeof(*pos), member), \ + prefetch(pos->member.next); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.next, typeof(*pos), member), \ + prefetch(pos->member.next)) + +/*! Iterate backwards over a linked list of a given type. + * \param pos the 'type *' to use as a loop counter. + * \param head the head of the list over which to iterate. + * \param member the name of the llist_head within the struct pos. + */ +#define llist_for_each_entry_reverse(pos, head, member) \ + for (pos = llist_entry((head)->prev, typeof(*pos), member), \ + prefetch(pos->member.prev); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.prev, typeof(*pos), member), \ + prefetch(pos->member.prev)) + +/*! Iterate over a linked list of a given type, + * continuing after an existing point. + * \param pos the 'type *' to use as a loop counter. + * \param head the head of the list over which to iterate. + * \param member the name of the llist_head within the struct pos. + */ +#define llist_for_each_entry_continue(pos, head, member) \ + for (pos = llist_entry(pos->member.next, typeof(*pos), member), \ + prefetch(pos->member.next); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.next, typeof(*pos), member), \ + prefetch(pos->member.next)) + +/*! Iterate over llist of given type, safe against removal of llist entry. + * \param pos the 'type *' to use as a loop counter. + * \param n another 'type *' to use as temporary storage. + * \param head the head of the list over which to iterate. + * \param member the name of the llist_head within the struct pos. + */ +#define llist_for_each_entry_safe(pos, n, head, member) \ + for (pos = llist_entry((head)->next, typeof(*pos), member), \ + n = llist_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = llist_entry(n->member.next, typeof(*n), member)) + +/*! Iterate over an rcu-protected llist. + * \param pos the llist_head to use as a loop counter. + * \param head the head of the list over which to iterate. + */ +#define llist_for_each_rcu(pos, head) \ + for (pos = (head)->next, prefetch(pos->next); pos != (head); \ + pos = pos->next, ({ smp_read_barrier_depends(); 0;}), prefetch(pos->next)) + +#define __llist_for_each_rcu(pos, head) \ + for (pos = (head)->next; pos != (head); \ + pos = pos->next, ({ smp_read_barrier_depends(); 0;})) + +/*! Iterate over an rcu-protected llist, safe against removal of llist entry. + * \param pos the llist_head to use as a loop counter. + * \param n another llist_head to use as temporary storage. + * \param head the head of the list over which to iterate. + */ +#define llist_for_each_safe_rcu(pos, n, head) \ + for (pos = (head)->next, n = pos->next; pos != (head); \ + pos = n, ({ smp_read_barrier_depends(); 0;}), n = pos->next) + +/*! Iterate over an rcu-protected llist of a given type. + * \param pos the 'type *' to use as a loop counter. + * \param head the head of the list over which to iterate. + * \param member the name of the llist_struct within the struct. + */ +#define llist_for_each_entry_rcu(pos, head, member) \ + for (pos = llist_entry((head)->next, typeof(*pos), member), \ + prefetch(pos->member.next); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.next, typeof(*pos), member), \ + ({ smp_read_barrier_depends(); 0;}), \ + prefetch(pos->member.next)) + + +/*! Iterate over an rcu-protected llist, continuing after existing point. + * \param pos the llist_head to use as a loop counter. + * \param head the head of the list over which to iterate. + */ +#define llist_for_each_continue_rcu(pos, head) \ + for ((pos) = (pos)->next, prefetch((pos)->next); (pos) != (head); \ + (pos) = (pos)->next, ({ smp_read_barrier_depends(); 0;}), prefetch((pos)->next)) + +/*! Count number of llist items by iterating. + * \param head the llist head to count items of. + * \returns Number of items. + * + * This function is not efficient, mostly useful for small lists and non time + * critical cases like unit tests. + */ +static inline unsigned int llist_count(const struct llist_head *head) +{ + struct llist_head *entry; + unsigned int i = 0; + llist_for_each(entry, head) + i++; + return i; +} + + + +/*! Double linked lists with a single pointer list head. + * Mostly useful for hash tables where the two pointer list head is + * too wasteful. + * You lose the ability to access the tail in O(1). + */ + +struct hlist_head { + struct hlist_node *first; +}; + +struct hlist_node { + struct hlist_node *next, **pprev; +}; + +#define HLIST_HEAD_INIT { .first = NULL } +#define HLIST_HEAD(name) struct hlist_head name = { .first = NULL } +#define INIT_HLIST_HEAD(ptr) ((ptr)->first = NULL) +static inline void INIT_HLIST_NODE(struct hlist_node *h) +{ + h->next = NULL; + h->pprev = NULL; +} + +#define READ_ONCE(x) x +#define WRITE_ONCE(a, b) a = b + +/*! Has node been removed from list and reinitialized?. + * \param[in] h: Node to be checked + * \return 1 if node is unhashed; 0 if not + * + * Not that not all removal functions will leave a node in unhashed + * state. For example, hlist_nulls_del_init_rcu() does leave the + * node in unhashed state, but hlist_nulls_del() does not. + */ +static inline int hlist_unhashed(const struct hlist_node *h) +{ + return !h->pprev; +} + +/*! Version of hlist_unhashed for lockless use. + * \param[in] n Node to be checked + * \return 1 if node is unhashed; 0 if not + * + * This variant of hlist_unhashed() must be used in lockless contexts + * to avoid potential load-tearing. The READ_ONCE() is paired with the + * various WRITE_ONCE() in hlist helpers that are defined below. + */ +static inline int hlist_unhashed_lockless(const struct hlist_node *h) +{ + return !READ_ONCE(h->pprev); +} + +/*!Is the specified hlist_head structure an empty hlist?. + * \param[in] h Structure to check. + * \return 1 if hlist is empty; 0 if not + */ +static inline int hlist_empty(const struct hlist_head *h) +{ + return !READ_ONCE(h->first); +} + +static inline void __hlist_del(struct hlist_node *n) +{ + struct hlist_node *next = n->next; + struct hlist_node **pprev = n->pprev; + + WRITE_ONCE(*pprev, next); + if (next) + WRITE_ONCE(next->pprev, pprev); +} + +/*! Delete the specified hlist_node from its list. + * \param[in] n: Node to delete. + * + * Note that this function leaves the node in hashed state. Use + * hlist_del_init() or similar instead to unhash @n. + */ +static inline void hlist_del(struct hlist_node *n) +{ + __hlist_del(n); + n->next = (struct hlist_node *)LLIST_POISON1; + n->pprev = (struct hlist_node **)LLIST_POISON2; +} + +/*! Delete the specified hlist_node from its list and initialize. + * \param[in] n Node to delete. + * + * Note that this function leaves the node in unhashed state. + */ +static inline void hlist_del_init(struct hlist_node *n) +{ + if (!hlist_unhashed(n)) { + __hlist_del(n); + INIT_HLIST_NODE(n); + } +} + +/*! add a new entry at the beginning of the hlist. + * \param[in] n new entry to be added + * \param[in] h hlist head to add it after + * + * Insert a new entry after the specified head. + * This is good for implementing stacks. + */ +static inline void hlist_add_head(struct hlist_node *n, struct hlist_head *h) +{ + struct hlist_node *first = h->first; + WRITE_ONCE(n->next, first); + if (first) + WRITE_ONCE(first->pprev, &n->next); + WRITE_ONCE(h->first, n); + WRITE_ONCE(n->pprev, &h->first); +} + +/*! add a new entry before the one specified. + * @n: new entry to be added + * @next: hlist node to add it before, which must be non-NULL + */ +static inline void hlist_add_before(struct hlist_node *n, + struct hlist_node *next) +{ + WRITE_ONCE(n->pprev, next->pprev); + WRITE_ONCE(n->next, next); + WRITE_ONCE(next->pprev, &n->next); + WRITE_ONCE(*(n->pprev), n); +} + +/*! add a new entry after the one specified + * \param[in] n new entry to be added + * \param[in] prev hlist node to add it after, which must be non-NULL + */ +static inline void hlist_add_behind(struct hlist_node *n, + struct hlist_node *prev) +{ + WRITE_ONCE(n->next, prev->next); + WRITE_ONCE(prev->next, n); + WRITE_ONCE(n->pprev, &prev->next); + + if (n->next) + WRITE_ONCE(n->next->pprev, &n->next); +} + +/*! create a fake hlist consisting of a single headless node. + * \param[in] n Node to make a fake list out of + * + * This makes @n appear to be its own predecessor on a headless hlist. + * The point of this is to allow things like hlist_del() to work correctly + * in cases where there is no list. + */ +static inline void hlist_add_fake(struct hlist_node *n) +{ + n->pprev = &n->next; +} + +/*! Is this node a fake hlist?. + * \param[in] h Node to check for being a self-referential fake hlist. + */ +static inline bool hlist_fake(struct hlist_node *h) +{ + return h->pprev == &h->next; +} + +/*!is node the only element of the specified hlist?. + * \param[in] n Node to check for singularity. + * \param[in] h Header for potentially singular list. + * + * Check whether the node is the only node of the head without + * accessing head, thus avoiding unnecessary cache misses. + */ +static inline bool +hlist_is_singular_node(struct hlist_node *n, struct hlist_head *h) +{ + return !n->next && n->pprev == &h->first; +} + +/*! Move an hlist. + * \param[in] old hlist_head for old list. + * \param[in] new hlist_head for new list. + * + * Move a list from one list head to another. Fixup the pprev + * reference of the first entry if it exists. + */ +static inline void hlist_move_list(struct hlist_head *old, + struct hlist_head *_new) +{ + _new->first = old->first; + if (_new->first) + _new->first->pprev = &_new->first; + old->first = NULL; +} + +#define hlist_entry(ptr, type, member) container_of(ptr,type,member) + +#define hlist_for_each(pos, head) \ + for (pos = (head)->first; pos ; pos = pos->next) + +#define hlist_for_each_safe(pos, n, head) \ + for (pos = (head)->first; pos && ({ n = pos->next; 1; }); \ + pos = n) + +#define hlist_entry_safe(ptr, type, member) \ + ({ typeof(ptr) ____ptr = (ptr); \ + ____ptr ? hlist_entry(____ptr, type, member) : NULL; \ + }) + +/*! iterate over list of given type. + * \param[out] pos the type * to use as a loop cursor. + * \param[in] head the head for your list. + * \param[in] member the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry(pos, head, member) \ + for (pos = hlist_entry_safe((head)->first, typeof(*(pos)), member);\ + pos; \ + pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member)) + +/*! iterate over a hlist continuing after current point. + * \param[out] pos the type * to use as a loop cursor. + * \param[in] member the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_continue(pos, member) \ + for (pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member);\ + pos; \ + pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member)) + +/*! iterate over a hlist continuing from current point. + * \param[out] pos the type * to use as a loop cursor. + * \param[in] member the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_from(pos, member) \ + for (; pos; \ + pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member)) + +/*! hlist_for_each_entry_safe - iterate over list of given type safe against removal of list entry. + * \param[out] pos the type * to use as a loop cursor. + * \param[out] n a &struct hlist_node to use as temporary storage + * \param[in] head the head for your list. + * \param[in] member the name of the hlist_node within the struct + */ +#define hlist_for_each_entry_safe(pos, n, head, member) \ + for (pos = hlist_entry_safe((head)->first, typeof(*pos), member);\ + pos && ({ n = pos->member.next; 1; }); \ + pos = hlist_entry_safe(n, typeof(*pos), member)) + + +/*! + * @} + */ diff --git a/uqmid/osmocom/logging.h b/uqmid/osmocom/logging.h new file mode 100644 index 0000000..bb24507 --- /dev/null +++ b/uqmid/osmocom/logging.h @@ -0,0 +1,48 @@ +#pragma once + +/* from osmocore/logging.h */ + +/*! different log levels */ +#define LOGL_DEBUG 1 /*!< debugging information */ +#define LOGL_INFO 3 /*!< general information */ +#define LOGL_NOTICE 5 /*!< abnormal/unexpected condition */ +#define LOGL_ERROR 7 /*!< error condition, requires user action */ +#define LOGL_FATAL 8 /*!< fatal, program aborted */ + +/* logging subsystems defined by the library itself */ +#define DLGLOBAL -1 /*!< global logging */ + +/*! Log a new message through the Osmocom logging framework + * \param[in] ss logging subsystem (e.g. \ref DLGLOBAL) + * \param[in] level logging level (e.g. \ref LOGL_NOTICE) + * \param[in] fmt format string + * \param[in] args variable argument list + */ +#define LOGP(ss, level, fmt, args...) \ + LOGPSRC(ss, level, NULL, 0, fmt, ## args) + +/*! Log through the Osmocom logging framework with explicit source. + * If caller_file is passed as NULL, __FILE__ and __LINE__ are used + * instead of caller_file and caller_line (so that this macro here defines + * both cases in the same place, and to catch cases where callers fail to pass + * a non-null filename string). + * \param[in] ss logging subsystem (e.g. \ref DLGLOBAL) + * \param[in] level logging level (e.g. \ref LOGL_NOTICE) + * \param[in] caller_file caller's source file string (e.g. __FILE__) + * \param[in] caller_line caller's source line nr (e.g. __LINE__) + * \param[in] fmt format string + * \param[in] args variable argument list + */ +#define LOGPSRC(ss, level, caller_file, caller_line, fmt, args...) \ + LOGPSRCC(ss, level, caller_file, caller_line, 0, fmt, ##args) + +/* TODO: implement proper logging */ +#define LOGPSRCC(ss, level, caller_file, caller_line, cont, fmt, args...) \ + do { \ + if (caller_file) \ + fprintf(stderr, "%d: %s:%d: " fmt, level, (char *) caller_file, caller_line, ##args); \ + else \ + fprintf(stderr, "%d: %s:%d: " fmt, level, __FILE__, __LINE__, ##args); \ + } while(0) + +#define osmo_log_backtrace(ss, level) fprintf(stderr, "%s:%d: backtrace not compiled in.", __FILE__, __LINE__); diff --git a/uqmid/osmocom/timer.c b/uqmid/osmocom/timer.c new file mode 100644 index 0000000..bb18d03 --- /dev/null +++ b/uqmid/osmocom/timer.c @@ -0,0 +1,75 @@ +/* + * timer.c a shim layer around osmo timer to use libubox/uloop timers + * + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + + +#include + +#include "timer.h" + +static void osmo_timer_call_callback(struct uloop_timeout *timeout) +{ + struct pipe_timer *timer = container_of(timeout, struct pipe_timer, timeout); + + if (timer->cb) + timer->cb(timer->data); +} + +void osmo_timer_setup(struct pipe_timer *timer, void (*cb)(void *data), void *data) +{ + timer->timeout.cb = osmo_timer_call_callback; + timer->cb = cb; + timer->data = data; +} + +void osmo_timer_add(struct pipe_timer *timer) +{ + uloop_timeout_add(&timer->timeout); +} + +void osmo_timer_schedule(struct pipe_timer *timer, int seconds, int microseconds) +{ + int msecs = seconds * 1000 + microseconds / 1000; + if (msecs == 0 && microseconds > 0) + msecs = 1; + + uloop_timeout_set(&timer->timeout, msecs); +} + +void osmo_timer_del(struct pipe_timer *timer) +{ + uloop_timeout_cancel(&timer->timeout); +} + +int osmo_timer_pending(const struct pipe_timer *timer) +{ + return timer->timeout.pending; +} + +int osmo_timer_remaining(struct pipe_timer *timer, + const struct timeval *now, + struct timeval *remaining) +{ + int64_t remain = uloop_timeout_remaining64(&timer->timeout); + if (remain > INT_MAX) + return INT_MAX; + + return remain; +} diff --git a/uqmid/osmocom/timer.h b/uqmid/osmocom/timer.h new file mode 100644 index 0000000..643ae6f --- /dev/null +++ b/uqmid/osmocom/timer.h @@ -0,0 +1,43 @@ +/* + * timer.h a shim layer around osmo timer to use libubox/uloop timers + * + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#pragma once + +#include + +/* add a simple layer struct to allow to support the *data pointer in libubox */ +struct pipe_timer { + struct uloop_timeout timeout; + void (*cb)(void*); + void *data; +}; + +/* struct osmo_timer_list -> struct uloop_timeout */ +#define osmo_timer_list pipe_timer + +void osmo_timer_setup(struct pipe_timer *timer, void (*cb)(void *data), void *data); +void osmo_timer_add(struct pipe_timer *timer); +void osmo_timer_schedule(struct pipe_timer *timer, int seconds, int microseconds); +void osmo_timer_del(struct pipe_timer *timer); +int osmo_timer_pending(const struct pipe_timer *timer); +int osmo_timer_remaining(struct pipe_timer *timer, + const struct timeval *now, + struct timeval *remaining); diff --git a/uqmid/osmocom/utils.c b/uqmid/osmocom/utils.c new file mode 100644 index 0000000..2119193 --- /dev/null +++ b/uqmid/osmocom/utils.c @@ -0,0 +1,285 @@ +/* + * a minimal version of utils.c from libosmocore + * + * (C) 2011 by Harald Welte + * (C) 2011 by Sylvain Munaut + * (C) 2014 by Nils O. SelÃ¥sdal + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "utils.h" + +static __thread char namebuf[255]; +static const char osmo_identifier_illegal_chars[] = "., {}[]()<>|~\\^`'\"?=;/+*&%$#!"; + +/*! Determine if a given identifier is valid, i.e. doesn't contain illegal chars + * \param[in] str String to validate + * \param[in] sep_chars Permitted separation characters between identifiers. + * \returns true in case \a str contains only valid identifiers and sep_chars, false otherwise + */ +bool osmo_separated_identifiers_valid(const char *str, const char *sep_chars) +{ + /* characters that are illegal in names */ + unsigned int i; + size_t len; + + /* an empty string is not a valid identifier */ + if (!str || (len = strlen(str)) == 0) + return false; + + for (i = 0; i < len; i++) { + if (sep_chars && strchr(sep_chars, str[i])) + continue; + /* check for 7-bit ASCII */ + if (str[i] & 0x80) + return false; + if (!isprint((int)str[i])) + return false; + /* check for some explicit reserved control characters */ + if (strchr(osmo_identifier_illegal_chars, str[i])) + return false; + } + + return true; +} + +/*! Determine if a given identifier is valid, i.e. doesn't contain illegal chars + * \param[in] str String to validate + * \returns true in case \a str contains valid identifier, false otherwise + */ +bool osmo_identifier_valid(const char *str) +{ + return osmo_separated_identifiers_valid(str, NULL); +} + +/*! Replace characters in the given string buffer so that it is guaranteed to pass osmo_separated_identifiers_valid(). + * To guarantee passing osmo_separated_identifiers_valid(), replace_with must not itself be an illegal character. If in + * doubt, use '-'. + * \param[inout] str Identifier to sanitize, must be nul terminated and in a writable buffer. + * \param[in] sep_chars Additional characters that are to be replaced besides osmo_identifier_illegal_chars. + * \param[in] replace_with Replace any illegal characters with this character. + */ +void osmo_identifier_sanitize_buf(char *str, const char *sep_chars, char replace_with) +{ + char *pos; + if (!str) + return; + for (pos = str; *pos; pos++) { + if (strchr(osmo_identifier_illegal_chars, *pos) + || (sep_chars && strchr(sep_chars, *pos))) + *pos = replace_with; + } +} + +/*! get human-readable string for given value + * \param[in] vs Array of value_string tuples + * \param[in] val Value to be converted + * \returns pointer to human-readable string + * + * If val is found in vs, the array's string entry is returned. Otherwise, an + * "unknown" string containing the actual value is composed in a static buffer + * that is reused across invocations. + */ +const char *get_value_string(const struct value_string *vs, uint32_t val) +{ + const char *str = get_value_string_or_null(vs, val); + if (str) + return str; + + snprintf(namebuf, sizeof(namebuf), "unknown 0x%"PRIx32, val); + namebuf[sizeof(namebuf) - 1] = '\0'; + return namebuf; +} + +/*! get human-readable string or NULL for given value + * \param[in] vs Array of value_string tuples + * \param[in] val Value to be converted + * \returns pointer to human-readable string or NULL if val is not found + */ +const char *get_value_string_or_null(const struct value_string *vs, + uint32_t val) +{ + int i; + + if (!vs) + return NULL; + + for (i = 0;; i++) { + if (vs[i].value == 0 && vs[i].str == NULL) + break; + if (vs[i].value == val) + return vs[i].str; + } + + return NULL; +} + +/*! get numeric value for given human-readable string + * \param[in] vs Array of value_string tuples + * \param[in] str human-readable string + * \returns numeric value (>0) or negative numer in case of error + */ +int get_string_value(const struct value_string *vs, const char *str) +{ + int i; + + for (i = 0;; i++) { + if (vs[i].value == 0 && vs[i].str == NULL) + break; + if (!strcasecmp(vs[i].str, str)) + return vs[i].value; + } + return -EINVAL; +} + +/*! Convert BCD-encoded digit into printable character + * \param[in] bcd A single BCD-encoded digit + * \returns single printable character + */ +char osmo_bcd2char(uint8_t bcd) +{ + if (bcd < 0xa) + return '0' + bcd; + else + return 'A' + (bcd - 0xa); +} + +/*! Convert number in ASCII to BCD value + * \param[in] c ASCII character + * \returns BCD encoded value of character + */ +uint8_t osmo_char2bcd(char c) +{ + if (c >= '0' && c <= '9') + return c - 0x30; + else if (c >= 'A' && c <= 'F') + return 0xa + (c - 'A'); + else if (c >= 'a' && c <= 'f') + return 0xa + (c - 'a'); + else + return 0; +} + +/*! Convert BCD to string. + * The given nibble offsets are interpreted in BCD order, i.e. nibble 0 is bcd[0] & 0xf, nibble 1 is bcd[0] >> 4, nibble + * 3 is bcd[1] & 0xf, etc.. + * \param[out] dst Output string buffer, is always nul terminated when dst_size > 0. + * \param[in] dst_size sizeof() the output string buffer. + * \param[in] bcd Binary coded data buffer. + * \param[in] start_nibble Offset to start from, in nibbles, typically 1 to skip the first nibble. + * \param[in] end_nibble Offset to stop before, in nibbles, e.g. sizeof(bcd)*2 - (bcd[0] & GSM_MI_ODD? 0:1). + * \param[in] allow_hex If false, return error if there are digits other than 0-9. If true, return those as [A-F]. + * \returns The strlen that would be written if the output buffer is large enough, excluding nul byte (like + * snprintf()), or -EINVAL if allow_hex is false and a digit > 9 is encountered. On -EINVAL, the conversion is + * still completed as if allow_hex were passed as true. Return -ENOMEM if dst is NULL or dst_size is zero. + * If end_nibble <= start_nibble, write an empty string to dst and return 0. + */ +int osmo_bcd2str(char *dst, size_t dst_size, const uint8_t *bcd, int start_nibble, int end_nibble, bool allow_hex) +{ + char *dst_end = dst + dst_size - 1; + int nibble_i; + int rc = 0; + + if (!dst || dst_size < 1 || start_nibble < 0) + return -ENOMEM; + + for (nibble_i = start_nibble; nibble_i < end_nibble && dst < dst_end; nibble_i++, dst++) { + uint8_t nibble = bcd[nibble_i >> 1]; + if ((nibble_i & 1)) + nibble >>= 4; + nibble &= 0xf; + + if (!allow_hex && nibble > 9) + rc = -EINVAL; + + *dst = osmo_bcd2char(nibble); + } + *dst = '\0'; + + if (rc < 0) + return rc; + return OSMO_MAX(0, end_nibble - start_nibble); +} + +/*! Convert string to BCD. + * The given nibble offsets are interpreted in BCD order, i.e. nibble 0 is bcd[0] & 0x0f, nibble 1 is bcd[0] & 0xf0, nibble + * 3 is bcd[1] & 0x0f, etc.. + * \param[out] dst Output BCD buffer. + * \param[in] dst_size sizeof() the output string buffer. + * \param[in] digits String containing decimal or hexadecimal digits in upper or lower case. + * \param[in] start_nibble Offset to start from, in nibbles, typically 1 to skip the first (MI type) nibble. + * \param[in] end_nibble Negative to write all digits found in str, followed by 0xf nibbles to fill any started octet. + * If >= 0, stop before this offset in nibbles, e.g. to get default behavior, pass + * start_nibble + strlen(str) + ((start_nibble + strlen(str)) & 1? 1 : 0) + 1. + * \param[in] allow_hex If false, return error if there are hexadecimal digits (A-F). If true, write those to + * BCD. + * \returns The buffer size in octets that is used to place all bcd digits (including the skipped nibbles + * from 'start_nibble' and rounded up to full octets); -EINVAL on invalid digits; + * -ENOMEM if dst is NULL, if dst_size is too small to contain all nibbles, or if start_nibble is negative. + */ +int osmo_str2bcd(uint8_t *dst, size_t dst_size, const char *digits, int start_nibble, int end_nibble, bool allow_hex) +{ + const char *digit = digits; + int nibble_i; + + if (!dst || !dst_size || start_nibble < 0) + return -ENOMEM; + + if (end_nibble < 0) { + end_nibble = start_nibble + strlen(digits); + /* If the last octet is not complete, add another filler nibble */ + if (end_nibble & 1) + end_nibble++; + } + if ((unsigned int) (end_nibble / 2) > dst_size) + return -ENOMEM; + + for (nibble_i = start_nibble; nibble_i < end_nibble; nibble_i++) { + uint8_t nibble = 0xf; + int octet = nibble_i >> 1; + if (*digit) { + char c = *digit; + digit++; + if (c >= '0' && c <= '9') + nibble = c - '0'; + else if (allow_hex && c >= 'A' && c <= 'F') + nibble = 0xa + (c - 'A'); + else if (allow_hex && c >= 'a' && c <= 'f') + nibble = 0xa + (c - 'a'); + else + return -EINVAL; + } + nibble &= 0xf; + if ((nibble_i & 1)) + dst[octet] = (nibble << 4) | (dst[octet] & 0x0f); + else + dst[octet] = (dst[octet] & 0xf0) | nibble; + } + + /* floor(float(end_nibble) / 2) */ + return end_nibble / 2; +} diff --git a/uqmid/osmocom/utils.h b/uqmid/osmocom/utils.h new file mode 100644 index 0000000..562b444 --- /dev/null +++ b/uqmid/osmocom/utils.h @@ -0,0 +1,77 @@ +#pragma once + +/* a minimal version of libosmocore utils.h */ + +#include +#include +#include +#include +#include + +/*! \defgroup utils General-purpose utility functions + * @{ + * \file utils.h */ + +/*! Determine number of elements in an array of static size */ +#ifndef ARRAY_SIZE +#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) +#endif + +#define OSMO_ASSERT(exp) \ +do { \ + if (OSMO_UNLIKELY(!(exp))) { \ + fprintf(stderr, "Assert failed %s %s:%d\n", #exp, __FILE__, __LINE__); \ + exit(2342); \ + } \ +} while (0) + +/*! Return the maximum of two specified values */ +#define OSMO_MAX(a, b) ((a) >= (b) ? (a) : (b)) +/*! Return the minimum of two specified values */ +#define OSMO_MIN(a, b) ((a) >= (b) ? (b) : (a)) +/*! Return a typical cmp result for comparable entities a and b. */ +#define OSMO_CMP(a, b) ((a) < (b)? -1 : ((a) > (b)? 1 : 0)) + +/*! Stringify the name of a macro x, e.g. an FSM event name. + * Note: if nested within another preprocessor macro, this will + * stringify the value of x instead of its name. */ +#define OSMO_STRINGIFY(x) #x +/*! Stringify the value of a macro x, e.g. a port number. */ +#define OSMO_STRINGIFY_VAL(x) OSMO_STRINGIFY(x) +/*! Make a value_string entry from an enum value name */ +#define OSMO_VALUE_STRING(x) { x, #x } + +/*! Branch prediction optimizations */ +#if defined(__GNUC__) +#define OSMO_LIKELY(exp) __builtin_expect(!!(exp), 1) +#define OSMO_UNLIKELY(exp) __builtin_expect(!!(exp), 0) +#else +#define OSMO_LIKELY(exp) exp +#define OSMO_UNLIKELY(exp) exp +#endif + + +/*! A mapping between human-readable string and numeric value */ +struct value_string { + uint32_t value; /*!< numeric value */ + const char *str; /*!< human-readable string */ +}; + +const char *get_value_string(const struct value_string *vs, uint32_t val); +const char *get_value_string_or_null(const struct value_string *vs, + uint32_t val); + +int get_string_value(const struct value_string *vs, const char *str); + +bool osmo_identifier_valid(const char *str); +bool osmo_separated_identifiers_valid(const char *str, const char *sep_chars); +void osmo_identifier_sanitize_buf(char *str, const char *sep_chars, char replace_with); + +char osmo_bcd2char(uint8_t bcd); +/* only works for numbers in ASCII */ +uint8_t osmo_char2bcd(char c); + +int osmo_bcd2str(char *dst, size_t dst_size, const uint8_t *bcd, int start_nibble, int end_nibble, bool allow_hex); +int osmo_str2bcd(uint8_t *dst, size_t dst_size, const char *digits, int start_nibble, int end_nibble, bool allow_hex); + +/*! @} */ diff --git a/uqmid/services.c b/uqmid/services.c new file mode 100644 index 0000000..1d50d3a --- /dev/null +++ b/uqmid/services.c @@ -0,0 +1,244 @@ +/* similar to dev.c but self container */ + +#include + +#include +#include + +#include "qmi-enums.h" +#include "qmi-message.h" + +#include "ctrl.h" +#include "logging.h" +#include "modem.h" +#include "services.h" +#include "uqmid.h" + + +#ifdef DEBUG_PACKET +static void dump_packet(const char *prefix, void *ptr, int len) +{ + unsigned char *data = ptr; + int i; + + fprintf(stderr, "%s:", prefix); + for (i = 0; i < len; i++) + fprintf(stderr, " %02x", data[i]); + fprintf(stderr, "\n"); +} +#else +static void dump_packet(const char *prefix, void *ptr, int len) +{ +} +#endif + +struct qmi_service * +uqmi_service_find(struct qmi_dev *qmi, int service_id) +{ + struct qmi_service *service; + list_for_each_entry(service, &qmi->services, list) { + if (service->service == service_id) + return service; + } + + return NULL; +} + +struct qmi_service * +uqmi_service_create(struct qmi_dev *qmi, int service_id) +{ + struct qmi_service *service = talloc_zero(qmi, struct qmi_service); + + service->service = service_id; + service->qmi = qmi; + service->client_id = -1; + + list_add(&service->list, &qmi->services); + INIT_LIST_HEAD(&service->reqs); + + return service; +} + +struct qmi_service * +uqmi_service_find_or_init(struct qmi_dev *qmi, int service_id) +{ + struct qmi_service *service = uqmi_service_find(qmi, service_id); + + if (service) + return service; + + return uqmi_service_create(qmi, service_id); +} + +int +uqmi_service_get_next_tid(struct qmi_service *service) +{ + service->tid++; + /* CTL only has 8 bit tid */ + if (service->service == QMI_SERVICE_CTL && service->tid >= (1 << 8)) + service->tid = 1; + else if (!service->tid) + service->tid = 1; + + return service->tid; +} + +static int +_service_send_request(struct qmi_service *service, struct qmi_request *req) +{ + struct qmi_msg *msg = req->msg; + int len = qmi_complete_request_message(msg); + uint16_t tid = uqmi_service_get_next_tid(service); + + if (req->service->service == QMI_SERVICE_CTL) { + msg->ctl.transaction = tid; + } else { + /* FIXME: check if service state is ready for this */ + msg->svc.transaction = cpu_to_le16(tid); + msg->qmux.client = service->client_id; + } + + req->ret = -1; + req->tid = tid; + req->pending = true; + + /* FIXME: fix mbim support */ + + if (service->service == QMI_SERVICE_CTL) + modem_log(service->qmi->modem, LOGL_DEBUG, "Transmit message to srv %d msg %04x flag: %02x tid: %02x", + msg->qmux.service, le16_to_cpu(msg->ctl.message), msg->flags, le16_to_cpu(msg->ctl.transaction)); + else + modem_log(service->qmi->modem, LOGL_DEBUG, "Transmit message to srv %d msg %04x flag: %02x tid: %04x", + msg->qmux.service, le16_to_cpu(msg->svc.message), msg->flags, le16_to_cpu(msg->svc.transaction)); + + dump_packet("Send packet", msg, len); + ustream_write(&service->qmi->sf.stream, (void *) msg, len, false); + + return 0; +} + +/* send out all pending request */ +static int +uqmi_service_send_request(struct qmi_service *service) +{ + struct qmi_request *req; + list_for_each_entry(req, &service->reqs, list) { + if (!req->pending && !req->complete) + _service_send_request(service, req); + } + + return 0; +} + +void +uqmi_service_get_client_id_cb(struct qmi_service *service, uint16_t client_id) +{ + service->client_id = client_id; + service->state = SERVICE_READY; + + service_log(service, LOGL_INFO, "Assigned client id %d. Service READY", client_id); + uqmi_service_send_request(service); +} + +/* get a client id via ctrl service */ +static int +uqmi_service_get_client_id(struct qmi_service *service) +{ + if (service->service == QMI_SERVICE_CTL) { + fprintf(stderr, "Foo!!!"); + return -EINVAL; + } + + service_log(service, LOGL_INFO, "Request client id"); + + service->state = SERVICE_WAIT_CID; + uqmi_ctrl_request_clientid(service); + + return 0; +} + +int +uqmi_service_send_simple(struct qmi_service *service, + int(*encoder)(struct qmi_msg *msg), + request_cb cb, void *cb_data) +{ + struct qmi_request *req = talloc_zero(service, struct qmi_request); + struct qmi_msg *msg = talloc_zero_size(req, 1024); + + req->msg = msg; + req->cb = cb; + req->cb_data = cb_data; + + int ret = encoder(msg); + if (ret) { + cb(service, req, NULL); + talloc_free(req); + modem_log(service->qmi->modem, LOGL_ERROR, "Failed to encode in send_simple"); + return -1; + } + + return uqmi_service_send_msg(service, req); +} + +int +uqmi_service_send_msg(struct qmi_service *service, struct qmi_request *req) +{ + req->pending = false; + req->complete = false; + req->service = service; + + list_add(&req->list, &service->reqs); + if (service->state == SERVICE_IDLE) + return uqmi_service_get_client_id(service); + else + return uqmi_service_send_request(service); +} + +int +uqmi_service_send_msg2(struct qmi_dev *qmi, struct qmi_request *req, int service_id) +{ + struct qmi_service *service = uqmi_service_find_or_init(qmi, service_id); + + if (!service) { + /* FIXME: error_log("Couldn't find/create service for id %d", service_id) */ + return -EINVAL; + } + + req->pending = false; + req->complete = false; + req->service = service; + + list_add(&req->list, &service->reqs); + if (service->state == SERVICE_IDLE) + return uqmi_service_get_client_id(service); + else + return uqmi_service_send_request(service); +} + +/* called when the call id returns */ +void uqmi_service_close_cb(struct qmi_service *service) +{ + struct qmi_dev *qmi = service->qmi; + service_log(service, LOGL_INFO, "Released service."); + + list_del(&service->list); + talloc_free(service); + qmi_device_service_closed(qmi); +} + +void uqmi_service_close(struct qmi_service *service) +{ + if (service->service == QMI_SERVICE_CTL) + return; + + service_log(service, LOGL_INFO, "Closing service %d (cid %d)", service->service, service->client_id); + + /* FIXME: SERVICE_WAIT_CID might/will leak a CID */ + if (service->state != SERVICE_READY) { + uqmi_service_close_cb(service); + return; + } + + /* Control service is special */ + uqmi_ctrl_release_clientid(service); +} diff --git a/uqmid/services.h b/uqmid/services.h new file mode 100644 index 0000000..ed5594e --- /dev/null +++ b/uqmid/services.h @@ -0,0 +1,73 @@ +/* + * uqmi -- tiny QMI support implementation + * + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#ifndef __UQMID_QMI_DEV_H +#define __UQMID_QMI_DEV_H + +#include +#include + +#include "uqmid.h" + +struct qmi_dev; +struct qmi_request; + +enum { + SERVICE_IDLE, + SERVICE_WAIT_CID, + SERVICE_READY, +}; + +struct qmi_service { + int service; /*! service id */ + uint16_t client_id; /*! client id, unique per service */ + uint16_t tid; /*! the current transaction id */ + + uint16_t major; + uint16_t minor; + + int state; + + struct qmi_dev *qmi; + struct list_head list; /*! entry on qmi_dev */ + + /* contains all pending requests */ + struct list_head reqs; +}; + +struct qmi_service *uqmi_service_find(struct qmi_dev *qmi, int service_id); +struct qmi_service *uqmi_service_find_or_init(struct qmi_dev *qmi, int service_id); +int uqmi_service_open(struct qmi_dev *qmi); +void uqmi_service_close(struct qmi_service *service); +int uqmi_service_send_msg(struct qmi_service *service, struct qmi_request *req); +int uqmi_service_send_msg2(struct qmi_dev *qmi, struct qmi_request *req, int service_id); +int uqmi_service_send_simple(struct qmi_service *service, + int(*encode)(struct qmi_msg *msg), + request_cb cb, void *cb_data); + +int uqmi_service_get_next_tid(struct qmi_service *service); +struct qmi_service *uqmi_service_create(struct qmi_dev *qmi, int service_id); + +/* callback for the ctrl when allocating the client id */ +void uqmi_service_get_client_id_cb(struct qmi_service *service, uint16_t client_id); +void uqmi_service_close_cb(struct qmi_service *service); + +#endif /* __UQMID_QMI_DEV_H */ diff --git a/uqmid/sim.c b/uqmid/sim.c new file mode 100644 index 0000000..92a0b21 --- /dev/null +++ b/uqmid/sim.c @@ -0,0 +1,59 @@ +/* + * uqmi -- tiny QMI support implementation + * + * Copyright (C) 2024 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#include "sim.h" +#include "qmi-enums-uim.h" + +enum uqmi_sim_state uim_card_application_state_to_uqmi_state(int app_state) +{ + switch (app_state) { + case QMI_UIM_CARD_APPLICATION_STATE_UNKNOWN: + return UQMI_SIM_UNKNOWN; + case QMI_UIM_CARD_APPLICATION_STATE_PIN1_OR_UPIN_PIN_REQUIRED: + return UQMI_SIM_PIN_REQUIRED; + case QMI_UIM_CARD_APPLICATION_STATE_PUK1_OR_UPIN_PUK_REQUIRED: + return UQMI_SIM_PUK_REQUIRED; + case QMI_UIM_CARD_APPLICATION_STATE_READY: + return UQMI_SIM_READY; + default: + return UQMI_SIM_UNKNOWN; + } +} + +enum uqmi_sim_state uim_pin_to_uqmi_state(int upin_state) +{ + switch (upin_state) { + case QMI_UIM_PIN_STATE_NOT_INITIALIZED: + return UQMI_SIM_UNKNOWN; + case QMI_UIM_PIN_STATE_DISABLED: + return UQMI_SIM_READY; + case QMI_UIM_PIN_STATE_ENABLED_VERIFIED: + return UQMI_SIM_READY; + case QMI_UIM_PIN_STATE_ENABLED_NOT_VERIFIED: + return UQMI_SIM_PIN_REQUIRED; + case QMI_UIM_PIN_STATE_BLOCKED: + return UQMI_SIM_PUK_REQUIRED; + case QMI_UIM_PIN_STATE_PERMANENTLY_BLOCKED: + return UQMI_SIM_BLOCKED; + default: + return UQMI_SIM_UNKNOWN; + } +} diff --git a/uqmid/sim.h b/uqmid/sim.h new file mode 100644 index 0000000..8b7bf65 --- /dev/null +++ b/uqmid/sim.h @@ -0,0 +1,38 @@ +/* + * uqmi -- tiny QMI support implementation + * + * Copyright (C) 2024 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#ifndef __QMID_SIM_H +#define __QMID_SIM_H + +#include + +enum uqmi_sim_state { + UQMI_SIM_UNKNOWN = 0, + UQMI_SIM_PIN_REQUIRED, /* Pin1 required */ + UQMI_SIM_PUK_REQUIRED, /* Puk1 required */ + UQMI_SIM_READY, /* Ready to operate, either no pin required or already enterd */ + UQMI_SIM_BLOCKED, /* Blocked without knowing how to unlock. */ +}; + +enum uqmi_sim_state uim_card_application_state_to_uqmi_state(int app_state); +enum uqmi_sim_state uim_pin_to_uqmi_state(int upin_state); + +#endif /* __UTILS_H */ diff --git a/uqmid/ubus.c b/uqmid/ubus.c new file mode 100644 index 0000000..e243271 --- /dev/null +++ b/uqmid/ubus.c @@ -0,0 +1,459 @@ +/* + * uqmid + * Copyright (C) 2023 Alexander Couzens + * + * based on netifd/ubus.c by + * Copyright (C) 2012 Felix Fietkau + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ +#include "osmocom/fsm.h" +#include "qmi-enums-wds.h" + +#include +#include +#include + +#include + +#include "uqmid.h" +#include "logging.h" +#include "modem.h" +#include "utils.h" + +#include "ubus.h" + +struct ubus_context *ubus_ctx = NULL; +static struct blob_buf b; +static const char *ubus_path; +/* global object */ + +static void uqmid_ubus_add_fd(void) +{ + ubus_add_uloop(ubus_ctx); + system_fd_set_cloexec(ubus_ctx->sock.fd); +} + +static int uqmid_handle_restart(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + /* TODO: netifd is calling itself via exec, unsure if we need this also. + uqmid_restart(); */ + return 0; +} + +static int uqmid_reload(void) +{ + return 1; +} + +static int uqmid_handle_reload(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + if (uqmid_reload()) + return UBUS_STATUS_NOT_FOUND; + + return UBUS_STATUS_OK; +} + +static int uqmid_add_object(struct ubus_object *obj) +{ + int ret = ubus_add_object(ubus_ctx, obj); + + if (ret != 0) + LOG_ERROR("Failed to publish object '%s': %s\n", obj->name, ubus_strerror(ret)); + + return ret; +} + +enum { + AM_NAME, + AM_DEVICE, + AM_DRIVER, + __AM_MAX +}; + +/** ubus call add_modem{name: slot1, device: /dev/cdc-wdm0, driver: qmi or mbim (optional, default qmi)}` */ +static const struct blobmsg_policy add_modem_policy[__AM_MAX] = { + [AM_NAME] = { .name = "name", .type = BLOBMSG_TYPE_STRING }, + [AM_DEVICE] = { .name = "device", .type = BLOBMSG_TYPE_STRING }, + [AM_DRIVER] = { .name = "driver", .type = BLOBMSG_TYPE_STRING }, +}; + +static int add_modem(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + struct blob_attr *tb[__AM_MAX]; + char *name = NULL; + char *device = NULL; + /* tunnel qmi over mbim? */ + bool qmi_over_mbim = false; + int ret; + + blobmsg_parse(add_modem_policy, __AM_MAX, tb, blob_data(msg), blob_len(msg)); + if (!tb[AM_NAME]) { + LOG_ERROR("ubus add_modem: missing required argument name."); + return UBUS_STATUS_INVALID_ARGUMENT; + } + name = blobmsg_get_string(tb[AM_NAME]); + + if (!tb[AM_DEVICE]) { + LOG_ERROR("ubus add_modem: missing required argument device."); + return UBUS_STATUS_INVALID_ARGUMENT; + } + device = blobmsg_get_string(tb[AM_DEVICE]); + + if (tb[AM_DRIVER]) { + const char *driver = blobmsg_get_string(tb[AM_DRIVER]); + if (!strcmp(driver, "qmi")) { + /* default */ + } else if (!strcmp(driver, "mbim")) { + qmi_over_mbim = true; + } else { + LOG_ERROR("ubus add_modem: invalid driver. Valid qmi or mbim."); + return UBUS_STATUS_INVALID_ARGUMENT; + } + } + + ret = uqmid_modem_add(name, device, qmi_over_mbim); + if (ret) + return UBUS_STATUS_UNKNOWN_ERROR; + + return UBUS_STATUS_OK; +} + +/** ubus call remove_modem{name: slot1}` */ +enum { + RM_NAME, + __RM_MAX, +}; + +static const struct blobmsg_policy remove_modem_policy[__RM_MAX] = { + [RM_NAME] = { .name = "name", .type = BLOBMSG_TYPE_STRING }, +}; + +static int remove_modem(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + struct blob_attr *tb[__RM_MAX]; + struct modem *modem; + char *name = NULL; + int ret; + + blobmsg_parse(remove_modem_policy, __RM_MAX, tb, blob_data(msg), blob_len(msg)); + if (!tb[RM_NAME]) { + LOG_ERROR("ubus add_modem: missing required argument name."); + return UBUS_STATUS_INVALID_ARGUMENT; + } + name = blobmsg_get_string(tb[RM_NAME]); + modem = uqmid_modem_find_by_name(name); + if (!modem) { + LOG_ERROR("Couldn't find modem %s.", name); + return UBUS_STATUS_NOT_FOUND; + } + + ret = uqmid_modem_remove(modem); + if (ret) + return UBUS_STATUS_UNKNOWN_ERROR; + + return UBUS_STATUS_OK; +} + +/** clean up the ubus state of a modem */ +void uqmid_ubus_modem_destroy(struct modem *modem) +{ + if (!modem->ubus.name) + return; + + free((void *)modem->ubus.name); + modem->ubus.name = NULL; + ubus_remove_object(ubus_ctx, &modem->ubus); +} + +/** inform ubus subscribers of a state change of a modem */ +void uqmid_ubus_modem_notify_change(struct modem *modem, int event) +{ + /* TODO: */ +} + +static void uqmid_ubus_reconnect_timer(struct uloop_timeout *timeout) +{ + static struct uloop_timeout retry = { + .cb = uqmid_ubus_reconnect_timer, + }; + int t = 2; + + if (ubus_reconnect(ubus_ctx, ubus_path) != 0) { + LOG_INFO("failed to reconnect, trying again in %d seconds\n", t); + uloop_timeout_set(&retry, t * 1000); + return; + } + + LOG_INFO("reconnected to ubus, new id: %08x\n", ubus_ctx->local_id); + uqmid_ubus_add_fd(); +} + +static void uqmid_ubus_connection_lost(struct ubus_context *ctx) +{ + uqmid_ubus_reconnect_timer(NULL); +} + +static struct ubus_method main_object_methods[] = { + { .name = "restart", .handler = uqmid_handle_restart }, + { .name = "reload", .handler = uqmid_handle_reload }, + UBUS_METHOD("add_modem", add_modem, add_modem_policy), + UBUS_METHOD("remove_modem", remove_modem, remove_modem_policy), +}; + +static struct ubus_object_type main_object_type = UBUS_OBJECT_TYPE("uqmid", main_object_methods); + +static struct ubus_object main_object = { + .name = "uqmid", + .type = &main_object_type, + .methods = main_object_methods, + .n_methods = ARRAY_SIZE(main_object_methods), +}; + +/* modem object */ + +/** ubus call uqmid.modem.some1 remove_modem{name: slot1}` */ +static int modem_remove(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + struct modem *modem = container_of(obj, struct modem, ubus); + int ret; + + ret = uqmid_modem_remove(modem); + if (ret) + return UBUS_STATUS_UNKNOWN_ERROR; + + return UBUS_STATUS_OK; +} + +enum { CFG_APN, CFG_PIN, CFG_ROAMING, __CFG_MAX }; + +/** ubus call modem_configure{apn: internet, pin: 2342, roaming: false}` */ +static const struct blobmsg_policy modem_configure_policy[__CFG_MAX] = { + [CFG_APN] = { .name = "apn", .type = BLOBMSG_TYPE_STRING }, + [CFG_PIN] = { .name = "pin", .type = BLOBMSG_TYPE_STRING }, + [CFG_ROAMING] = { .name = "roaming", .type = BLOBMSG_TYPE_BOOL }, +}; + +static int modem_configure(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + struct modem *modem = container_of(obj, struct modem, ubus); + struct blob_attr *tb[__CFG_MAX]; + int ret; + + /* prevent mixing previous configure calls */ + memset(&modem->config, 0, sizeof(struct modem_config)); + blobmsg_parse(modem_configure_policy, __CFG_MAX, tb, blob_data(msg), blob_len(msg)); + if (tb[CFG_APN]) { + if (modem->config.apn) { + free(modem->config.apn); + } + modem->config.apn = strdup(blobmsg_get_string(tb[CFG_APN])); + } + + if (tb[CFG_PIN]) { + if (modem->config.pin) { + free(modem->config.pin); + } + modem->config.pin = strdup(blobmsg_get_string(tb[CFG_APN])); + } + + if (tb[CFG_ROAMING]) { + modem->config.roaming = blobmsg_get_bool(tb[CFG_ROAMING]); + } + + modem->config.pdp_type = QMI_WDS_PDP_TYPE_IPV4; + modem->config.configured = true; + + ret = uqmid_modem_configured(modem); + if (ret) + return UBUS_STATUS_UNKNOWN_ERROR; + + return UBUS_STATUS_OK; +} + +static void modem_get_opmode_cb(void *data, int opmode_err, int opmode) +{ + struct ubus_request_data *req = data; + if (opmode_err) { + blob_buf_init(&b, 0); + blobmsg_add_u16(&b, "operror", opmode_err & 0xff); + } else { + blob_buf_init(&b, 0); + blobmsg_add_u16(&b, "opmode", opmode & 0xff); + } + /* TODO: add additional opmode as str */ + ubus_send_reply(ubus_ctx, req, b.head); + ubus_complete_deferred_request(ubus_ctx, req, 0); + free(req); +} + +static int modem_get_opmode(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + struct modem *modem = container_of(obj, struct modem, ubus); + int ret; + struct ubus_request_data *new_req = calloc(sizeof(*req), 1); + if (!new_req) + return UBUS_STATUS_NO_MEMORY; + + ubus_defer_request(ctx, req, new_req); + ret = uqmid_modem_get_opmode(modem, modem_get_opmode_cb, new_req); + if (ret) + return UBUS_STATUS_UNKNOWN_ERROR; + + return UBUS_STATUS_OK; +} + +static int modem_networkstatus(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + struct modem *modem = container_of(obj, struct modem, ubus); + int ret; + + ret = uqmid_modem_networkstatus(modem); + if (ret) + return UBUS_STATUS_UNKNOWN_ERROR; + + return UBUS_STATUS_OK; +} + +static void blob_add_addr(struct blob_buf *blob, const char *name, struct sockaddr *addr) +{ + struct sockaddr_in *v4 = (struct sockaddr_in *)addr; + struct sockaddr_in6 *v6 = (struct sockaddr_in6 *)addr; + char buf[INET6_ADDRSTRLEN]; + + if (!addr) { + blobmsg_add_string(blob, name, ""); + return; + } + + switch (addr->sa_family) { + case AF_INET: + blobmsg_add_string(blob, name, inet_ntop(AF_INET, &v4->sin_addr, buf, sizeof(buf))); + break; + case AF_INET6: + blobmsg_add_string(blob, name, inet_ntop(AF_INET6, &v6->sin6_addr, buf, sizeof(buf))); + break; + } +} + +#define BLOBMSG_ADD_STR_CHECK(buffer, field, value) blobmsg_add_string(buffer, field, value ? value : "") + +static int modem_dump_state(struct ubus_context *ctx, struct ubus_object *obj, struct ubus_request_data *req, + const char *method, struct blob_attr *msg) +{ + struct modem *modem = container_of(obj, struct modem, ubus); + + blob_buf_init(&b, 0); + blobmsg_add_string(&b, "name", modem->name); + blobmsg_add_string(&b, "device", modem->device); + blobmsg_add_u32(&b, "state", modem->fi->state); + BLOBMSG_ADD_STR_CHECK(&b, "manufactor", modem->manuf); + BLOBMSG_ADD_STR_CHECK(&b, "model", modem->model); + BLOBMSG_ADD_STR_CHECK(&b, "rev", modem->rev); + BLOBMSG_ADD_STR_CHECK(&b, "imei", modem->imei); + BLOBMSG_ADD_STR_CHECK(&b, "imsi", modem->imsi); + BLOBMSG_ADD_STR_CHECK(&b, "iccid", modem->iccid); + /* session state */ + BLOBMSG_ADD_STR_CHECK(&b, "config_apn", modem->config.apn); + blob_add_addr(&b, "ipv4_addr", (struct sockaddr *)&modem->brearer.v4_addr); + blob_add_addr(&b, "ipv4_netmask", (struct sockaddr *)&modem->brearer.v4_netmask); + blob_add_addr(&b, "ipv4_gateway", (struct sockaddr *)&modem->brearer.v4_gateway); + blob_add_addr(&b, "ipv6", (struct sockaddr *)&modem->brearer.v6); + blob_add_addr(&b, "dns1", (struct sockaddr *)&modem->brearer.dns1); + blob_add_addr(&b, "dns2", (struct sockaddr *)&modem->brearer.dns2); + /* sim */ + /* TODO: add human readable enum values */ + blobmsg_add_u16(&b, "sim_state", modem->sim.state); + blobmsg_add_u8(&b, "sim_use_upin", modem->sim.use_upin); + blobmsg_add_u16(&b, "sim_pin_retries", modem->sim.pin_retries & 0xff); + blobmsg_add_u16(&b, "sim_puk_retries", modem->sim.puk_retries & 0xff); + + ubus_send_reply(ubus_ctx, req, b.head); + return UBUS_STATUS_OK; +} + +static struct ubus_method modem_instance_object_methods[] = { + UBUS_METHOD("configure", modem_configure, modem_configure_policy), + UBUS_METHOD_NOARG("remove", modem_remove), + UBUS_METHOD_NOARG("opmode", modem_get_opmode), + UBUS_METHOD_NOARG("networkstatus", modem_networkstatus), + UBUS_METHOD_NOARG("dump", modem_dump_state), + // { .name = "serving_system", .handler = modem_get_serving_system}, +}; + +static struct ubus_object_type modem_instance_object_type = + UBUS_OBJECT_TYPE("uqmid_modem_instance", modem_instance_object_methods); + +/* TODO: rename modem, modem instance */ +static struct ubus_method modem_object_methods[] = {}; + +static struct ubus_object_type modem_object_type = UBUS_OBJECT_TYPE("uqmid_modem", modem_object_methods); + +/* empty uqmid.modem object, only used as paraent */ +static struct ubus_object modem_object = { + .name = "uqmid.modem", + .type = &modem_object_type, + .methods = modem_object_methods, + .n_methods = 0, +}; + +/** called by modem.c to create the ubus object for a modem */ +int uqmid_ubus_modem_add(struct modem *modem) +{ + struct ubus_object *obj = &modem->ubus; + char *name = NULL; + int ret = 0; + + if (asprintf(&name, "%s.modem.%s", main_object.name, modem->name) == -1) + return -ENOMEM; + + obj->name = name; + obj->type = &modem_instance_object_type; + obj->methods = modem_instance_object_methods; + obj->n_methods = ARRAY_SIZE(modem_instance_object_methods); + if ((ret = ubus_add_object(ubus_ctx, &modem->ubus))) { + LOG_ERROR("failed to publish ubus object for modem '%s' (ret = %d).\n", modem->name, ret); + free(name); + obj->name = NULL; + return ret; + } + + return ret; +} + +int uqmid_ubus_init(const char *path) +{ + int ret; + + ubus_path = path; + ubus_ctx = ubus_connect(path); + if (!ubus_ctx) + return -EIO; + + LOG_DEBUG("connected as %08x\n", ubus_ctx->local_id); + ubus_ctx->connection_lost = uqmid_ubus_connection_lost; + uqmid_ubus_add_fd(); + + ret = uqmid_add_object(&main_object); + if ((ret = uqmid_add_object(&modem_object))) { + fprintf(stderr, "Failed to add modem object %d", ret); + return ret; + } + + return 0; +} diff --git a/uqmid/ubus.h b/uqmid/ubus.h new file mode 100644 index 0000000..e134cde --- /dev/null +++ b/uqmid/ubus.h @@ -0,0 +1,27 @@ +/* + * uqmid + * Copyright (C) 2023 Alexander Couzens + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ +#ifndef __UQMID_UBUS_H +#define __UQMID_UBUS_H + +struct modem; +struct ubus_context; + +extern struct ubus_context *ubus_ctx; +int uqmid_ubus_init(const char *path); + +int uqmid_ubus_modem_add(struct modem *modem); +void uqmid_ubus_modem_destroy(struct modem *modem); +void uqmid_ubus_modem_notify_change(struct modem *modem, int event); + +#endif /* __UQMID_UBUS_H */ diff --git a/uqmid/uqmid.c b/uqmid/uqmid.c new file mode 100644 index 0000000..58e0c7f --- /dev/null +++ b/uqmid/uqmid.c @@ -0,0 +1,79 @@ +/* + * uqmid -- implement a daemon based on the uqmi idea + * + * Copyright (C) 2014-2015 Felix Fietkau + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uqmid.h" +#include "ubus.h" + +static const struct option uqmid_getopt[] = { + { NULL, 0, NULL, 0 } +}; +#undef __uqmi_command + +static int usage(const char *progname) +{ + fprintf(stderr, "Usage: %s \n" + "\n", progname); + return 1; +} + +static void handle_exit_signal(int signal) +{ + uloop_end(); +} + +int main(int argc, char **argv) +{ + int ch, ret; + + uloop_init(); + signal(SIGINT, handle_exit_signal); + signal(SIGTERM, handle_exit_signal); + + while ((ch = getopt_long(argc, argv, "d:", uqmid_getopt, NULL)) != -1) { + switch(ch) { + default: + return usage(argv[0]); + } + } + + uloop_init(); + /* TODO: add option for the ubus path */ + ret = uqmid_ubus_init(NULL); + if (ret) { + fprintf(stderr, "Failed to connect"); + exit(1); + } + + uloop_run(); + + return ret; +} diff --git a/uqmid/uqmid.h b/uqmid/uqmid.h new file mode 100644 index 0000000..c239304 --- /dev/null +++ b/uqmid/uqmid.h @@ -0,0 +1,95 @@ +/* + * uqmid -- tiny QMI support implementation + * + * Copyright (C) 2014-2015 Felix Fietkau + * Copyright (C) 2023 Alexander Couzens + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#ifndef __UQMID_H +#define __UQMID_H + +#include + +enum { + L_CRIT, + L_WARNING, + L_NOTICE, + L_INFO, + L_DEBUG +}; + +struct qmi_dev; +struct qmi_service; +struct qmi_request; +struct qmi_msg; +struct modem; + +typedef void (*request_cb)(struct qmi_service *service, struct qmi_request *req, struct qmi_msg *msg); + +struct qmi_request { + struct list_head list; /*! entry on service->reqs */ + struct qmi_service *service; /*! pointer back */ + + struct qmi_msg *msg; + + request_cb cb; + void *cb_data; + + bool complete; + bool pending; + bool no_error_cb; + uint16_t tid; + int ret; +}; + +/* FIXME: describe state machine */ +enum { + QMI_RUNNING, + QMI_ERROR, + QMI_STOPPING, +}; + +struct qmi_dev { + struct ustream_fd sf; + + struct list_head services; + struct qmi_service *ctrl; + struct modem *modem; + + bool is_mbim; + + struct { + bool valid; + uint8_t profile_id; + uint8_t packet_handle; + uint8_t ip_family; + } wds; + + int state; + struct uloop_timeout shutdown; + void (*error_cb)(struct qmi_dev *dev, void *data); + void *error_cb_data; + void (*closing_cb)(struct qmi_dev *dev, void *data); + void *closing_cb_data; +}; + +struct qmi_dev *qmi_device_open(struct modem *modem, const char *path); +void qmi_device_close(struct qmi_dev *qmi, int timeout_ms); +void qmi_device_service_closed(struct qmi_dev *qmi); + +#endif /* __UQMID_H */ diff --git a/uqmid/uqmid.init.sh b/uqmid/uqmid.init.sh new file mode 100755 index 0000000..564f394 --- /dev/null +++ b/uqmid/uqmid.init.sh @@ -0,0 +1,12 @@ +#!/bin/sh /etc/rc.common + +START=35 +STOP=85 +USE_PROCD=1 + +start_service() { + procd_open_instance + procd_set_param command /sbin/uqmid + procd_set_param respawn + procd_close_instance +} -- 2.30.2