Debian Codegraph @

src/libqmi-glib/qmi-endpoint-qrtr.c

raw
1 /* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
2 /*
3 * libqmi-glib -- GLib/GIO based library to control QMI devices
4 *
5 * This library is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU Lesser General Public
7 * License as published by the Free Software Foundation; either
8 * version 2 of the License, or (at your option) any later version.
9 *
10 * This library is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * Lesser General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public
16 * License along with this library; if not, write to the
17 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
18 * Boston, MA 02110-1301 USA.
19 *
20 * Copyright (C) 2019-2020 Eric Caruso <[email protected]>
21 * Copyright (C) 2020 Aleksander Morgado <[email protected]>
22 */
23
24 #include <errno.h>
25 #include <linux/qrtr.h>
26 #include <string.h>
27 #include <sys/socket.h>
28 #include <sys/types.h>
29
30 #include <gmodule.h>
31 #include <gio/gio.h>
32
33 #include <libqrtr-glib.h>
34
35 #include "qmi-endpoint-qrtr.h"
36 #include "qmi-errors.h"
37 #include "qmi-enum-types.h"
38 #include "qmi-flag-types.h"
39 #include "qmi-error-types.h"
40 #include "qmi-message.h"
41
42 #define QMI_MESSAGE_OUTPUT_TLV_RESULT 0x02
43
44 #define QMI_MESSAGE_TLV_ALLOCATION_INFO 0x01
45 #define QMI_MESSAGE_INPUT_TLV_SERVICE 0x01
46
47 #define QMI_MESSAGE_CTL_GET_VERSION_INFO 0x0021
48 #define QMI_MESSAGE_CTL_SYNC 0x0027
49
50 G_DEFINE_TYPE (QmiEndpointQrtr, qmi_endpoint_qrtr, QMI_TYPE_ENDPOINT)
51
52 struct _QmiEndpointQrtrPrivate {
53 QrtrNode *node;
54 guint node_removed_id;
55 gboolean node_removed;
56
57 gboolean endpoint_open;
58 GList *clients;
59 };
60
61 /*****************************************************************************/
62
63 static void
64 add_qmi_message_to_buffer (QmiEndpointQrtr *self,
65 QmiMessage *message)
66 {
67 g_autoptr(GError) error = NULL;
68 const guint8 *raw_message;
69 gsize raw_message_len;
70
71 raw_message = qmi_message_get_raw (message, &raw_message_len, &error);
72 if (!raw_message)
73 g_warning ("[%s] Got malformed QMI message: %s",
74 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
75 else
76 qmi_endpoint_add_message (QMI_ENDPOINT (self), raw_message, raw_message_len);
77 qmi_message_unref (message);
78 }
79
80 /*****************************************************************************/
81
82 #define QRTR_CLIENT_DATA_SERVICE "service"
83 #define QRTR_CLIENT_DATA_CID "cid"
84
85 typedef struct {
86 QmiService service;
87 guint cid;
88 QrtrClient *client;
89 guint client_message_id;
90 } ClientInfo;
91
92 static void
93 client_info_free (ClientInfo *client_info)
94 {
95 g_signal_handler_disconnect (client_info->client, client_info->client_message_id);
96 g_object_unref (client_info->client);
97 g_slice_free (ClientInfo, client_info);
98 }
99
100 static void
101 client_info_list_free (GList *list)
102 {
103 g_list_free_full (list, (GDestroyNotify) client_info_free);
104 }
105
106 static ClientInfo *
107 client_info_lookup (QmiEndpointQrtr *self,
108 QmiService service,
109 guint cid)
110 {
111 GList *l;
112
113 for (l = self->priv->clients; l; l = g_list_next (l)) {
114 ClientInfo *client_info = l->data;
115
116 if ((service == client_info->service) &&
117 (cid == client_info->cid))
118 return client_info;
119 }
120 return NULL;
121 }
122
123 static gint
124 client_info_cmp (const ClientInfo *a,
125 const ClientInfo *b)
126 {
127 if (a->service != b->service)
128 return a->service - b->service;
129 return a->cid - b->cid;
130 }
131
132 static void
133 client_message_cb (QrtrClient *qrtr_client,
134 GByteArray *qrtr_message,
135 QmiEndpointQrtr *self)
136 {
137 QmiMessage *message;
138 QmiService service;
139 guint cid;
140 g_autoptr(GError) error = NULL;
141
142 service = GPOINTER_TO_UINT (g_object_get_data (G_OBJECT (qrtr_client), QRTR_CLIENT_DATA_SERVICE));
143 cid = GPOINTER_TO_UINT (g_object_get_data (G_OBJECT (qrtr_client), QRTR_CLIENT_DATA_CID));
144
145 /* Create a fake QMUX/QRTR header and add this message to the buffer */
146 message = qmi_message_new_from_data (service, cid, qrtr_message, &error);
147 if (!message)
148 g_warning ("[%s] Got malformed QMI message: %s",
149 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
150 else
151 add_qmi_message_to_buffer (self, message);
152 }
153
154 static ClientInfo *
155 client_info_new (QmiEndpointQrtr *self,
156 QmiService service,
157 GError **error)
158 {
159 ClientInfo *client_info;
160 QrtrClient *qrtr_client;
161 GList *l;
162 guint max_cid = 0;
163 guint min_available_cid = 1;
164 guint cid = 0;
165 gint32 port;
166
167 for (l = self->priv->clients; l; l = g_list_next (l)) {
168 client_info = l->data;
169
170 if (service != client_info->service)
171 continue;
172
173 max_cid = client_info->cid;
174 if (min_available_cid == client_info->cid)
175 min_available_cid++;
176 }
177
178 cid = max_cid + 1;
179 if (cid > G_MAXUINT8) {
180 cid = min_available_cid;
181 if (min_available_cid > G_MAXUINT8) {
182 g_set_error (error, QMI_PROTOCOL_ERROR, QMI_PROTOCOL_ERROR_CLIENT_IDS_EXHAUSTED,
183 "Client IDs have been exhausted");
184 return NULL;
185 }
186 }
187
188 port = qrtr_node_lookup_port (self->priv->node, service);
189 if (port < 0) {
190 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_UNSUPPORTED,
191 "Service not supported");
192 return NULL;
193 }
194
195 qrtr_client = qrtr_client_new (self->priv->node, (guint)port, NULL, error);
196 if (!qrtr_client) {
197 g_prefix_error (error, "Couldn't create QRTR client: ");
198 return NULL;
199 }
200
201 /* store service and cid info in the client itself for quicker access */
202 g_object_set_data (G_OBJECT (qrtr_client), QRTR_CLIENT_DATA_SERVICE, GUINT_TO_POINTER (service));
203 g_object_set_data (G_OBJECT (qrtr_client), QRTR_CLIENT_DATA_CID, GUINT_TO_POINTER (cid));
204
205 client_info = g_slice_new0 (ClientInfo);
206 client_info->service = service;
207 client_info->cid = cid;
208 client_info->client = qrtr_client;
209 client_info->client_message_id = g_signal_connect (qrtr_client,
210 QRTR_CLIENT_SIGNAL_MESSAGE,
211 G_CALLBACK (client_message_cb),
212 self);
213
214 self->priv->clients = g_list_insert_sorted (self->priv->clients,
215 client_info,
216 (GCompareFunc)client_info_cmp);
217
218 return client_info;
219 }
220
221 /*****************************************************************************/
222 /* Client info operations */
223
224 static guint
225 allocate_client (QmiEndpointQrtr *self,
226 QmiService service,
227 GError **error)
228 {
229 ClientInfo *client_info;
230
231 if (!self->priv->endpoint_open) {
232 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_WRONG_STATE,
233 "Endpoint is not open");
234 return 0;
235 }
236
237 client_info = client_info_new (self, service, error);
238 if (!client_info)
239 return 0;
240
241 return client_info->cid;
242 }
243
244 static void
245 release_client (QmiEndpointQrtr *self,
246 QmiService service,
247 guint cid)
248 {
249 ClientInfo *client_info;
250
251 client_info = client_info_lookup (self, service, cid);
252 if (!client_info)
253 return;
254
255 self->priv->clients = g_list_remove (self->priv->clients, client_info);
256 client_info_free (client_info);
257 }
258
259 /*****************************************************************************/
260
261 static gboolean
262 construct_alloc_tlv (QmiMessage *message,
263 QmiService service,
264 guint8 client)
265 {
266 gsize init_offset;
267
268 init_offset = qmi_message_tlv_write_init (message,
269 QMI_MESSAGE_TLV_ALLOCATION_INFO,
270 NULL);
271
272 if (qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_ALLOCATE_CID ||
273 qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_RELEASE_CID) {
274 g_assert (service <= G_MAXUINT8);
275 return init_offset &&
276 qmi_message_tlv_write_guint8 (message, service, NULL) &&
277 qmi_message_tlv_write_guint8 (message, client, NULL) &&
278 qmi_message_tlv_write_complete (message, init_offset, NULL);
279 }
280
281 if (qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_INTERNAL_ALLOCATE_CID_QRTR ||
282 qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_INTERNAL_RELEASE_CID_QRTR) {
283 g_assert (service <= G_MAXUINT16);
284 return init_offset &&
285 qmi_message_tlv_write_guint16 (message, QMI_ENDIAN_LITTLE, service, NULL) &&
286 qmi_message_tlv_write_guint8 (message, client, NULL) &&
287 qmi_message_tlv_write_complete (message, init_offset, NULL);
288 }
289
290 g_assert_not_reached ();
291 }
292
293 static void
294 reply_protocol_error (QmiEndpointQrtr *self,
295 QmiMessage *message,
296 QmiProtocolError error)
297 {
298 QmiMessage *response = NULL;
299
300 response = qmi_message_response_new (message, error);
301 if (response)
302 add_qmi_message_to_buffer (self, response);
303 }
304
305 static void
306 handle_alloc_cid (QmiEndpointQrtr *self,
307 QmiMessage *message)
308 {
309 gsize offset = 0;
310 gsize init_offset;
311 QmiService service = QMI_SERVICE_UNKNOWN;
312 guint cid;
313 g_autoptr(QmiMessage) response = NULL;
314 g_autoptr(GError) error = NULL;
315
316 if ((init_offset = qmi_message_tlv_read_init (message, QMI_MESSAGE_TLV_ALLOCATION_INFO, NULL, &error)) == 0) {
317 g_debug ("[%s] error allocating CID: could not parse message: %s",
318 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
319 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_MALFORMED_MESSAGE);
320 return;
321 }
322
323 if (qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_ALLOCATE_CID) {
324 guint8 service_tmp;
325
326 if (!qmi_message_tlv_read_guint8 (message, init_offset, &offset, &service_tmp, &error)) {
327 g_debug ("[%s] error allocating CID: failed to read service: %s",
328 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
329 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_MALFORMED_MESSAGE);
330 return;
331 }
332 service = (QmiService)service_tmp;
333 } else if (qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_INTERNAL_ALLOCATE_CID_QRTR) {
334 guint16 service_tmp;
335
336 if (!qmi_message_tlv_read_guint16 (message, init_offset, &offset, QMI_ENDIAN_LITTLE, &service_tmp, &error)) {
337 g_debug ("[%s] error allocating CID (QRTR): failed to read service: %s",
338 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
339 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_MALFORMED_MESSAGE);
340 return;
341 }
342 service = (QmiService)service_tmp;
343 } else
344 g_assert_not_reached ();
345
346 cid = allocate_client (self, service, &error);
347 if (!cid) {
348 g_debug ("[%s] error allocating CID: %s",
349 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
350 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_INTERNAL);
351 return;
352 }
353
354 response = qmi_message_response_new (message, QMI_PROTOCOL_ERROR_NONE);
355 if (!response)
356 return;
357
358 if (!construct_alloc_tlv (response, service, cid))
359 return;
360
361 add_qmi_message_to_buffer (self, g_steal_pointer (&response));
362 }
363
364 static void
365 handle_release_cid (QmiEndpointQrtr *self,
366 QmiMessage *message)
367 {
368 gsize offset = 0;
369 gsize init_offset;
370 QmiService service;
371 guint8 cid;
372 g_autoptr(QmiMessage) response = NULL;
373 g_autoptr(GError) error = NULL;
374
375 if ((init_offset = qmi_message_tlv_read_init (message, QMI_MESSAGE_TLV_ALLOCATION_INFO, NULL, &error)) == 0) {
376 g_debug ("[%s] error releasing CID: could not parse message: %s",
377 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
378 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_MALFORMED_MESSAGE);
379 return;
380 }
381
382 if (qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_RELEASE_CID) {
383 guint8 service_tmp;
384
385 if (!qmi_message_tlv_read_guint8 (message, init_offset, &offset, &service_tmp, &error)) {
386 g_debug ("[%s] error releasing CID: could not read service: %s",
387 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
388 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_MALFORMED_MESSAGE);
389 return;
390 }
391 service = (QmiService)service_tmp;
392 } else if (qmi_message_get_message_id (message) == QMI_MESSAGE_CTL_INTERNAL_RELEASE_CID_QRTR) {
393 guint16 service_tmp;
394
395 if (!qmi_message_tlv_read_guint16 (message, init_offset, &offset, QMI_ENDIAN_LITTLE, &service_tmp, &error)) {
396 g_debug ("[%s] error releasing CID (QRTR): could not read service: %s",
397 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
398 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_MALFORMED_MESSAGE);
399 return;
400 }
401 service = (QmiService)service_tmp;
402 } else
403 g_assert_not_reached ();
404
405 if (!qmi_message_tlv_read_guint8 (message, init_offset, &offset, &cid, &error)) {
406 g_debug ("[%s] error releasing CID: could not client id: %s",
407 qmi_endpoint_get_name (QMI_ENDPOINT (self)), error->message);
408 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_MALFORMED_MESSAGE);
409 return;
410 }
411
412 release_client (self, service, cid);
413
414 response = qmi_message_response_new (message, QMI_PROTOCOL_ERROR_NONE);
415 if (!response)
416 return;
417
418 if (!construct_alloc_tlv (response, service, cid))
419 return;
420
421 add_qmi_message_to_buffer (self, g_steal_pointer (&response));
422 }
423
424 static void
425 handle_sync (QmiEndpointQrtr *self,
426 QmiMessage *message)
427 {
428 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_NONE);
429 }
430
431 static void
432 unhandled_message (QmiEndpointQrtr *self,
433 QmiMessage *message)
434 {
435 reply_protocol_error (self, message, QMI_PROTOCOL_ERROR_NOT_SUPPORTED);
436 }
437
438 static void
439 handle_ctl_message (QmiEndpointQrtr *self,
440 QmiMessage *message)
441 {
442 switch (qmi_message_get_message_id (message)) {
443 case QMI_MESSAGE_CTL_ALLOCATE_CID:
444 case QMI_MESSAGE_CTL_INTERNAL_ALLOCATE_CID_QRTR:
445 handle_alloc_cid (self, message);
446 break;
447 case QMI_MESSAGE_CTL_RELEASE_CID:
448 case QMI_MESSAGE_CTL_INTERNAL_RELEASE_CID_QRTR:
449 handle_release_cid (self, message);
450 break;
451 case QMI_MESSAGE_CTL_SYNC:
452 handle_sync (self, message);
453 break;
454 case QMI_MESSAGE_CTL_GET_VERSION_INFO:
455 default:
456 unhandled_message (self, message);
457 break;
458 }
459 }
460
461 /*****************************************************************************/
462
463 static gboolean
464 endpoint_open_finish (QmiEndpoint *self,
465 GAsyncResult *res,
466 GError **error)
467 {
468 return g_task_propagate_boolean (G_TASK (res), error);
469 }
470
471 static void
472 endpoint_open (QmiEndpoint *endpoint,
473 gboolean use_proxy,
474 guint timeout,
475 GCancellable *cancellable,
476 GAsyncReadyCallback callback,
477 gpointer user_data)
478 {
479 QmiEndpointQrtr *self = QMI_ENDPOINT_QRTR (endpoint);
480 GTask *task;
481
482 g_assert (!use_proxy);
483
484 task = g_task_new (self, cancellable, callback, user_data);
485
486 if (self->priv->node_removed) {
487 g_task_return_new_error (task,
488 QMI_CORE_ERROR,
489 QMI_CORE_ERROR_FAILED,
490 "Node is not present on bus");
491 g_object_unref (task);
492 return;
493 }
494
495 if (self->priv->endpoint_open) {
496 g_task_return_new_error (task,
497 QMI_CORE_ERROR,
498 QMI_CORE_ERROR_WRONG_STATE,
499 "Already open");
500 g_object_unref (task);
501 return;
502 }
503
504 g_assert (self->priv->clients == NULL);
505 self->priv->endpoint_open = TRUE;
506
507 g_task_return_boolean (task, TRUE);
508 g_object_unref (task);
509 }
510
511 static gboolean
512 endpoint_is_open (QmiEndpoint *self)
513 {
514 return QMI_ENDPOINT_QRTR (self)->priv->endpoint_open;
515 }
516
517 static gboolean
518 endpoint_send (QmiEndpoint *endpoint,
519 QmiMessage *message,
520 guint timeout,
521 GCancellable *cancellable,
522 GError **error)
523 {
524 QmiEndpointQrtr *self = QMI_ENDPOINT_QRTR (endpoint);
525 ClientInfo *client_info;
526 QmiService service;
527 guint cid;
528 gconstpointer raw_message;
529 gsize raw_message_len;
530 g_autoptr(GByteArray) qrtr_message = NULL;
531
532 /* We implement the CTL service here, so divert those messages */
533 service = qmi_message_get_service (message);
534 if (service == QMI_SERVICE_CTL) {
535 handle_ctl_message (self, message);
536 return TRUE;
537 }
538
539 cid = qmi_message_get_client_id (message);
540 client_info = client_info_lookup (self, service, cid);
541 if (!client_info) {
542 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_WRONG_STATE,
543 "Unknown client %u for service %s", cid, qmi_service_get_string (service));
544 return FALSE;
545 }
546
547 /* Build raw QRTR message without QMUX/QRTR header */
548 raw_message = qmi_message_get_data (message, &raw_message_len, error);
549 if (!raw_message) {
550 g_prefix_error (error, "Invalid QMI message: ");
551 return FALSE;
552 }
553 qrtr_message = g_byte_array_sized_new (raw_message_len);
554 g_byte_array_append (qrtr_message, raw_message, raw_message_len);
555
556 return qrtr_client_send (client_info->client,
557 qrtr_message,
558 cancellable,
559 error);
560 }
561
562 /*****************************************************************************/
563
564 static void
565 internal_close (QmiEndpointQrtr *self)
566 {
567 g_clear_pointer (&self->priv->clients, client_info_list_free);
568 self->priv->endpoint_open = FALSE;
569 }
570
571 static gboolean
572 endpoint_close_finish (QmiEndpoint *self,
573 GAsyncResult *res,
574 GError **error)
575 {
576 return g_task_propagate_boolean (G_TASK (res), error);
577 }
578
579 static void
580 endpoint_close (QmiEndpoint *endpoint,
581 guint timeout,
582 GCancellable *cancellable,
583 GAsyncReadyCallback callback,
584 gpointer user_data)
585 {
586 QmiEndpointQrtr *self = QMI_ENDPOINT_QRTR (endpoint);
587 GTask *task;
588
589 task = g_task_new (self, cancellable, callback, user_data);
590
591 internal_close (self);
592
593 g_task_return_boolean (task, TRUE);
594 g_object_unref (task);
595 }
596
597 /*****************************************************************************/
598
599 static void
600 node_removed_cb (QrtrNode *node,
601 QmiEndpointQrtr *self)
602 {
603 self->priv->node_removed = TRUE;
604 g_signal_emit_by_name (QMI_ENDPOINT (self), QMI_ENDPOINT_SIGNAL_HANGUP);
605 }
606
607 QmiEndpointQrtr *
608 qmi_endpoint_qrtr_new (QrtrNode *node)
609 {
610 QmiEndpointQrtr *self;
611 g_autofree gchar *uri = NULL;
612 g_autoptr(GFile) gfile = NULL;
613 g_autoptr(QmiFile) file = NULL;
614
615 if (!node)
616 return NULL;
617
618 uri = qrtr_get_uri_for_node (qrtr_node_get_id (node));
619 gfile = g_file_new_for_uri (uri);
620 file = qmi_file_new (gfile);
621
622 self = g_object_new (QMI_TYPE_ENDPOINT_QRTR,
623 QMI_ENDPOINT_FILE, file,
624 NULL);
625
626 self->priv->node = g_object_ref (node);
627 self->priv->node_removed_id = g_signal_connect (node,
628 QRTR_NODE_SIGNAL_REMOVED,
629 G_CALLBACK (node_removed_cb),
630 self);
631 return self;
632 }
633
634 /*****************************************************************************/
635
636 static void
637 qmi_endpoint_qrtr_init (QmiEndpointQrtr *self)
638 {
639 self->priv = G_TYPE_INSTANCE_GET_PRIVATE ((self),
640 QMI_TYPE_ENDPOINT_QRTR,
641 QmiEndpointQrtrPrivate);
642 }
643
644 static void
645 dispose (GObject *object)
646 {
647 QmiEndpointQrtr *self = QMI_ENDPOINT_QRTR (object);
648
649 internal_close (self);
650
651 if (self->priv->node) {
652 if (self->priv->node_removed_id) {
653 g_signal_handler_disconnect (self->priv->node, self->priv->node_removed_id);
654 self->priv->node_removed_id = 0;
655 }
656 g_clear_object (&self->priv->node);
657 }
658
659 G_OBJECT_CLASS (qmi_endpoint_qrtr_parent_class)->dispose (object);
660 }
661
662 static void
663 qmi_endpoint_qrtr_class_init (QmiEndpointQrtrClass *klass)
664 {
665 GObjectClass *object_class = G_OBJECT_CLASS (klass);
666 QmiEndpointClass *endpoint_class = QMI_ENDPOINT_CLASS (klass);
667
668 g_type_class_add_private (object_class, sizeof (QmiEndpointQrtrPrivate));
669
670 object_class->dispose = dispose;
671
672 endpoint_class->open = endpoint_open;
673 endpoint_class->open_finish = endpoint_open_finish;
674 endpoint_class->is_open = endpoint_is_open;
675 endpoint_class->send = endpoint_send;
676 endpoint_class->close = endpoint_close;
677 endpoint_class->close_finish = endpoint_close_finish;
678 }