From 2b126725bff51752ce586be8b7ff03de87e5aef9 Mon Sep 17 00:00:00 2001 From: Andrew Lassalle Date: Tue, 24 Nov 2020 16:24:03 -0800 Subject: qrtr-bus-watcher: add a class to watch for QRTR nodes Add a watcher to listen for QRTR add/remove signals. --- src/Makefile.am | 7 + src/mm-base-manager.c | 53 ++++++- src/mm-qrtr-bus-watcher.c | 354 ++++++++++++++++++++++++++++++++++++++++++++++ src/mm-qrtr-bus-watcher.h | 69 +++++++++ 4 files changed, 482 insertions(+), 1 deletion(-) create mode 100644 src/mm-qrtr-bus-watcher.c create mode 100644 src/mm-qrtr-bus-watcher.h diff --git a/src/Makefile.am b/src/Makefile.am index 8de74a0b..030492e7 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -373,6 +373,13 @@ ModemManager_SOURCES = \ mm-shared.h \ $(NULL) +if WITH_QRTR +ModemManager_SOURCES += \ + mm-qrtr-bus-watcher.h \ + mm-qrtr-bus-watcher.c \ + $(NULL) +endif + nodist_ModemManager_SOURCES = $(DAEMON_ENUMS_GENERATED) # Additional suspend/resume support via systemd diff --git a/src/mm-base-manager.c b/src/mm-base-manager.c index 03a387c3..61b30146 100644 --- a/src/mm-base-manager.c +++ b/src/mm-base-manager.c @@ -27,8 +27,9 @@ #if defined WITH_QMI # include #endif -#if defined WITH_QMI && QMI_QRTR_SUPPORTED +#if defined WITH_QRTR # include "mm-kernel-device-qrtr.h" +# include "mm-qrtr-bus-watcher.h" #endif #if defined WITH_UDEV # include "mm-kernel-device-udev.h" @@ -104,6 +105,10 @@ struct _MMBaseManagerPrivate { /* The UDev client */ GUdevClient *udev; #endif +#if defined WITH_QRTR + /* The Qrtr Bus Watcher */ + MMQrtrBusWatcher *qrtr_bus_watcher; +#endif }; /*****************************************************************************/ @@ -365,6 +370,34 @@ device_added (MMBaseManager *self, mm_device_grab_port (device, port); } +#if defined WITH_QRTR + +static void +handle_qrtr_device_added (MMBaseManager *self, + guint node_id, + MMQrtrBusWatcher *bus_watcher) +{ + g_autoptr(MMKernelDevice) kernel_device = NULL; + QrtrNode *node; + + node = mm_qrtr_bus_watcher_peek_node (bus_watcher, node_id); + + kernel_device = mm_kernel_device_qrtr_new (node); + + device_added (self, kernel_device, TRUE, FALSE); +} + +static void +handle_qrtr_device_removed (MMBaseManager *self, + guint node_id) +{ + g_autofree gchar *qrtr_device_name = NULL; + + qrtr_device_name = mm_kernel_device_qrtr_helper_build_name (node_id); + device_removed (self, MM_KERNEL_DEVICE_QRTR_SUBSYSTEM, qrtr_device_name); +} +#endif + static gboolean handle_kernel_event (MMBaseManager *self, MMKernelEventProperties *properties, @@ -1422,6 +1455,19 @@ initable_init (GInitable *initable, { MMBaseManager *self = MM_BASE_MANAGER (initable); +#if defined WITH_QRTR + /* Create and setup the QrtrBusWatcher */ + self->priv->qrtr_bus_watcher = mm_qrtr_bus_watcher_new (); + mm_qrtr_bus_watcher_start (self->priv->qrtr_bus_watcher, NULL, NULL); + + /* If autoscan enabled, list for QrtrBusWatcher events */ + if (self->priv->auto_scan) { + g_object_connect (self->priv->qrtr_bus_watcher, + "swapped-signal::" MM_QRTR_BUS_WATCHER_DEVICE_ADDED, G_CALLBACK (handle_qrtr_device_added), self, + "swapped-signal::" MM_QRTR_BUS_WATCHER_DEVICE_REMOVED, G_CALLBACK (handle_qrtr_device_removed), self, + NULL); + } +#endif /* Create filter */ self->priv->filter = mm_filter_new (self->priv->filter_policy, error); if (!self->priv->filter) @@ -1486,6 +1532,11 @@ finalize (GObject *object) g_object_unref (self->priv->udev); #endif +#if defined WITH_QRTR + if (self->priv->qrtr_bus_watcher) + g_object_unref (self->priv->qrtr_bus_watcher); +#endif + if (self->priv->filter) g_object_unref (self->priv->filter); diff --git a/src/mm-qrtr-bus-watcher.c b/src/mm-qrtr-bus-watcher.c new file mode 100644 index 00000000..ee0cb28b --- /dev/null +++ b/src/mm-qrtr-bus-watcher.c @@ -0,0 +1,354 @@ +/* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ +/* + * 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: + * + * Copyright 2020 Google LLC + */ + +#include +#include +#include + +#include "mm-utils.h" + +#include "mm-qrtr-bus-watcher.h" + +static void log_object_iface_init (MMLogObjectInterface *iface); + +G_DEFINE_TYPE_EXTENDED (MMQrtrBusWatcher, mm_qrtr_bus_watcher, G_TYPE_OBJECT, 0, + G_IMPLEMENT_INTERFACE (MM_TYPE_LOG_OBJECT, log_object_iface_init)) + +struct _MMQrtrBusWatcherPrivate { + QrtrBus *qrtr_bus; + guint node_added_id; + guint node_removed_id; + + /* Map of NodeNumber -> QRTR nodes available */ + GHashTable *nodes; +}; + +enum { + QRTR_DEVICE_ADDED, + QRTR_DEVICE_REMOVED, + LAST_SIGNAL, +}; + +static guint signals[LAST_SIGNAL] = { 0 }; + +/*****************************************************************************/ + +static gchar * +log_object_build_id (MMLogObject *_self) +{ + return g_strdup ("qrtr-bus-watcher"); +} + +/*****************************************************************************/ + +typedef struct { + MMQrtrBusWatcher *self; + QrtrNode *node; +} DeviceContext; + +static void +device_context_free (DeviceContext *ctx) +{ + g_object_unref (ctx->self); + g_object_unref (ctx->node); + g_slice_free (DeviceContext, ctx); +} + +static void +qrtr_node_services_ready (QrtrNode *node, + GAsyncResult *res, + DeviceContext *ctx) +{ + guint32 node_id; + + node_id = qrtr_node_get_id (node); + if (!qrtr_node_wait_for_services_finish (node, res, NULL)) { + mm_obj_dbg (ctx->self, + "qrtr node %u doesn't have required services to be considered a control node", + node_id); + g_hash_table_remove (ctx->self->priv->nodes, GUINT_TO_POINTER (node_id)); + device_context_free (ctx); + return; + } + + mm_obj_info (ctx->self, "qrtr services ready for node %u", node_id); + g_signal_emit (ctx->self, signals[QRTR_DEVICE_ADDED], 0, node_id); + device_context_free (ctx); +} + +static void +handle_qrtr_node_added (QrtrBus *qrtr_bus, + guint32 node_id, + MMQrtrBusWatcher *self) +{ + g_autoptr(QrtrNode) node = NULL; + g_autoptr(GArray) services = NULL; + DeviceContext *ctx; + static const QmiService required_services[] = { + QMI_SERVICE_WDS, + QMI_SERVICE_NAS, + QMI_SERVICE_DMS + }; + + mm_obj_dbg (self, "qrtr node %u added", node_id); + + node = qrtr_bus_get_node (qrtr_bus, node_id); + if (!node) { + mm_obj_warn (self, "cannot find node %u", node_id); + return; + } + + if (g_hash_table_contains (self->priv->nodes, GUINT_TO_POINTER (node_id))) { + mm_obj_warn (self, "qrtr node %u was previously added", node_id); + return; + } + + /* a full node reference now owned by the hash table */ + g_hash_table_insert (self->priv->nodes, GUINT_TO_POINTER (node_id), g_object_ref (node)); + + mm_obj_dbg (self, "waiting for modem services on node %u", node_id); + + /* Check if the node provides services to be sure the node represents a + * modem. */ + services = g_array_sized_new (FALSE, FALSE, sizeof (QmiService), G_N_ELEMENTS (required_services)); + g_array_append_vals (services, required_services, G_N_ELEMENTS (required_services)); + + /* Setup command context */ + ctx = g_slice_new0 (DeviceContext); + ctx->self = g_object_ref (self); + ctx->node = g_object_ref (node); + + qrtr_node_wait_for_services (node, + services, + 1000, /* ms */ + NULL, + (GAsyncReadyCallback) qrtr_node_services_ready, + ctx); +} + +static void +handle_qrtr_node_removed (QrtrBus *qrtr_bus, + guint32 node_id, + MMQrtrBusWatcher *self) +{ + QrtrNode *node; + + node = qrtr_bus_get_node (qrtr_bus, node_id); + if (!node) { + mm_obj_warn (self, "cannot find node %u", node_id); + return; + } + + g_hash_table_remove (self->priv->nodes, GUINT_TO_POINTER (node_id)); + mm_obj_info (self, "qrtr node %u removed", node_id); + + g_signal_emit (self, signals[QRTR_DEVICE_REMOVED], 0, node_id); +} + +/*****************************************************************************/ + +QrtrNode * +mm_qrtr_bus_watcher_peek_node (MMQrtrBusWatcher *self, + guint32 node_id) +{ + g_assert (MM_IS_QRTR_BUS_WATCHER (self)); + + return g_hash_table_lookup (self->priv->nodes, GUINT_TO_POINTER (node_id)); +} + +/*****************************************************************************/ + +gboolean +mm_qrtr_bus_watcher_start_finish (MMQrtrBusWatcher *self, + GAsyncResult *res, + GError **error) +{ + return g_task_propagate_boolean (G_TASK (res), error); +} + +typedef struct { + MMQrtrBusWatcher *self; + QrtrNode *node; +} ProcessExistingNodes; + +static gboolean +process_existing_nodes_idle (ProcessExistingNodes *ctx) +{ + handle_qrtr_node_added ( + ctx->self->priv->qrtr_bus, qrtr_node_get_id (ctx->node), ctx->self); + + g_object_unref (ctx->self); + g_object_unref (ctx->node); + g_slice_free (ProcessExistingNodes, ctx); + return G_SOURCE_REMOVE; +} + +static void +process_existing_nodes (MMQrtrBusWatcher *self) +{ + GList *nodes, *l; + QrtrNode *node; + ProcessExistingNodes *ctx; + + nodes = qrtr_bus_peek_nodes (self->priv->qrtr_bus); + for (l = nodes; l; l = g_list_next (l)) { + node = l->data; + ctx = g_slice_new (ProcessExistingNodes); + ctx->self = g_object_ref (self); + ctx->node = g_object_ref (node); + g_idle_add ((GSourceFunc) process_existing_nodes_idle, ctx); + } +} + +static void +qrtr_bus_ready (GObject *source, + GAsyncResult *res, + GTask *task) +{ + MMQrtrBusWatcher *self; + GError *error = NULL; + + self = g_task_get_source_object (task); + + self->priv->qrtr_bus = qrtr_bus_new_finish (res, &error); + if (!self->priv->qrtr_bus) { + g_task_return_error (task, error); + g_object_unref (task); + return; + } + + /* Listen for bus events */ + self->priv->node_added_id = g_signal_connect (self->priv->qrtr_bus, + QRTR_BUS_SIGNAL_NODE_ADDED, + G_CALLBACK (handle_qrtr_node_added), + self); + self->priv->node_removed_id = g_signal_connect (self->priv->qrtr_bus, + QRTR_BUS_SIGNAL_NODE_REMOVED, + G_CALLBACK (handle_qrtr_node_removed), + self); + + process_existing_nodes (self); + + g_task_return_boolean (task, TRUE); + g_object_unref (task); +} + +void +mm_qrtr_bus_watcher_start (MMQrtrBusWatcher *self, + GAsyncReadyCallback callback, + gpointer user_data) +{ + GTask *task; + + task = g_task_new (self, NULL, callback, user_data); + + qrtr_bus_new (0, /* disable initial lookup wait */ + NULL, + (GAsyncReadyCallback)qrtr_bus_ready, + task); +} + +/*****************************************************************************/ + +MMQrtrBusWatcher * +mm_qrtr_bus_watcher_new (void) +{ + return MM_QRTR_BUS_WATCHER (g_object_new (MM_TYPE_QRTR_BUS_WATCHER, NULL)); +} + +static void +mm_qrtr_bus_watcher_init (MMQrtrBusWatcher *self) +{ + /* Initialize opaque pointer to private data */ + self->priv = G_TYPE_INSTANCE_GET_PRIVATE (self, + MM_TYPE_QRTR_BUS_WATCHER, + MMQrtrBusWatcherPrivate); + /* Setup internal lists of device and node objects */ + self->priv->nodes = g_hash_table_new_full (g_direct_hash, + g_direct_equal, + NULL, + (GDestroyNotify) g_object_unref); +} + +static void +finalize (GObject *object) +{ + MMQrtrBusWatcher *self = MM_QRTR_BUS_WATCHER (object); + + g_hash_table_destroy (self->priv->nodes); + + if (self->priv->node_added_id) + g_signal_handler_disconnect (self->priv->qrtr_bus, self->priv->node_added_id); + if (self->priv->node_removed_id) + g_signal_handler_disconnect (self->priv->qrtr_bus, self->priv->node_removed_id); + g_clear_object (&self->priv->qrtr_bus); + + G_OBJECT_CLASS (mm_qrtr_bus_watcher_parent_class)->finalize (object); +} + +static void +log_object_iface_init (MMLogObjectInterface *iface) +{ + iface->build_id = log_object_build_id; +} + +static void +mm_qrtr_bus_watcher_class_init (MMQrtrBusWatcherClass *klass) +{ + GObjectClass *object_class = G_OBJECT_CLASS (klass); + + g_type_class_add_private (object_class, sizeof (MMQrtrBusWatcherPrivate)); + + /* Virtual methods */ + object_class->finalize = finalize; + + /** + * QrtrBusWatcher::qrtr-device-added: + * @self: the #QrtrBusWatcher + * @node: the node ID of the modem that is added + * + * The ::qrtr-device-added signal is emitted when a new qrtr modem is + * available on the QRTR bus. + */ + signals[QRTR_DEVICE_ADDED] = g_signal_new (MM_QRTR_BUS_WATCHER_DEVICE_ADDED, + G_OBJECT_CLASS_TYPE (object_class), + G_SIGNAL_RUN_FIRST, + G_STRUCT_OFFSET (MMQrtrBusWatcherClass, qrtr_device_added), + NULL, /* accumulator */ + NULL, /* accumulator data */ + g_cclosure_marshal_generic, + G_TYPE_NONE, + 1, + G_TYPE_UINT); + + /** + * QrtrBusWatcher::qrtr-device-removed: + * @self: the #QrtrBusWatcher + * @node: the node ID of the modem that is removed + * + * The ::qrtr-device-removed signal is emitted when a qrtr modem deregisters + * all services from the QRTR bus. + */ + signals[QRTR_DEVICE_REMOVED] = g_signal_new (MM_QRTR_BUS_WATCHER_DEVICE_REMOVED, + G_OBJECT_CLASS_TYPE (object_class), + G_SIGNAL_RUN_FIRST, + G_STRUCT_OFFSET (MMQrtrBusWatcherClass, qrtr_device_removed), + NULL, /* accumulator */ + NULL, /* accumulator data */ + g_cclosure_marshal_generic, + G_TYPE_NONE, + 1, + G_TYPE_UINT); +} diff --git a/src/mm-qrtr-bus-watcher.h b/src/mm-qrtr-bus-watcher.h new file mode 100644 index 00000000..b7275447 --- /dev/null +++ b/src/mm-qrtr-bus-watcher.h @@ -0,0 +1,69 @@ +/* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ +/* + * 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: + * + * Copyright 2020 Google LLC + */ + +#ifndef MM_QRTR_BUS_WATCHER_H +#define MM_QRTR_BUS_WATCHER_H + +#include +#include + +#include + +G_BEGIN_DECLS + +#define MM_TYPE_QRTR_BUS_WATCHER (mm_qrtr_bus_watcher_get_type ()) +#define MM_QRTR_BUS_WATCHER(obj) (G_TYPE_CHECK_INSTANCE_CAST ((obj), MM_TYPE_QRTR_BUS_WATCHER, MMQrtrBusWatcher)) +#define MM_QRTR_BUS_WATCHER_CLASS(klass) (G_TYPE_CHECK_CLASS_CAST ((klass), MM_TYPE_QRTR_BUS_WATCHER, MMQrtrBusWatcherClass)) +#define MM_IS_QRTR_BUS_WATCHER(obj) (G_TYPE_CHECK_INSTANCE_TYPE ((obj), MM_TYPE_QRTR_BUS_WATCHER)) +#define MM_IS_QRTR_BUS_WATCHER_CLASS(klass) (G_TYPE_CHECK_CLASS_TYPE ((klass), MM_TYPE_QRTR_BUS_WATCHER)) +#define MM_QRTR_BUS_WATCHER_GET_CLASS(obj) (G_TYPE_INSTANCE_GET_CLASS ((obj), MM_TYPE_QRTR_BUS_WATCHER, MMQrtrBusWatcherClass)) + +#define MM_QRTR_BUS_WATCHER_DEVICE_ADDED "qrtr-device-added" +#define MM_QRTR_BUS_WATCHER_DEVICE_REMOVED "qrtr-device-removed" + +typedef struct _MMQrtrBusWatcher MMQrtrBusWatcher; +typedef struct _MMQrtrBusWatcherClass MMQrtrBusWatcherClass; +typedef struct _MMQrtrBusWatcherPrivate MMQrtrBusWatcherPrivate; + +struct _MMQrtrBusWatcher { + GObject parent; + MMQrtrBusWatcherPrivate *priv; +}; + +struct _MMQrtrBusWatcherClass { + GObjectClass parent; + + void (* qrtr_device_added) (MMQrtrBusWatcher *bus_watcher); + void (* qrtr_device_removed) (MMQrtrBusWatcher *bus_watcher); +}; + +GType mm_qrtr_bus_watcher_get_type (void); +G_DEFINE_AUTOPTR_CLEANUP_FUNC (MMQrtrBusWatcher, g_object_unref) + +MMQrtrBusWatcher *mm_qrtr_bus_watcher_new (void); + +void mm_qrtr_bus_watcher_start (MMQrtrBusWatcher *self, + GAsyncReadyCallback callback, + gpointer user_data); +gboolean mm_qrtr_bus_watcher_start_finish (MMQrtrBusWatcher *self, + GAsyncResult *res, + GError **error); + +QrtrNode *mm_qrtr_bus_watcher_peek_node (MMQrtrBusWatcher *self, + guint32 node_id); + +G_END_DECLS + +#endif /* MM_QRTR_BUS_WATCHER_H */ -- cgit v1.2.3