From: Steve Schrock <steve.schrock@getcruise.com>
To: ofono@lists.linux.dev
Cc: Steve Schrock <steve.schrock@getcruise.com>
Subject: [PATCH] qmimodem: QRTR service discovery
Date: Tue, 27 Feb 2024 17:12:35 +0000 [thread overview]
Message-ID: <20240227171235.128397-1-steve.schrock@getcruise.com> (raw)
QRTR service discovery works by sending QRTR_TYPE_NEW_LOOKUP to
the special socket of type AF_QIPCRTR. Then the services are received
one-by-one. Soon they will be stored and made available for use by the
qmi client.
QRTR modem discovery and instantiation will be implemented later.
---
drivers/qmimodem/qmi.c | 312 +++++++++++++++++++++++++++++++++++++++++
drivers/qmimodem/qmi.h | 1 +
2 files changed, 313 insertions(+)
diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c
index 35751d7c..5f730a74 100644
--- a/drivers/qmimodem/qmi.c
+++ b/drivers/qmimodem/qmi.c
@@ -33,6 +33,8 @@
#include <stdlib.h>
#include <string.h>
#include <linux/limits.h>
+#include <linux/qrtr.h>
+#include <sys/socket.h>
#include <ell/ell.h>
@@ -1874,6 +1876,316 @@ struct qmi_device *qmi_device_new_qmux(const char *device)
return &qmux->super;
}
+struct qmi_device_qrtr {
+ struct qmi_device super;
+ qmi_shutdown_func_t shutdown_func;
+ void *shutdown_user_data;
+ qmi_destroy_func_t shutdown_destroy;
+ struct l_idle *shutdown_idle;
+};
+
+static void qrtr_debug_ctrl_request(const struct qrtr_ctrl_pkt *packet,
+ qmi_debug_func_t function,
+ void *user_data)
+{
+ char strbuf[72 + 16], *str;
+ const char *type;
+
+ if (!function)
+ return;
+
+ str = strbuf;
+ str += sprintf(str, " %s",
+ __service_type_to_string(QMI_SERVICE_CONTROL));
+
+ type = "_pkt";
+
+ str += sprintf(str, "%s cmd=%d", type,
+ L_LE32_TO_CPU(packet->cmd));
+
+ function(strbuf, user_data);
+}
+
+static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
+ const struct qrtr_ctrl_pkt *packet)
+{
+ struct qmi_device *device = &qrtr->super;
+ uint32_t cmd;
+ uint32_t type;
+ uint32_t instance;
+ uint32_t version;
+ uint32_t node;
+ uint32_t port;
+
+ qrtr_debug_ctrl_request(packet, device->debug_func,
+ device->debug_data);
+
+ cmd = L_LE32_TO_CPU(packet->cmd);
+ if (cmd != QRTR_TYPE_NEW_SERVER) {
+ DBG("Unknown command: %d", cmd);
+ return;
+ }
+
+ if (!packet->server.service && !packet->server.instance &&
+ !packet->server.node && !packet->server.port) {
+ struct discover_data *data;
+
+ DBG("Initial service discovery has completed");
+
+ data = l_queue_peek_head(device->discovery_queue);
+
+ if (data->func)
+ data->func(data->user_data);
+
+ __qmi_device_discovery_complete(device, &data->super);
+
+ return;
+ }
+
+ type = L_LE32_TO_CPU(packet->server.service);
+ version = L_LE32_TO_CPU(packet->server.instance) & 0xff;
+ instance = L_LE32_TO_CPU(packet->server.instance) >> 8;
+
+ node = L_LE32_TO_CPU(packet->server.node);
+ port = L_LE32_TO_CPU(packet->server.port);
+
+ if (cmd == QRTR_TYPE_NEW_SERVER) {
+ DBG("New server: Type: %d Version: %d Instance: %d Node: %d Port: %d",
+ type, version, instance, node, port);
+ }
+}
+
+static void qrtr_handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port,
+ const void *buf, ssize_t len)
+{
+ const struct qrtr_ctrl_pkt *packet = buf;
+
+ if (sending_port != QRTR_PORT_CTRL) {
+ DBG("Receive of service data is not implemented");
+ return;
+ }
+
+ if ((unsigned long) len < sizeof(*packet)) {
+ DBG("qrtr control packet is too small");
+ return;
+ }
+
+ qrtr_handle_control_packet(qrtr, packet);
+}
+
+static bool qrtr_received_data(struct l_io *io, void *user_data)
+{
+ struct qmi_device_qrtr *qrtr = user_data;
+ struct sockaddr_qrtr addr;
+ unsigned char buf[2048];
+ ssize_t bytes_read;
+ socklen_t addr_size;
+
+ addr_size = sizeof(addr);
+ bytes_read = recvfrom(l_io_get_fd(qrtr->super.io), buf, sizeof(buf), 0,
+ (struct sockaddr *) &addr, &addr_size);
+ DBG("Received %zd bytes from Node: %d Port: 0x%x", bytes_read,
+ addr.sq_node, addr.sq_port);
+
+ if (bytes_read < 0)
+ return true;
+
+ l_util_hexdump(true, buf, bytes_read, qrtr->super.debug_func,
+ qrtr->super.debug_data);
+
+ qrtr_handle_packet(qrtr, addr.sq_port, buf, bytes_read);
+
+ return true;
+}
+
+static void qrtr_discover_reply_timeout(struct l_timeout *timeout,
+ void *user_data)
+{
+ struct discover_data *data = user_data;
+ struct qmi_device *device = data->device;
+
+ l_timeout_remove(data->timeout);
+ data->timeout = NULL;
+
+ if (data->func)
+ data->func(data->user_data);
+
+ __qmi_device_discovery_complete(device, &data->super);
+}
+
+static int qmi_device_qrtr_discover(struct qmi_device *device,
+ qmi_discover_func_t func,
+ void *user_data,
+ qmi_destroy_func_t destroy)
+{
+ struct qmi_device_qrtr *qrtr =
+ l_container_of(device, struct qmi_device_qrtr, super);
+ struct discover_data *data;
+ struct qrtr_ctrl_pkt packet;
+ struct sockaddr_qrtr addr;
+ socklen_t addr_len;
+ int rc;
+ ssize_t bytes_written;
+ int fd;
+
+ __debug_device(device, "device %p discover", device);
+
+ if (l_queue_length(device->discovery_queue) > 0)
+ return -EINPROGRESS;
+
+ data = l_new(struct discover_data, 1);
+
+ data->super.destroy = discover_data_free;
+ data->device = device;
+ data->func = func;
+ data->user_data = user_data;
+ data->destroy = destroy;
+
+ fd = l_io_get_fd(device->io);
+
+ /*
+ * The control node is configured by the system. Use getsockname to
+ * get its value.
+ */
+ addr_len = sizeof(addr);
+ rc = getsockname(fd, (struct sockaddr *) &addr, &addr_len);
+ if (rc) {
+ DBG("getsockname failed: %s", strerror(errno));
+ rc = -errno;
+ goto error;
+ }
+
+ if (addr.sq_family != AF_QIPCRTR || addr_len != sizeof(addr)) {
+ DBG("Unexpected sockaddr from getsockname. family: %d size: %d",
+ addr.sq_family, addr_len);
+ rc = -EIO;
+ goto error;
+ }
+
+ addr.sq_port = QRTR_PORT_CTRL;
+ memset(&packet, 0, sizeof(packet));
+ packet.cmd = L_CPU_TO_LE32(QRTR_TYPE_NEW_LOOKUP);
+
+ bytes_written = sendto(fd, &packet,
+ sizeof(packet), 0,
+ (struct sockaddr *) &addr, addr_len);
+ if (bytes_written < 0) {
+ DBG("Failure sending data: %s", strerror(errno));
+ rc = -errno;
+ goto error;
+ }
+
+ l_util_hexdump(false, &packet, bytes_written,
+ device->debug_func, device->debug_data);
+
+ data->timeout = l_timeout_create(5, qrtr_discover_reply_timeout,
+ data, NULL);
+
+ __qmi_device_discovery_started(device, &data->super);
+
+ return 0;
+
+error:
+ __discovery_free(&data->super);
+
+ return rc;
+}
+
+static void qrtr_shutdown_destroy(void *user_data)
+{
+ struct qmi_device_qrtr *qrtr = user_data;
+
+ if (qrtr->shutdown_destroy)
+ qrtr->shutdown_destroy(qrtr->shutdown_user_data);
+
+ qrtr->shutdown_idle = NULL;
+
+ __qmi_device_shutdown_finished(&qrtr->super);
+}
+
+static void qrtr_shutdown_callback(struct l_idle *idle, void *user_data)
+{
+ struct qmi_device_qrtr *qrtr = user_data;
+
+ qrtr->super.shutting_down = true;
+
+ if (qrtr->shutdown_func)
+ qrtr->shutdown_func(qrtr->shutdown_user_data);
+
+ qrtr->super.shutting_down = false;
+
+ l_idle_remove(qrtr->shutdown_idle);
+}
+
+/*
+ * Prevents problems caused by device destruction through re-entrancy by
+ * performing destruction in an idle callback.
+ */
+static int qmi_device_qrtr_shutdown(struct qmi_device *device,
+ qmi_shutdown_func_t func,
+ void *user_data,
+ qmi_destroy_func_t destroy)
+{
+ struct qmi_device_qrtr *qrtr =
+ l_container_of(device, struct qmi_device_qrtr, super);
+
+ if (qrtr->shutdown_idle)
+ return -EALREADY;
+
+ __debug_device(&qrtr->super, "device %p shutdown", &qrtr->super);
+
+ qrtr->shutdown_idle = l_idle_create(qrtr_shutdown_callback, qrtr,
+ qrtr_shutdown_destroy);
+
+ if (!qrtr->shutdown_idle)
+ return -EIO;
+
+ qrtr->shutdown_func = func;
+ qrtr->shutdown_user_data = user_data;
+ qrtr->shutdown_destroy = destroy;
+
+ return 0;
+}
+
+static void qmi_device_qrtr_destroy(struct qmi_device *device)
+{
+ struct qmi_device_qrtr *qrtr =
+ l_container_of(device, struct qmi_device_qrtr, super);
+
+ l_free(qrtr);
+}
+
+static const struct qmi_device_ops qrtr_ops = {
+ .write = NULL,
+ .discover = qmi_device_qrtr_discover,
+ .client_create = NULL,
+ .client_release = NULL,
+ .shutdown = qmi_device_qrtr_shutdown,
+ .destroy = qmi_device_qrtr_destroy,
+};
+
+struct qmi_device *qmi_device_new_qrtr(void)
+{
+ struct qmi_device_qrtr *qrtr;
+ int fd;
+
+ fd = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (fd < 0)
+ return NULL;
+
+ qrtr = l_new(struct qmi_device_qrtr, 1);
+
+ if (qmi_device_init(&qrtr->super, fd, &qrtr_ops) < 0) {
+ close(fd);
+ l_free(qrtr);
+ return NULL;
+ }
+
+ l_io_set_read_handler(qrtr->super.io, qrtr_received_data, qrtr, NULL);
+
+ return &qrtr->super;
+}
+
struct qmi_param *qmi_param_new(void)
{
struct qmi_param *param;
diff --git a/drivers/qmimodem/qmi.h b/drivers/qmimodem/qmi.h
index 6a3d3415..506fed6b 100644
--- a/drivers/qmimodem/qmi.h
+++ b/drivers/qmimodem/qmi.h
@@ -101,6 +101,7 @@ bool qmi_device_set_expected_data_format(struct qmi_device *device,
enum qmi_device_expected_data_format format);
struct qmi_device *qmi_device_new_qmux(const char *device);
+struct qmi_device *qmi_device_new_qrtr(void);
struct qmi_param;
--
2.43.2
--
*Confidentiality Note:* We care about protecting our proprietary
information, confidential material, and trade secrets. This message may
contain some or all of those things. Cruise will suffer material harm if
anyone other than the intended recipient disseminates or takes any action
based on this message. If you have received this message (including any
attachments) in error, please delete it immediately and notify the sender
promptly.
next reply other threads:[~2024-02-27 17:14 UTC|newest]
Thread overview: 3+ messages / expand[flat|nested] mbox.gz Atom feed top
2024-02-27 17:12 Steve Schrock [this message]
-- strict thread matches above, loose matches on Subject: below --
2024-02-27 21:01 [PATCH] qmimodem: QRTR service discovery Steve Schrock
2024-02-27 22:10 ` patchwork-bot+ofono
Reply instructions:
You may reply publicly to this message via plain-text email
using any one of the following methods:
* Save the following mbox file, import it into your mail client,
and reply-to-all from there: mbox
Avoid top-posting and favor interleaved quoting:
https://en.wikipedia.org/wiki/Posting_style#Interleaved_style
* Reply using the --to, --cc, and --in-reply-to
switches of git-send-email(1):
git send-email \
--in-reply-to=20240227171235.128397-1-steve.schrock@getcruise.com \
--to=steve.schrock@getcruise.com \
--cc=ofono@lists.linux.dev \
/path/to/YOUR_REPLY
https://kernel.org/pub/software/scm/git/docs/git-send-email.html
* If your mail client supports setting the In-Reply-To header
via mailto: links, try the mailto: link
Be sure your reply has a Subject: header at the top and a blank line
before the message body.
This is a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox;
as well as URLs for read-only IMAP folder(s) and NNTP newsgroup(s).