Debian Codegraph @

src/libqmi-glib/qmi-message.c

raw
1 /* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
2
3 /*
4 * Copyright (c) 2012 The Chromium OS Authors. All rights reserved.
5 * Use of this source code is governed by a BSD-style license that can be
6 * found in the LICENSE file of the libqmi library.
7 */
8
9 /*
10 * libqmi-glib -- GLib/GIO based library to control QMI devices
11 *
12 * This library is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU Lesser General Public
14 * License as published by the Free Software Foundation; either
15 * version 2 of the License, or (at your option) any later version.
16 *
17 * This library is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
20 * Lesser General Public License for more details.
21 *
22 * You should have received a copy of the GNU Lesser General Public
23 * License along with this library; if not, write to the
24 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
25 * Boston, MA 02110-1301 USA.
26 *
27 * Copyright (C) 2012-2019 Aleksander Morgado <[email protected]>
28 * Copyright (c) 2022 Qualcomm Innovation Center, Inc.
29 */
30
31 #include <glib.h>
32 #include <stdint.h>
33 #include <stdio.h>
34 #include <string.h>
35 #include <endian.h>
36
37 #include "qmi-common.h"
38 #include "qmi-message.h"
39 #include "qmi-helpers.h"
40 #include "qmi-enums-private.h"
41 #include "qmi-enum-types-private.h"
42 #include "qmi-flag-types-private.h"
43 #include "qmi-enum-types.h"
44 #include "qmi-flag-types.h"
45 #include "qmi-error-types.h"
46
47 #include "qmi-ctl.h"
48 #include "qmi-dms.h"
49 #include "qmi-wds.h"
50 #include "qmi-nas.h"
51 #include "qmi-wms.h"
52 #include "qmi-pdc.h"
53 #include "qmi-pds.h"
54 #include "qmi-pbm.h"
55 #include "qmi-uim.h"
56 #include "qmi-oma.h"
57 #include "qmi-wda.h"
58 #include "qmi-voice.h"
59 #include "qmi-loc.h"
60 #include "qmi-qos.h"
61 #include "qmi-gas.h"
62 #include "qmi-gms.h"
63 #include "qmi-dsd.h"
64 #include "qmi-sar.h"
65 #include "qmi-dpm.h"
66 #include "qmi-fox.h"
67 #include "qmi-atr.h"
68 #include "qmi-ssc.h"
69 #include "qmi-ims.h"
70 #include "qmi-imsa.h"
71 #include "qmi-imsp.h"
72
73 #define PACKED __attribute__((packed))
74
75 struct qmux_header {
76 guint16 length;
77 guint8 flags;
78 guint8 service;
79 guint8 client;
80 } PACKED;
81
82 /* This is not a real header in QRTR messages, it is exclusively used within
83 * libqmi to flag messages that have 16-bit service id and would otherwise not
84 * fit in a standard QMUX message. */
85 struct qrtr_header {
86 guint16 length;
87 guint16 service;
88 guint8 client;
89 } PACKED;
90
91 struct control_header {
92 guint8 flags;
93 guint8 transaction;
94 guint16 message;
95 guint16 tlv_length;
96 } PACKED;
97
98 struct service_header {
99 guint8 flags;
100 guint16 transaction;
101 guint16 message;
102 guint16 tlv_length;
103 } PACKED;
104
105 struct tlv {
106 guint8 type;
107 guint16 length;
108 guint8 value[];
109 } PACKED;
110
111 struct control_message {
112 struct control_header header;
113 struct tlv tlv[];
114 } PACKED;
115
116 struct service_message {
117 struct service_header header;
118 struct tlv tlv[];
119 } PACKED;
120
121 struct full_message {
122 guint8 marker;
123 union {
124 struct qmux_header qmux;
125 struct qrtr_header qrtr;
126 } header;
127 union {
128 struct control_message control;
129 struct service_message service;
130 } qmi;
131 } PACKED;
132
133 /* qmux_header and qrtr_header are of the same size, so that the overall
134 * QMI header struct is also always of the same size. This is convenient
135 * when building new QMI messages or ensuring the full header is available,
136 * but it is not a strict requirement. */
137 G_STATIC_ASSERT (sizeof (struct qmux_header) == sizeof (struct qrtr_header));
138
139 #define MESSAGE_IS_QMUX(self) (((struct full_message *)(self->data))->marker == QMI_MESSAGE_QMUX_MARKER)
140 #define MESSAGE_IS_QRTR(self) (((struct full_message *)(self->data))->marker == QMI_MESSAGE_QRTR_MARKER)
141
142 static inline gboolean
143 message_is_control (QmiMessage *self)
144 {
145 return MESSAGE_IS_QMUX (self) ?
146 ((struct full_message *)(self->data))->header.qmux.service == QMI_SERVICE_CTL :
147 ((struct full_message *)(self->data))->header.qrtr.service == QMI_SERVICE_CTL;
148 }
149
150 static inline guint16
151 get_message_length (QmiMessage *self)
152 {
153 return MESSAGE_IS_QMUX (self) ?
154 GUINT16_FROM_LE (((struct full_message *)(self->data))->header.qmux.length) :
155 GUINT16_FROM_LE (((struct full_message *)(self->data))->header.qrtr.length);
156 }
157
158 static inline void
159 set_message_length (QmiMessage *self,
160 guint16 length)
161 {
162 if (MESSAGE_IS_QMUX (self))
163 ((struct full_message *)(self->data))->header.qmux.length = GUINT16_TO_LE (length);
164 else
165 ((struct full_message *)(self->data))->header.qrtr.length = GUINT16_TO_LE (length);
166 }
167
168 static inline guint8
169 get_qmux_flags (QmiMessage *self)
170 {
171 /* QMI_MESSAGE_QRTR_MARKER does not support flags */
172 g_assert (MESSAGE_IS_QMUX (self));
173 return ((struct full_message *)(self->data))->header.qmux.flags;
174 }
175
176 static inline guint8
177 get_qmi_flags (QmiMessage *self)
178 {
179 if (message_is_control (self))
180 return ((struct full_message *)(self->data))->qmi.control.header.flags;
181
182 return ((struct full_message *)(self->data))->qmi.service.header.flags;
183 }
184
185 gboolean
186 qmi_message_is_request (QmiMessage *self)
187 {
188 g_return_val_if_fail (self != NULL, FALSE);
189
190 return (!qmi_message_is_response (self) && !qmi_message_is_indication (self));
191 }
192
193 gboolean
194 qmi_message_is_response (QmiMessage *self)
195 {
196 g_return_val_if_fail (self != NULL, FALSE);
197
198 if (message_is_control (self)) {
199 if (((struct full_message *)(self->data))->qmi.control.header.flags & QMI_CTL_FLAG_RESPONSE)
200 return TRUE;
201 } else {
202 if (((struct full_message *)(self->data))->qmi.service.header.flags & QMI_SERVICE_FLAG_RESPONSE)
203 return TRUE;
204 }
205
206 return FALSE;
207 }
208
209 gboolean
210 qmi_message_is_indication (QmiMessage *self)
211 {
212 g_return_val_if_fail (self != NULL, FALSE);
213
214 if (message_is_control (self)) {
215 if (((struct full_message *)(self->data))->qmi.control.header.flags & QMI_CTL_FLAG_INDICATION)
216 return TRUE;
217 } else {
218 if (((struct full_message *)(self->data))->qmi.service.header.flags & QMI_SERVICE_FLAG_INDICATION)
219 return TRUE;
220 }
221
222 return FALSE;
223 }
224
225 QmiService
226 qmi_message_get_service (QmiMessage *self)
227 {
228 g_return_val_if_fail (self != NULL, QMI_SERVICE_UNKNOWN);
229
230 if (MESSAGE_IS_QMUX (self))
231 return (QmiService)((struct full_message *)(self->data))->header.qmux.service;
232
233 return (QmiService)GUINT16_FROM_LE (((struct full_message *)(self->data))->header.qrtr.service);
234 }
235
236 guint8
237 qmi_message_get_client_id (QmiMessage *self)
238 {
239 g_return_val_if_fail (self != NULL, 0);
240
241 if (MESSAGE_IS_QMUX (self))
242 return ((struct full_message *)(self->data))->header.qmux.client;
243
244 return ((struct full_message *)(self->data))->header.qrtr.client;
245 }
246
247 guint16
248 qmi_message_get_transaction_id (QmiMessage *self)
249 {
250 g_return_val_if_fail (self != NULL, 0);
251
252 if (message_is_control (self))
253 /* note: only 1 byte for transaction in CTL message */
254 return (guint16)((struct full_message *)(self->data))->qmi.control.header.transaction;
255
256 return GUINT16_FROM_LE (((struct full_message *)(self->data))->qmi.service.header.transaction);
257 }
258
259 void
260 qmi_message_set_transaction_id (QmiMessage *self,
261 guint16 transaction_id)
262 {
263 g_return_if_fail (self != NULL);
264
265 if (message_is_control (self))
266 ((struct full_message *)self->data)->qmi.control.header.transaction = (guint8)transaction_id;
267 else
268 ((struct full_message *)self->data)->qmi.service.header.transaction = GUINT16_TO_LE (transaction_id);
269 }
270
271 guint16
272 qmi_message_get_message_id (QmiMessage *self)
273 {
274 g_return_val_if_fail (self != NULL, 0);
275
276 if (message_is_control (self))
277 return GUINT16_FROM_LE (((struct full_message *)(self->data))->qmi.control.header.message);
278
279 return GUINT16_FROM_LE (((struct full_message *)(self->data))->qmi.service.header.message);
280 }
281
282 gsize
283 qmi_message_get_length (QmiMessage *self)
284 {
285 g_return_val_if_fail (self != NULL, 0);
286
287 return self->len;
288 }
289
290 static inline guint16
291 get_all_tlvs_length (QmiMessage *self)
292 {
293 if (message_is_control (self))
294 return GUINT16_FROM_LE (((struct full_message *)(self->data))->qmi.control.header.tlv_length);
295
296 return GUINT16_FROM_LE (((struct full_message *)(self->data))->qmi.service.header.tlv_length);
297 }
298
299 static inline void
300 set_all_tlvs_length (QmiMessage *self,
301 guint16 length)
302 {
303 if (message_is_control (self))
304 ((struct full_message *)(self->data))->qmi.control.header.tlv_length = GUINT16_TO_LE (length);
305 else
306 ((struct full_message *)(self->data))->qmi.service.header.tlv_length = GUINT16_TO_LE (length);
307 }
308
309 static inline struct tlv *
310 qmi_tlv (QmiMessage *self)
311 {
312 if (message_is_control (self))
313 return ((struct full_message *)(self->data))->qmi.control.tlv;
314
315 return ((struct full_message *)(self->data))->qmi.service.tlv;
316 }
317
318 static inline guint8 *
319 qmi_end (QmiMessage *self)
320 {
321 return (guint8 *) self->data + self->len;
322 }
323
324 static inline struct tlv *
325 tlv_next (struct tlv *tlv)
326 {
327 return (struct tlv *)((guint8 *)tlv + sizeof(struct tlv) + GUINT16_FROM_LE (tlv->length));
328 }
329
330 static inline struct tlv *
331 qmi_tlv_first (QmiMessage *self)
332 {
333 if (get_all_tlvs_length (self))
334 return qmi_tlv (self);
335
336 return NULL;
337 }
338
339 static inline struct tlv *
340 qmi_tlv_next (QmiMessage *self,
341 struct tlv *tlv)
342 {
343 struct tlv *end;
344 struct tlv *next;
345
346 end = (struct tlv *) qmi_end (self);
347 next = tlv_next (tlv);
348
349 return (next < end ? next : NULL);
350 }
351
352 /*
353 * Checks the validity of a QMI message.
354 *
355 * In particular, checks:
356 * 1. The message has space for all required headers.
357 * 2. The length of the buffer, the qmux length field, and the QMI tlv_length
358 * field are all consistent.
359 * 3. The TLVs in the message fit exactly in the payload size.
360 *
361 * Returns: %TRUE if the message is valid, %FALSE otherwise.
362 */
363 static gboolean
364 message_check (QmiMessage *self,
365 GError **error)
366 {
367 gsize header_length;
368 gsize message_length;
369 guint8 *end;
370 struct tlv *tlv;
371
372 if (self->len < (1 + sizeof (struct qmux_header))) {
373 g_set_error (error,
374 QMI_CORE_ERROR,
375 QMI_CORE_ERROR_INVALID_MESSAGE,
376 "Buffer length too short for QMUX header (%u < %" G_GSIZE_FORMAT ")",
377 self->len, 1 + sizeof (struct qmux_header));
378 return FALSE;
379 }
380
381 if (!MESSAGE_IS_QMUX (self) && !MESSAGE_IS_QRTR (self)) {
382 g_set_error (error,
383 QMI_CORE_ERROR,
384 QMI_CORE_ERROR_INVALID_MESSAGE,
385 "Unexpected marker (0x%02x)",
386 ((struct full_message *)(self->data))->marker);
387 return FALSE;
388 }
389
390 /*
391 * message length is one byte shorter than buffer length because it
392 * does not include the qmux frame marker.
393 */
394 message_length = get_message_length (self);
395 if (message_length != self->len - 1) {
396 g_set_error (error,
397 QMI_CORE_ERROR,
398 QMI_CORE_ERROR_INVALID_MESSAGE,
399 "Message length and buffer length don't match (%u != %u)",
400 get_message_length (self), self->len - 1);
401 return FALSE;
402 }
403
404 header_length = sizeof (struct qmux_header) + (message_is_control (self) ?
405 sizeof (struct control_header) :
406 sizeof (struct service_header));
407
408 if (message_length < header_length) {
409 g_set_error (error,
410 QMI_CORE_ERROR,
411 QMI_CORE_ERROR_INVALID_MESSAGE,
412 "Message length too short for QMI header (%u < %" G_GSIZE_FORMAT ")",
413 get_message_length (self), header_length);
414 return FALSE;
415 }
416
417 if (message_length - header_length != get_all_tlvs_length (self)) {
418 g_set_error (error,
419 QMI_CORE_ERROR,
420 QMI_CORE_ERROR_INVALID_MESSAGE,
421 "QMUX length and QMI TLV lengths don't match (%u - %" G_GSIZE_FORMAT " != %u)",
422 get_message_length (self), header_length, get_all_tlvs_length (self));
423 return FALSE;
424 }
425
426 end = qmi_end (self);
427 for (tlv = qmi_tlv (self); tlv < (struct tlv *)end; tlv = tlv_next (tlv)) {
428 if (tlv->value > end) {
429 g_set_error (error,
430 QMI_CORE_ERROR,
431 QMI_CORE_ERROR_INVALID_MESSAGE,
432 "TLV header runs over buffer (%p > %p)",
433 tlv->value, end);
434 return FALSE;
435 }
436 if (tlv->value + GUINT16_FROM_LE (tlv->length) > end) {
437 g_set_error (error,
438 QMI_CORE_ERROR,
439 QMI_CORE_ERROR_INVALID_MESSAGE,
440 "TLV value runs over buffer (%p + %u > %p)",
441 tlv->value, GUINT16_FROM_LE (tlv->length), end);
442 return FALSE;
443 }
444 }
445
446 /*
447 * If this assert triggers, one of the if statements in the loop is wrong.
448 * (It shouldn't be reached on malformed QMI messages.)
449 */
450 g_assert (tlv == (struct tlv *)end);
451
452 return TRUE;
453 }
454
455 QmiMessage *
456 qmi_message_new (QmiService service,
457 guint8 client_id,
458 guint16 transaction_id,
459 guint16 message_id)
460 {
461 GByteArray *self;
462 struct full_message *buffer;
463 gsize buffer_len;
464
465 /* Transaction ID in the control service is 8bit only */
466 g_return_val_if_fail ((service != QMI_SERVICE_CTL || transaction_id <= G_MAXUINT8), NULL);
467
468 /* Service must fit in 16 bits */
469 g_return_val_if_fail (service <= G_MAXUINT16, NULL);
470
471 /* Create array with enough size for the QMUX marker, the QMUX header and
472 * the QMI header. Use the qmux_header size for both QMUX and QRTR messages
473 * as they are the same size. */
474 buffer_len = (1 +
475 sizeof (struct qmux_header) +
476 (service == QMI_SERVICE_CTL ? sizeof (struct control_header) : sizeof (struct service_header)));
477
478 /* Create the GByteArray with buffer_len bytes preallocated */
479 self = g_byte_array_sized_new (buffer_len);
480 /* Actually flag as all the buffer_len bytes being used. */
481 g_byte_array_set_size (self, buffer_len);
482
483 buffer = (struct full_message *)(self->data);
484 /* QMI messages of services up to 255 are QMUX compatible */
485 if (service <= G_MAXUINT8) {
486 buffer->marker = QMI_MESSAGE_QMUX_MARKER;
487 buffer->header.qmux.flags = 0;
488 buffer->header.qmux.service = (guint8) service;
489 buffer->header.qmux.client = client_id;
490 } else if (service <= G_MAXUINT16) {
491 buffer->marker = QMI_MESSAGE_QRTR_MARKER;
492 buffer->header.qrtr.service = (guint16) service;
493 buffer->header.qrtr.client = client_id;
494 } else
495 g_assert_not_reached ();
496
497 if (service == QMI_SERVICE_CTL) {
498 buffer->qmi.control.header.flags = 0;
499 buffer->qmi.control.header.transaction = (guint8)transaction_id;
500 buffer->qmi.control.header.message = GUINT16_TO_LE (message_id);
501 } else {
502 buffer->qmi.service.header.flags = 0;
503 buffer->qmi.service.header.transaction = GUINT16_TO_LE (transaction_id);
504 buffer->qmi.service.header.message = GUINT16_TO_LE (message_id);
505 }
506
507 /* Update length fields. */
508 set_message_length (self, buffer_len - 1); /* marker not included in length */
509 set_all_tlvs_length (self, 0);
510
511 /* We shouldn't create invalid empty messages */
512 g_assert (message_check (self, NULL));
513
514 return (QmiMessage *)self;
515 }
516
517 QmiMessage *
518 qmi_message_new_from_data (QmiService service,
519 guint8 client_id,
520 GByteArray *qmi_data,
521 GError **error)
522 {
523 g_autoptr(GByteArray) self = NULL;
524 struct full_message *buffer;
525 gsize buffer_len;
526 gsize message_len;
527
528 /* Service must fit in 16 bits */
529 g_return_val_if_fail (service <= G_MAXUINT16, NULL);
530
531 /* Create array with enough size for the QMUX marker and QMUX header, and
532 * with enough room to copy the rest of the message into */
533 if (service == QMI_SERVICE_CTL) {
534 message_len = sizeof (struct control_header) +
535 GUINT16_FROM_LE (((struct control_message *)(qmi_data->data))->header.tlv_length);
536 } else {
537 message_len = sizeof (struct service_header) +
538 GUINT16_FROM_LE (((struct service_message *)(qmi_data->data))->header.tlv_length);
539 }
540
541 /* Use the size of qmux_header for both QMUX and QRTR as they are the same */
542 buffer_len = (1 + sizeof (struct qmux_header) + message_len);
543
544 /* Create the GByteArray with buffer_len bytes preallocated */
545 self = g_byte_array_sized_new (buffer_len);
546 g_byte_array_set_size (self, buffer_len);
547
548 /* Set up fake QMUX header */
549 buffer = (struct full_message *)(self->data);
550 if (service <= G_MAXUINT8) {
551 buffer->marker = QMI_MESSAGE_QMUX_MARKER;
552 buffer->header.qmux.length = GUINT16_TO_LE (buffer_len - 1);
553 buffer->header.qmux.flags = 0;
554 buffer->header.qmux.service = (guint8) service;
555 buffer->header.qmux.client = client_id;
556 } else if (service <= G_MAXUINT16) {
557 buffer->marker = QMI_MESSAGE_QRTR_MARKER;
558 buffer->header.qrtr.length = GUINT16_TO_LE (buffer_len - 1);
559 buffer->header.qrtr.service = (guint16) service;
560 buffer->header.qrtr.client = client_id;
561 } else
562 g_assert_not_reached ();
563
564 /* Move bytes from the qmi_data array to the newly created message */
565 memcpy (&buffer->qmi, qmi_data->data, message_len);
566 g_byte_array_remove_range (qmi_data, 0, message_len);
567
568 /* Check input message validity as soon as we create the QmiMessage */
569 if (!message_check (self, error))
570 return NULL;
571
572 return (QmiMessage *) g_steal_pointer (&self);
573 }
574
575 QmiMessage *
576 qmi_message_response_new (QmiMessage *request,
577 QmiProtocolError error)
578 {
579 QmiMessage *response;
580 gsize tlv_offset;
581
582 response = qmi_message_new (qmi_message_get_service (request),
583 qmi_message_get_client_id (request),
584 qmi_message_get_transaction_id (request),
585 qmi_message_get_message_id (request));
586
587 /* Set sender type flag */
588 if (qmi_message_get_service (request) <= G_MAXUINT8)
589 ((struct full_message *)(((GByteArray *)response)->data))->header.qmux.flags = 0x80;
590
591 /* Set the response flag */
592 if (message_is_control (request))
593 ((struct full_message *)(((GByteArray *)response)->data))->qmi.control.header.flags |= QMI_CTL_FLAG_RESPONSE;
594 else
595 ((struct full_message *)(((GByteArray *)response)->data))->qmi.service.header.flags |= QMI_SERVICE_FLAG_RESPONSE;
596
597 /* Add result TLV, should never fail */
598 tlv_offset = qmi_message_tlv_write_init (response, 0x02, NULL);
599 qmi_message_tlv_write_guint16 (response, QMI_ENDIAN_LITTLE, (error != QMI_PROTOCOL_ERROR_NONE), NULL);
600 qmi_message_tlv_write_guint16 (response, QMI_ENDIAN_LITTLE, error, NULL);
601 qmi_message_tlv_write_complete (response, tlv_offset, NULL);
602
603 /* We shouldn't create invalid response messages */
604 g_assert (message_check (response, NULL));
605
606 return response;
607 }
608
609 QmiMessage *
610 qmi_message_ref (QmiMessage *self)
611 {
612 g_return_val_if_fail (self != NULL, NULL);
613
614 return (QmiMessage *)g_byte_array_ref (self);
615 }
616
617 void
618 qmi_message_unref (QmiMessage *self)
619 {
620 g_return_if_fail (self != NULL);
621
622 g_byte_array_unref (self);
623 }
624
625 guint8
626 qmi_message_get_marker (QmiMessage *self)
627 {
628 g_return_val_if_fail (self != NULL, 0x00);
629
630 return ((struct full_message *)(self->data))->marker;
631 }
632
633 const guint8 *
634 qmi_message_get_raw (QmiMessage *self,
635 gsize *length,
636 GError **error)
637 {
638 g_return_val_if_fail (self != NULL, NULL);
639 g_return_val_if_fail (length != NULL, NULL);
640
641 *length = self->len;
642 return self->data;
643 }
644
645 const guint8 *
646 qmi_message_get_data (QmiMessage *self,
647 gsize *length,
648 GError **error)
649 {
650 g_return_val_if_fail (self != NULL, NULL);
651 g_return_val_if_fail (length != NULL, NULL);
652
653 if (message_is_control (self))
654 *length = sizeof (struct control_header);
655 else
656 *length = sizeof (struct service_header);
657 *length += get_all_tlvs_length (self);
658 return (guint8 *)(&((struct full_message *)(self->data))->qmi);
659 }
660
661 /*****************************************************************************/
662 /* TLV builder & writer */
663
664 static gboolean
665 tlv_error_if_write_overflow (QmiMessage *self,
666 gsize len,
667 GError **error)
668 {
669 /* Check for overflow of message size. */
670 if (self->len + len > G_MAXUINT16) {
671 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_TLV_TOO_LONG,
672 "Writing TLV would overflow");
673 return FALSE;
674 }
675
676 return TRUE;
677 }
678
679 static struct tlv *
680 tlv_get_header (QmiMessage *self,
681 gsize init_offset)
682 {
683 g_assert (init_offset <= self->len);
684 return (struct tlv *)(&self->data[init_offset]);
685 }
686
687 gsize
688 qmi_message_tlv_write_init (QmiMessage *self,
689 guint8 type,
690 GError **error)
691 {
692 gsize init_offset;
693 struct tlv *tlv;
694
695 g_return_val_if_fail (self != NULL, 0);
696 g_return_val_if_fail (self->len > 0, 0);
697
698 /* Check for overflow of message size. Note that a valid TLV will at least
699 * have 1 byte of value. */
700 if (!tlv_error_if_write_overflow (self, sizeof (struct tlv) + 1, error))
701 return 0;
702
703 /* Store where exactly we started adding the TLV */
704 init_offset = self->len;
705
706 /* Resize buffer to fit the TLV header */
707 g_byte_array_set_size (self, self->len + sizeof (struct tlv));
708
709 /* Write the TLV header */
710 tlv = tlv_get_header (self, init_offset);
711 tlv->type = type;
712 tlv->length = 0; /* Correct value will be set in complete() */
713
714 return init_offset;
715 }
716
717 void
718 qmi_message_tlv_write_reset (QmiMessage *self,
719 gsize tlv_offset)
720 {
721 g_return_if_fail (self != NULL);
722
723 g_byte_array_set_size (self, tlv_offset);
724 }
725
726 gboolean
727 qmi_message_tlv_write_complete (QmiMessage *self,
728 gsize tlv_offset,
729 GError **error)
730 {
731 gsize tlv_length;
732 struct tlv *tlv;
733
734 g_return_val_if_fail (self != NULL, FALSE);
735 g_return_val_if_fail (self->len >= (tlv_offset + sizeof (struct tlv)), FALSE);
736
737 /* A TLV without content is actually not an error, e.g. TLV strings with no
738 * data are totally valid. */
739 tlv_length = self->len - tlv_offset;
740
741 /* Update length fields. */
742 tlv = tlv_get_header (self, tlv_offset);
743 tlv->length = GUINT16_TO_LE (tlv_length - sizeof (struct tlv));
744 set_message_length (self, (guint16)(get_message_length (self) + tlv_length));
745 set_all_tlvs_length (self, (guint16)(get_all_tlvs_length (self) + tlv_length));
746
747 /* Make sure we didn't break anything. */
748 g_assert (message_check (self, NULL));
749
750 return TRUE;
751 }
752
753 gboolean
754 qmi_message_tlv_write_guint8 (QmiMessage *self,
755 guint8 in,
756 GError **error)
757 {
758 g_return_val_if_fail (self != NULL, FALSE);
759
760 /* Check for overflow of message size */
761 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
762 return FALSE;
763
764 g_byte_array_append (self, &in, sizeof (in));
765 return TRUE;
766 }
767
768 gboolean
769 qmi_message_tlv_write_gint8 (QmiMessage *self,
770 gint8 in,
771 GError **error)
772 {
773 g_return_val_if_fail (self != NULL, FALSE);
774
775 /* Check for overflow of message size */
776 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
777 return FALSE;
778
779 g_byte_array_append (self, (guint8 *)&in, sizeof (in));
780 return TRUE;
781 }
782
783 gboolean
784 qmi_message_tlv_write_guint16 (QmiMessage *self,
785 QmiEndian endian,
786 guint16 in,
787 GError **error)
788 {
789 guint16 tmp;
790
791 g_return_val_if_fail (self != NULL, FALSE);
792
793 /* Check for overflow of message size */
794 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
795 return FALSE;
796
797 tmp = (endian == QMI_ENDIAN_BIG ? GUINT16_TO_BE (in) : GUINT16_TO_LE (in));
798 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
799 return TRUE;
800 }
801
802 gboolean
803 qmi_message_tlv_write_gint16 (QmiMessage *self,
804 QmiEndian endian,
805 gint16 in,
806 GError **error)
807 {
808 gint16 tmp;
809
810 g_return_val_if_fail (self != NULL, FALSE);
811
812 /* Check for overflow of message size */
813 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
814 return FALSE;
815
816 tmp = (endian == QMI_ENDIAN_BIG ? GINT16_TO_BE (in) : GINT16_TO_LE (in));
817 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
818 return TRUE;
819 }
820
821 gboolean
822 qmi_message_tlv_write_guint32 (QmiMessage *self,
823 QmiEndian endian,
824 guint32 in,
825 GError **error)
826 {
827 guint32 tmp;
828
829 g_return_val_if_fail (self != NULL, FALSE);
830
831 /* Check for overflow of message size */
832 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
833 return FALSE;
834
835 tmp = (endian == QMI_ENDIAN_BIG ? GUINT32_TO_BE (in) : GUINT32_TO_LE (in));
836 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
837 return TRUE;
838 }
839
840 gboolean
841 qmi_message_tlv_write_gint32 (QmiMessage *self,
842 QmiEndian endian,
843 gint32 in,
844 GError **error)
845 {
846 gint32 tmp;
847
848 g_return_val_if_fail (self != NULL, FALSE);
849
850 /* Check for overflow of message size */
851 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
852 return FALSE;
853
854 tmp = (endian == QMI_ENDIAN_BIG ? GINT32_TO_BE (in) : GINT32_TO_LE (in));
855 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
856 return TRUE;
857 }
858
859 gboolean
860 qmi_message_tlv_write_guint64 (QmiMessage *self,
861 QmiEndian endian,
862 guint64 in,
863 GError **error)
864 {
865 guint64 tmp;
866
867 g_return_val_if_fail (self != NULL, FALSE);
868
869 /* Check for overflow of message size */
870 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
871 return FALSE;
872
873 tmp = (endian == QMI_ENDIAN_BIG ? GUINT64_TO_BE (in) : GUINT64_TO_LE (in));
874 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
875 return TRUE;
876 }
877
878 gboolean
879 qmi_message_tlv_write_gint64 (QmiMessage *self,
880 QmiEndian endian,
881 gint64 in,
882 GError **error)
883 {
884 gint64 tmp;
885
886 g_return_val_if_fail (self != NULL, FALSE);
887
888 /* Check for overflow of message size */
889 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
890 return FALSE;
891
892 tmp = (endian == QMI_ENDIAN_BIG ? GINT64_TO_BE (in) : GINT64_TO_LE (in));
893 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
894 return TRUE;
895 }
896
897 gboolean
898 qmi_message_tlv_write_sized_guint (QmiMessage *self,
899 guint n_bytes,
900 QmiEndian endian,
901 guint64 in,
902 GError **error)
903 {
904 guint64 tmp;
905 goffset offset;
906
907 g_return_val_if_fail (self != NULL, FALSE);
908 g_return_val_if_fail (n_bytes <= 8, FALSE);
909
910 /* Check for overflow of message size */
911 if (!tlv_error_if_write_overflow (self, n_bytes, error))
912 return FALSE;
913
914 tmp = (endian == QMI_ENDIAN_BIG ? GUINT64_TO_BE (in) : GUINT64_TO_LE (in));
915
916 /* Update buffer size */
917 offset = self->len;
918 g_byte_array_set_size (self, self->len + n_bytes);
919
920 /* In Little Endian, we read the bytes from the beginning of the buffer */
921 if (endian == QMI_ENDIAN_LITTLE) {
922 memcpy (&self->data[offset], &tmp, n_bytes);
923 }
924 /* In Big Endian, we read the bytes from the end of the buffer */
925 else {
926 guint8 tmp_buffer[8];
927
928 memcpy (&tmp_buffer[0], &tmp, 8);
929 memcpy (&self->data[offset], &tmp_buffer[8 - n_bytes], n_bytes);
930 }
931
932 return TRUE;
933 }
934
935 gboolean
936 qmi_message_tlv_write_gfloat (QmiMessage *self,
937 QmiEndian endian,
938 gfloat in,
939 GError **error)
940 {
941 gfloat tmp;
942
943 g_return_val_if_fail (self != NULL, FALSE);
944
945 /* Check for overflow of message size */
946 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
947 return FALSE;
948
949 tmp = (endian == QMI_ENDIAN_BIG ? QMI_GFLOAT_TO_BE (in) : QMI_GFLOAT_TO_LE (in));
950 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
951 return TRUE;
952 }
953
954 gboolean
955 qmi_message_tlv_write_gdouble (QmiMessage *self,
956 QmiEndian endian,
957 gdouble in,
958 GError **error)
959 {
960 gdouble tmp;
961
962 g_return_val_if_fail (self != NULL, FALSE);
963
964 /* Check for overflow of message size */
965 if (!tlv_error_if_write_overflow (self, sizeof (in), error))
966 return FALSE;
967
968 tmp = (endian == QMI_ENDIAN_BIG ? QMI_GDOUBLE_TO_BE (in) : QMI_GDOUBLE_TO_LE (in));
969 g_byte_array_append (self, (guint8 *)&tmp, sizeof (tmp));
970 return TRUE;
971 }
972
973 gboolean
974 qmi_message_tlv_write_string (QmiMessage *self,
975 guint8 n_size_prefix_bytes,
976 const gchar *in,
977 gssize in_length,
978 GError **error)
979 {
980 gsize len;
981
982 g_return_val_if_fail (self != NULL, FALSE);
983 g_return_val_if_fail (in != NULL, FALSE);
984 g_return_val_if_fail (n_size_prefix_bytes <= 2, FALSE);
985
986 len = (in_length < 0 ? strlen (in) : (gsize) in_length);
987
988 /* Write size prefix first */
989 switch (n_size_prefix_bytes) {
990 case 0:
991 break;
992 case 1:
993 if (len > G_MAXUINT8) {
994 g_set_error (error,
995 QMI_CORE_ERROR,
996 QMI_CORE_ERROR_INVALID_ARGS,
997 "String too long for a 1 byte size prefix: %" G_GSIZE_FORMAT, len);
998 return FALSE;
999 }
1000 if (!qmi_message_tlv_write_guint8 (self, (guint8) len, error)) {
1001 g_prefix_error (error, "Cannot append string 1 byte size prefix");
1002 return FALSE;
1003 }
1004 break;
1005 case 2:
1006 if (len > G_MAXUINT16) {
1007 g_set_error (error,
1008 QMI_CORE_ERROR,
1009 QMI_CORE_ERROR_INVALID_ARGS,
1010 "String too long for a 2 byte size prefix: %" G_GSIZE_FORMAT, len);
1011 return FALSE;
1012 }
1013 if (!qmi_message_tlv_write_guint16 (self, QMI_ENDIAN_LITTLE, (guint16) len, error)) {
1014 g_prefix_error (error, "Cannot append string 2 byte size prefix");
1015 return FALSE;
1016 }
1017 break;
1018 default:
1019 g_assert_not_reached ();
1020 }
1021
1022 /* Check for overflow of string size */
1023 if (!tlv_error_if_write_overflow (self, len, error))
1024 return FALSE;
1025
1026 g_byte_array_append (self, (guint8 *)in, len);
1027 return TRUE;
1028 }
1029
1030 /*****************************************************************************/
1031 /* TLV reader */
1032
1033 gsize
1034 qmi_message_tlv_read_init (QmiMessage *self,
1035 guint8 type,
1036 guint16 *out_tlv_length,
1037 GError **error)
1038 {
1039 struct tlv *tlv;
1040 guint16 tlv_length;
1041
1042 g_return_val_if_fail (self != NULL, 0);
1043 g_return_val_if_fail (self->len > 0, 0);
1044
1045 for (tlv = qmi_tlv_first (self); tlv; tlv = qmi_tlv_next (self, tlv)) {
1046 if (tlv->type == type)
1047 break;
1048 }
1049
1050 if (!tlv) {
1051 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_TLV_NOT_FOUND,
1052 "TLV 0x%02X not found", type);
1053 return 0;
1054 }
1055
1056 tlv_length = GUINT16_FROM_LE (tlv->length);
1057
1058 if (((guint8 *) tlv_next (tlv)) > ((guint8 *) qmi_end (self))) {
1059 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_TLV_TOO_LONG,
1060 "Invalid length for TLV 0x%02X: %" G_GUINT16_FORMAT, type, tlv_length);
1061 return 0;
1062 }
1063
1064 if (out_tlv_length)
1065 *out_tlv_length = tlv_length;
1066
1067 return (((guint8 *)tlv) - self->data);
1068 }
1069
1070 static const guint8 *
1071 tlv_error_if_read_overflow (QmiMessage *self,
1072 gsize tlv_offset,
1073 gsize offset,
1074 gsize len,
1075 GError **error)
1076 {
1077 const guint8 *ptr;
1078 struct tlv *tlv;
1079
1080 tlv = (struct tlv *) &(self->data[tlv_offset]);
1081 ptr = ((guint8 *)tlv) + sizeof (struct tlv) + offset;
1082
1083 if (((guint8 *)(ptr + len) > (guint8 *)tlv_next (tlv)) ||
1084 ((guint8 *)(ptr + len) > (guint8 *)qmi_end (self))) {
1085 g_set_error (error,
1086 QMI_CORE_ERROR,
1087 QMI_CORE_ERROR_TLV_TOO_LONG,
1088 "Reading TLV would overflow");
1089 return NULL;
1090 }
1091
1092 return ptr;
1093 }
1094
1095 gboolean
1096 qmi_message_tlv_read_guint8 (QmiMessage *self,
1097 gsize tlv_offset,
1098 gsize *offset,
1099 guint8 *out,
1100 GError **error)
1101 {
1102 const guint8 *ptr;
1103
1104 g_return_val_if_fail (self != NULL, FALSE);
1105 g_return_val_if_fail (offset != NULL, FALSE);
1106 g_return_val_if_fail (out != NULL, FALSE);
1107
1108 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1109 return FALSE;
1110
1111 *offset = *offset + 1;
1112 *out = *ptr;
1113 return TRUE;
1114 }
1115
1116 gboolean
1117 qmi_message_tlv_read_gint8 (QmiMessage *self,
1118 gsize tlv_offset,
1119 gsize *offset,
1120 gint8 *out,
1121 GError **error)
1122 {
1123 const guint8 *ptr;
1124
1125 g_return_val_if_fail (self != NULL, FALSE);
1126 g_return_val_if_fail (offset != NULL, FALSE);
1127 g_return_val_if_fail (out != NULL, FALSE);
1128
1129 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1130 return FALSE;
1131
1132 *out = (gint8)(*ptr);
1133 *offset = *offset + 1;
1134 return TRUE;
1135 }
1136
1137 gboolean
1138 qmi_message_tlv_read_guint16 (QmiMessage *self,
1139 gsize tlv_offset,
1140 gsize *offset,
1141 QmiEndian endian,
1142 guint16 *out,
1143 GError **error)
1144 {
1145 const guint8 *ptr;
1146
1147 g_return_val_if_fail (self != NULL, FALSE);
1148 g_return_val_if_fail (offset != NULL, FALSE);
1149 g_return_val_if_fail (out != NULL, FALSE);
1150
1151 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1152 return FALSE;
1153
1154 memcpy (out, ptr, 2);
1155 if (endian == QMI_ENDIAN_BIG)
1156 *out = GUINT16_FROM_BE (*out);
1157 else
1158 *out = GUINT16_FROM_LE (*out);
1159 *offset = *offset + 2;
1160 return TRUE;
1161 }
1162
1163 gboolean
1164 qmi_message_tlv_read_gint16 (QmiMessage *self,
1165 gsize tlv_offset,
1166 gsize *offset,
1167 QmiEndian endian,
1168 gint16 *out,
1169 GError **error)
1170 {
1171 const guint8 *ptr;
1172
1173 g_return_val_if_fail (self != NULL, FALSE);
1174 g_return_val_if_fail (offset != NULL, FALSE);
1175 g_return_val_if_fail (out != NULL, FALSE);
1176
1177 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1178 return FALSE;
1179
1180 memcpy (out, ptr, 2);
1181 if (endian == QMI_ENDIAN_BIG)
1182 *out = GUINT16_FROM_BE (*out);
1183 else
1184 *out = GUINT16_FROM_LE (*out);
1185 *offset = *offset + 2;
1186 return TRUE;
1187 }
1188
1189 gboolean
1190 qmi_message_tlv_read_guint32 (QmiMessage *self,
1191 gsize tlv_offset,
1192 gsize *offset,
1193 QmiEndian endian,
1194 guint32 *out,
1195 GError **error)
1196 {
1197 const guint8 *ptr;
1198
1199 g_return_val_if_fail (self != NULL, FALSE);
1200 g_return_val_if_fail (offset != NULL, FALSE);
1201 g_return_val_if_fail (out != NULL, FALSE);
1202
1203 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1204 return FALSE;
1205
1206 memcpy (out, ptr, 4);
1207 if (endian == QMI_ENDIAN_BIG)
1208 *out = GUINT32_FROM_BE (*out);
1209 else
1210 *out = GUINT32_FROM_LE (*out);
1211 *offset = *offset + 4;
1212 return TRUE;
1213 }
1214
1215 gboolean
1216 qmi_message_tlv_read_gint32 (QmiMessage *self,
1217 gsize tlv_offset,
1218 gsize *offset,
1219 QmiEndian endian,
1220 gint32 *out,
1221 GError **error)
1222 {
1223 const guint8 *ptr;
1224
1225 g_return_val_if_fail (self != NULL, FALSE);
1226 g_return_val_if_fail (offset != NULL, FALSE);
1227 g_return_val_if_fail (out != NULL, FALSE);
1228
1229 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1230 return FALSE;
1231
1232 memcpy (out, ptr, 4);
1233 if (endian == QMI_ENDIAN_BIG)
1234 *out = GINT32_FROM_BE (*out);
1235 else
1236 *out = GINT32_FROM_LE (*out);
1237 *offset = *offset + 4;
1238 return TRUE;
1239 }
1240
1241 gboolean
1242 qmi_message_tlv_read_guint64 (QmiMessage *self,
1243 gsize tlv_offset,
1244 gsize *offset,
1245 QmiEndian endian,
1246 guint64 *out,
1247 GError **error)
1248 {
1249 const guint8 *ptr;
1250
1251 g_return_val_if_fail (self != NULL, FALSE);
1252 g_return_val_if_fail (offset != NULL, FALSE);
1253 g_return_val_if_fail (out != NULL, FALSE);
1254
1255 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1256 return FALSE;
1257
1258 memcpy (out, ptr, 8);
1259 if (endian == QMI_ENDIAN_BIG)
1260 *out = GUINT64_FROM_BE (*out);
1261 else
1262 *out = GUINT64_FROM_LE (*out);
1263 *offset = *offset + 8;
1264 return TRUE;
1265 }
1266
1267 gboolean
1268 qmi_message_tlv_read_gint64 (QmiMessage *self,
1269 gsize tlv_offset,
1270 gsize *offset,
1271 QmiEndian endian,
1272 gint64 *out,
1273 GError **error)
1274 {
1275 const guint8 *ptr;
1276
1277 g_return_val_if_fail (self != NULL, FALSE);
1278 g_return_val_if_fail (offset != NULL, FALSE);
1279 g_return_val_if_fail (out != NULL, FALSE);
1280
1281 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, sizeof (*out), error)))
1282 return FALSE;
1283
1284 memcpy (out, ptr, 8);
1285 if (endian == QMI_ENDIAN_BIG)
1286 *out = GINT64_FROM_BE (*out);
1287 else
1288 *out = GINT64_FROM_LE (*out);
1289 *offset = *offset + 8;
1290 return TRUE;
1291 }
1292
1293 gboolean
1294 qmi_message_tlv_read_sized_guint (QmiMessage *self,
1295 gsize tlv_offset,
1296 gsize *offset,
1297 guint n_bytes,
1298 QmiEndian endian,
1299 guint64 *out,
1300 GError **error)
1301 {
1302 const guint8 *ptr;
1303
1304 g_return_val_if_fail (self != NULL, FALSE);
1305 g_return_val_if_fail (offset != NULL, FALSE);
1306 g_return_val_if_fail (out != NULL, FALSE);
1307 g_return_val_if_fail (n_bytes <= 8, FALSE);
1308
1309 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, n_bytes, error)))
1310 return FALSE;
1311
1312 *out = 0;
1313
1314 /* In Little Endian, we copy the bytes to the beginning of the output
1315 * buffer. */
1316 if (endian == QMI_ENDIAN_LITTLE) {
1317 memcpy (out, ptr, n_bytes);
1318 *out = GUINT64_FROM_LE (*out);
1319 }
1320 /* In Big Endian, we copy the bytes to the end of the output buffer */
1321 else {
1322 guint8 tmp[8] = { 0 };
1323
1324 memcpy (&tmp[8 - n_bytes], ptr, n_bytes);
1325 memcpy (out, &tmp[0], 8);
1326 *out = GUINT64_FROM_BE (*out);
1327 }
1328
1329 *offset = *offset + n_bytes;
1330 return TRUE;
1331 }
1332
1333 gboolean
1334 qmi_message_tlv_read_gfloat_endian (QmiMessage *self,
1335 gsize tlv_offset,
1336 gsize *offset,
1337 QmiEndian endian,
1338 gfloat *out,
1339 GError **error)
1340 {
1341 const guint8 *ptr;
1342
1343 g_return_val_if_fail (self != NULL, FALSE);
1344 g_return_val_if_fail (offset != NULL, FALSE);
1345 g_return_val_if_fail (out != NULL, FALSE);
1346
1347 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, 4, error)))
1348 return FALSE;
1349
1350 memcpy (out, ptr, 4);
1351 if (endian == QMI_ENDIAN_BIG)
1352 *out = QMI_GFLOAT_FROM_BE (*out);
1353 else
1354 *out = QMI_GFLOAT_FROM_LE (*out);
1355 *offset = *offset + 4;
1356 return TRUE;
1357 }
1358
1359 gboolean
1360 qmi_message_tlv_read_gdouble (QmiMessage *self,
1361 gsize tlv_offset,
1362 gsize *offset,
1363 QmiEndian endian,
1364 gdouble *out,
1365 GError **error)
1366 {
1367 const guint8 *ptr;
1368
1369 g_return_val_if_fail (self != NULL, FALSE);
1370 g_return_val_if_fail (offset != NULL, FALSE);
1371 g_return_val_if_fail (out != NULL, FALSE);
1372
1373 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, 8, error)))
1374 return FALSE;
1375
1376 /* Yeah, do this for now */
1377 memcpy (out, ptr, 8);
1378 if (endian == QMI_ENDIAN_BIG)
1379 *out = QMI_GDOUBLE_FROM_BE (*out);
1380 else
1381 *out = QMI_GDOUBLE_FROM_LE (*out);
1382 *offset = *offset + 8;
1383 return TRUE;
1384 }
1385
1386 gboolean
1387 qmi_message_tlv_read_string (QmiMessage *self,
1388 gsize tlv_offset,
1389 gsize *offset,
1390 guint8 n_size_prefix_bytes,
1391 guint16 max_size,
1392 gchar **out,
1393 GError **error)
1394 {
1395 const guint8 *ptr;
1396 guint16 string_length;
1397 guint16 valid_string_length;
1398
1399 g_return_val_if_fail (self != NULL, FALSE);
1400 g_return_val_if_fail (offset != NULL, FALSE);
1401 g_return_val_if_fail (out != NULL, FALSE);
1402 g_return_val_if_fail (n_size_prefix_bytes <= 2, FALSE);
1403
1404 switch (n_size_prefix_bytes) {
1405 case 0: {
1406 struct tlv *tlv;
1407
1408 if (!tlv_error_if_read_overflow (self, tlv_offset, *offset, 0, error))
1409 return FALSE;
1410
1411 /* If no length prefix given, read the remaining TLV buffer into a string */
1412 tlv = (struct tlv *) &(self->data[tlv_offset]);
1413 string_length = (GUINT16_FROM_LE (tlv->length) - *offset);
1414 break;
1415 }
1416 case 1: {
1417 guint8 string_length_8;
1418
1419 if (!qmi_message_tlv_read_guint8 (self, tlv_offset, offset, &string_length_8, error))
1420 return FALSE;
1421 string_length = (guint16) string_length_8;
1422 break;
1423 }
1424 case 2:
1425 if (!qmi_message_tlv_read_guint16 (self, tlv_offset, offset, QMI_ENDIAN_LITTLE, &string_length, error))
1426 return FALSE;
1427 break;
1428 default:
1429 g_assert_not_reached ();
1430 }
1431
1432 if (string_length == 0) {
1433 *out = g_strdup ("");
1434 return TRUE;
1435 }
1436
1437 if (max_size > 0 && string_length > max_size)
1438 valid_string_length = max_size;
1439 else
1440 valid_string_length = string_length;
1441
1442 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, valid_string_length, error)))
1443 return FALSE;
1444
1445 /* Perform a quick UTF-8 validation check first. This check isn't perfect,
1446 * because there may be GSM-7 encoded strings that are valid UTF-8 as well,
1447 * but hey, the strings read using this method should all really be ASCII-7
1448 * and we're trying to do our best to overcome modem firmware problems...
1449 */
1450 if (qmi_helpers_string_utf8_validate_printable (ptr, valid_string_length)) {
1451 *out = g_malloc (valid_string_length + 1);
1452 memcpy (*out, ptr, valid_string_length);
1453 (*out)[valid_string_length] = '\0';
1454 } else {
1455 /* Otherwise, attempt GSM-7 */
1456 *out =qmi_helpers_string_utf8_from_gsm7 (ptr, valid_string_length);
1457 if (*out == NULL) {
1458 /* Otherwise, attempt UCS-2 */
1459 *out = qmi_helpers_string_utf8_from_ucs2le (ptr, valid_string_length);
1460 if (*out == NULL) {
1461 /* Otherwise, error */
1462 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_INVALID_DATA, "invalid string");
1463 return FALSE;
1464 }
1465 }
1466 }
1467
1468 *offset = (*offset + string_length);
1469 return TRUE;
1470 }
1471
1472 gboolean
1473 qmi_message_tlv_read_fixed_size_string (QmiMessage *self,
1474 gsize tlv_offset,
1475 gsize *offset,
1476 guint16 string_length,
1477 gchar *out,
1478 GError **error)
1479 {
1480 const guint8 *ptr;
1481 const gchar *end = NULL;
1482
1483 g_return_val_if_fail (self != NULL, FALSE);
1484 g_return_val_if_fail (offset != NULL, FALSE);
1485 g_return_val_if_fail (out != NULL, FALSE);
1486
1487 if (string_length == 0)
1488 return TRUE;
1489
1490 if (!(ptr = tlv_error_if_read_overflow (self, tlv_offset, *offset, string_length, error)))
1491 return FALSE;
1492
1493 /* full string valid? */
1494 if (g_utf8_validate ((const gchar *)ptr, string_length, &end)) {
1495 memcpy (out, ptr, string_length);
1496 *offset = (*offset + string_length);
1497 return TRUE;
1498 }
1499
1500 /* partial string valid? */
1501 if (end && end > (const gchar *)ptr) {
1502 /* copy only the valid bytes */
1503 memcpy (out, ptr, end - (const gchar *)ptr);
1504 /* but update offset with the full expected length */
1505 *offset = (*offset + string_length);
1506 return TRUE;
1507 }
1508
1509 /* Empty string? */
1510 if (!ptr[0]) {
1511 /* terminator at start of string */
1512 *out = '\0';
1513 /* but update offset with the full expected length */
1514 *offset = (*offset + string_length);
1515 return TRUE;
1516 }
1517
1518 g_set_error (error, QMI_CORE_ERROR, QMI_CORE_ERROR_INVALID_DATA, "invalid string");
1519 return FALSE;
1520 }
1521
1522 guint16
1523 qmi_message_tlv_read_remaining_size (QmiMessage *self,
1524 gsize tlv_offset,
1525 gsize offset)
1526 {
1527 struct tlv *tlv;
1528
1529 g_return_val_if_fail (self != NULL, FALSE);
1530
1531 tlv = (struct tlv *) &(self->data[tlv_offset]);
1532
1533 g_warn_if_fail (GUINT16_FROM_LE (tlv->length) >= offset);
1534 return (GUINT16_FROM_LE (tlv->length) >= offset ? (GUINT16_FROM_LE (tlv->length) - offset) : 0);
1535 }
1536
1537 /*****************************************************************************/
1538
1539 const guint8 *
1540 qmi_message_get_raw_tlv (QmiMessage *self,
1541 guint8 type,
1542 guint16 *length)
1543 {
1544 struct tlv *tlv;
1545
1546 g_return_val_if_fail (self != NULL, NULL);
1547 g_return_val_if_fail (length != NULL, NULL);
1548
1549 for (tlv = qmi_tlv_first (self); tlv; tlv = qmi_tlv_next (self, tlv)) {
1550 if (tlv->type == type) {
1551 *length = GUINT16_FROM_LE (tlv->length);
1552 return (guint8 *)&(tlv->value[0]);
1553 }
1554 }
1555
1556 return NULL;
1557 }
1558
1559 void
1560 qmi_message_foreach_raw_tlv (QmiMessage *self,
1561 QmiMessageForeachRawTlvFn func,
1562 gpointer user_data)
1563 {
1564 struct tlv *tlv;
1565
1566 g_return_if_fail (self != NULL);
1567 g_return_if_fail (func != NULL);
1568
1569 for (tlv = qmi_tlv_first (self); tlv; tlv = qmi_tlv_next (self, tlv)) {
1570 func (tlv->type,
1571 (const guint8 *)tlv->value,
1572 (gsize)(GUINT16_FROM_LE (tlv->length)),
1573 user_data);
1574 }
1575 }
1576
1577 gboolean
1578 qmi_message_add_raw_tlv (QmiMessage *self,
1579 guint8 type,
1580 const guint8 *raw,
1581 gsize length,
1582 GError **error)
1583 {
1584 size_t tlv_len;
1585 struct tlv *tlv;
1586
1587 g_return_val_if_fail (self != NULL, FALSE);
1588 g_return_val_if_fail (raw != NULL, FALSE);
1589 g_return_val_if_fail (length > 0, FALSE);
1590
1591 /* Find length of new TLV */
1592 tlv_len = sizeof (struct tlv) + length;
1593
1594 /* Check for overflow of message size. */
1595 if (get_message_length (self) + tlv_len > G_MAXUINT16) {
1596 g_set_error (error,
1597 QMI_CORE_ERROR,
1598 QMI_CORE_ERROR_TLV_TOO_LONG,
1599 "TLV to add is too long");
1600 return FALSE;
1601 }
1602
1603 /* Resize buffer. */
1604 g_byte_array_set_size (self, self->len + tlv_len);
1605
1606 /* Fill in new TLV. */
1607 tlv = (struct tlv *)(qmi_end (self) - tlv_len);
1608 tlv->type = type;
1609 tlv->length = GUINT16_TO_LE (length);
1610 memcpy (tlv->value, raw, length);
1611
1612 /* Update length fields. */
1613 set_message_length (self, (guint16)(get_message_length (self) + tlv_len));
1614 set_all_tlvs_length (self, (guint16)(get_all_tlvs_length (self) + tlv_len));
1615
1616 /* Make sure we didn't break anything. */
1617 g_assert (message_check (self, NULL));
1618
1619 return TRUE;
1620 }
1621
1622 QmiMessage *
1623 qmi_message_new_from_raw (GByteArray *raw,
1624 GError **error)
1625 {
1626 GByteArray *self;
1627 gsize message_len;
1628
1629 g_return_val_if_fail (raw != NULL, NULL);
1630
1631 if (MESSAGE_IS_QMUX (raw))
1632 message_len = GUINT16_FROM_LE (((struct full_message *)raw->data)->header.qmux.length);
1633 else
1634 message_len = GUINT16_FROM_LE (((struct full_message *)raw->data)->header.qrtr.length);
1635
1636 /* If we didn't even read the QMUX header (comes after the 1-byte marker),
1637 * leave */
1638 if (raw->len < (sizeof (struct qrtr_header) + 1))
1639 return NULL;
1640
1641 /* We need to have read the length reported by the QMUX header (plus the
1642 * initial 1-byte marker) */
1643 if (raw->len < (message_len + 1))
1644 return NULL;
1645
1646 /* Ok, so we should have all the data available already */
1647 self = g_byte_array_sized_new (message_len + 1);
1648 g_byte_array_prepend (self, raw->data, message_len + 1);
1649
1650 /* We got a complete QMI message, remove from input buffer */
1651 g_byte_array_remove_range (raw, 0, self->len);
1652
1653 /* Check input message validity as soon as we create the QmiMessage */
1654 if (!message_check (self, error)) {
1655 /* Yes, we lose the whole message here */
1656 qmi_message_unref (self);
1657 return NULL;
1658 }
1659
1660 return (QmiMessage *)self;
1661 }
1662
1663 gchar *
1664 qmi_message_get_tlv_printable (QmiMessage *self,
1665 const gchar *line_prefix,
1666 guint8 type,
1667 const guint8 *raw,
1668 gsize raw_length)
1669 {
1670 gchar *printable;
1671 gchar *value_hex;
1672
1673 g_return_val_if_fail (self != NULL, NULL);
1674 g_return_val_if_fail (line_prefix != NULL, NULL);
1675 g_return_val_if_fail (raw != NULL, NULL);
1676
1677 value_hex = qmi_common_str_hex (raw, raw_length, ':');
1678 printable = g_strdup_printf ("%sTLV:\n"
1679 "%s type = 0x%02x\n"
1680 "%s length = %" G_GSIZE_FORMAT "\n"
1681 "%s value = %s\n",
1682 line_prefix,
1683 line_prefix, type,
1684 line_prefix, raw_length,
1685 line_prefix, value_hex);
1686 g_free (value_hex);
1687 return printable;
1688 }
1689
1690 static gchar *
1691 get_generic_printable (QmiMessage *self,
1692 const gchar *line_prefix)
1693 {
1694 GString *printable;
1695 struct tlv *tlv;
1696
1697 printable = g_string_new ("");
1698
1699 g_string_append_printf (printable,
1700 "%s message = (0x%04x)\n",
1701 line_prefix, qmi_message_get_message_id (self));
1702
1703 for (tlv = qmi_tlv_first (self); tlv; tlv = qmi_tlv_next (self, tlv)) {
1704 gchar *printable_tlv;
1705
1706 printable_tlv = qmi_message_get_tlv_printable (self,
1707 line_prefix,
1708 tlv->type,
1709 tlv->value,
1710 GUINT16_FROM_LE (tlv->length));
1711 g_string_append (printable, printable_tlv);
1712 g_free (printable_tlv);
1713 }
1714
1715 return g_string_free (printable, FALSE);
1716 }
1717
1718 gchar *
1719 qmi_message_get_printable_full (QmiMessage *self,
1720 QmiMessageContext *context,
1721 const gchar *line_prefix)
1722 {
1723 GString *printable;
1724 gchar *qmi_flags_str;
1725 gchar *contents;
1726
1727 g_return_val_if_fail (self != NULL, NULL);
1728
1729 if (!line_prefix)
1730 line_prefix = "";
1731
1732 printable = g_string_new ("");
1733 if (MESSAGE_IS_QMUX (self)) {
1734 g_string_append_printf (printable,
1735 "%sQMUX:\n"
1736 "%s length = %u\n"
1737 "%s flags = 0x%02x\n"
1738 "%s service = \"%s\"\n"
1739 "%s client = %u\n",
1740 line_prefix,
1741 line_prefix, get_message_length (self),
1742 line_prefix, get_qmux_flags (self),
1743 line_prefix, qmi_service_get_string (qmi_message_get_service (self)),
1744 line_prefix, qmi_message_get_client_id (self));
1745 } else if (MESSAGE_IS_QRTR (self)) {
1746 g_string_append_printf (printable,
1747 "%sQRTR:\n"
1748 "%s length = %u\n"
1749 "%s service = \"%s\"\n"
1750 "%s client = %u\n",
1751 line_prefix,
1752 line_prefix, get_message_length (self),
1753 line_prefix, qmi_service_get_string (qmi_message_get_service (self)),
1754 line_prefix, qmi_message_get_client_id (self));
1755 } else {
1756 g_warn_if_reached ();
1757 return g_string_free (printable, FALSE);
1758 }
1759
1760 if (qmi_message_get_service (self) == QMI_SERVICE_CTL)
1761 qmi_flags_str = qmi_ctl_flag_build_string_from_mask (get_qmi_flags (self));
1762 else
1763 qmi_flags_str = qmi_service_flag_build_string_from_mask (get_qmi_flags (self));
1764
1765 g_string_append_printf (printable,
1766 "%sQMI:\n"
1767 "%s flags = \"%s\"\n"
1768 "%s transaction = %u\n"
1769 "%s tlv_length = %u\n",
1770 line_prefix,
1771 line_prefix, qmi_flags_str,
1772 line_prefix, qmi_message_get_transaction_id (self),
1773 line_prefix, get_all_tlvs_length (self));
1774 g_free (qmi_flags_str);
1775
1776 contents = NULL;
1777 switch (qmi_message_get_service (self)) {
1778 case QMI_SERVICE_CTL:
1779 #if defined HAVE_QMI_SERVICE_CTL
1780 contents = __qmi_message_ctl_get_printable (self, context, line_prefix);
1781 #endif
1782 break;
1783 case QMI_SERVICE_DMS:
1784 #if defined HAVE_QMI_SERVICE_DMS
1785 contents = __qmi_message_dms_get_printable (self, context, line_prefix);
1786 #endif
1787 break;
1788 case QMI_SERVICE_WDS:
1789 #if defined HAVE_QMI_SERVICE_WDS
1790 contents = __qmi_message_wds_get_printable (self, context, line_prefix);
1791 #endif
1792 break;
1793 case QMI_SERVICE_NAS:
1794 #if defined HAVE_QMI_SERVICE_NAS
1795 contents = __qmi_message_nas_get_printable (self, context, line_prefix);
1796 #endif
1797 break;
1798 case QMI_SERVICE_WMS:
1799 #if defined HAVE_QMI_SERVICE_WMS
1800 contents = __qmi_message_wms_get_printable (self, context, line_prefix);
1801 #endif
1802 break;
1803 case QMI_SERVICE_PDC:
1804 #if defined HAVE_QMI_SERVICE_PDC
1805 contents = __qmi_message_pdc_get_printable (self, context, line_prefix);
1806 #endif
1807 break;
1808 case QMI_SERVICE_PDS:
1809 #if defined HAVE_QMI_SERVICE_PDS
1810 contents = __qmi_message_pds_get_printable (self, context, line_prefix);
1811 #endif
1812 break;
1813 case QMI_SERVICE_PBM:
1814 #if defined HAVE_QMI_SERVICE_PBM
1815 contents = __qmi_message_pbm_get_printable (self, context, line_prefix);
1816 #endif
1817 break;
1818 case QMI_SERVICE_UIM:
1819 #if defined HAVE_QMI_SERVICE_UIM
1820 contents = __qmi_message_uim_get_printable (self, context, line_prefix);
1821 #endif
1822 break;
1823 case QMI_SERVICE_OMA:
1824 #if defined HAVE_QMI_SERVICE_OMA
1825 contents = __qmi_message_oma_get_printable (self, context, line_prefix);
1826 #endif
1827 break;
1828 case QMI_SERVICE_GAS:
1829 #if defined HAVE_QMI_SERVICE_GAS
1830 contents = __qmi_message_gas_get_printable (self, context, line_prefix);
1831 #endif
1832 break;
1833 case QMI_SERVICE_GMS:
1834 #if defined HAVE_QMI_SERVICE_GMS
1835 contents = __qmi_message_gms_get_printable (self, context, line_prefix);
1836 #endif
1837 break;
1838 case QMI_SERVICE_WDA:
1839 #if defined HAVE_QMI_SERVICE_WDA
1840 contents = __qmi_message_wda_get_printable (self, context, line_prefix);
1841 #endif
1842 break;
1843 case QMI_SERVICE_VOICE:
1844 #if defined HAVE_QMI_SERVICE_VOICE
1845 contents = __qmi_message_voice_get_printable (self, context, line_prefix);
1846 #endif
1847 break;
1848 case QMI_SERVICE_LOC:
1849 #if defined HAVE_QMI_SERVICE_LOC
1850 contents = __qmi_message_loc_get_printable (self, context, line_prefix);
1851 #endif
1852 break;
1853 case QMI_SERVICE_QOS:
1854 #if defined HAVE_QMI_SERVICE_QOS
1855 contents = __qmi_message_qos_get_printable (self, context, line_prefix);
1856 #endif
1857 break;
1858 case QMI_SERVICE_DSD:
1859 #if defined HAVE_QMI_SERVICE_DSD
1860 contents = __qmi_message_dsd_get_printable (self, context, line_prefix);
1861 #endif
1862 break;
1863 case QMI_SERVICE_DPM:
1864 #if defined HAVE_QMI_SERVICE_DPM
1865 contents = __qmi_message_dpm_get_printable (self, context, line_prefix);
1866 #endif
1867 break;
1868 case QMI_SERVICE_FOX:
1869 #if defined HAVE_QMI_SERVICE_FOX
1870 contents = __qmi_message_fox_get_printable (self, context, line_prefix);
1871 #endif
1872 break;
1873 case QMI_SERVICE_ATR:
1874 #if defined HAVE_QMI_SERVICE_ATR
1875 contents = __qmi_message_atr_get_printable (self, context, line_prefix);
1876 #endif
1877 break;
1878 case QMI_SERVICE_IMSP:
1879 #if defined HAVE_QMI_SERVICE_IMSP
1880 contents = __qmi_message_imsp_get_printable (self, context, line_prefix);
1881 #endif
1882 break;
1883 case QMI_SERVICE_IMSA:
1884 #if defined HAVE_QMI_SERVICE_IMSA
1885 contents = __qmi_message_imsa_get_printable (self, context, line_prefix);
1886 #endif
1887 break;
1888 case QMI_SERVICE_IMSDCM:
1889 #if defined HAVE_QMI_SERVICE_IMSDCM
1890 contents = __qmi_message_imsdcm_get_printable (self, context, line_prefix);
1891 #endif
1892 break;
1893 case QMI_SERVICE_IMS:
1894 #if defined HAVE_QMI_SERVICE_IMS
1895 contents = __qmi_message_ims_get_printable (self, context, line_prefix);
1896 #endif
1897 break;
1898 case QMI_SERVICE_SSC:
1899 #if defined HAVE_QMI_SERVICE_SSC
1900 contents = __qmi_message_ssc_get_printable (self, context, line_prefix);
1901 #endif
1902 break;
1903
1904 case QMI_SERVICE_UNKNOWN:
1905 g_assert_not_reached ();
1906
1907 case QMI_SERVICE_AUTH:
1908 case QMI_SERVICE_AT:
1909 case QMI_SERVICE_CAT2:
1910 case QMI_SERVICE_QCHAT:
1911 case QMI_SERVICE_RMTFS:
1912 case QMI_SERVICE_TEST:
1913 case QMI_SERVICE_SAR:
1914 case QMI_SERVICE_ADC:
1915 case QMI_SERVICE_CSD:
1916 case QMI_SERVICE_MFS:
1917 case QMI_SERVICE_TIME:
1918 case QMI_SERVICE_TS:
1919 case QMI_SERVICE_TMD:
1920 case QMI_SERVICE_SAP:
1921 case QMI_SERVICE_TSYNC:
1922 case QMI_SERVICE_RFSA:
1923 case QMI_SERVICE_CSVT:
1924 case QMI_SERVICE_QCMAP:
1925 case QMI_SERVICE_IMSVT:
1926 case QMI_SERVICE_COEX:
1927 case QMI_SERVICE_STX:
1928 case QMI_SERVICE_BIT:
1929 case QMI_SERVICE_IMSRTP:
1930 case QMI_SERVICE_RFRPE:
1931 case QMI_SERVICE_SSCTL:
1932 case QMI_SERVICE_CAT:
1933 case QMI_SERVICE_RMS:
1934 case QMI_SERVICE_FOTA:
1935 default:
1936 break;
1937 }
1938
1939 if (!contents)
1940 contents = get_generic_printable (self, line_prefix);
1941 g_string_append (printable, contents);
1942 g_free (contents);
1943
1944 return g_string_free (printable, FALSE);
1945 }
1946
1947 gboolean
1948 __qmi_message_is_abortable (QmiMessage *self,
1949 QmiMessageContext *context)
1950 {
1951 switch (qmi_message_get_service (self)) {
1952 case QMI_SERVICE_WDS:
1953 #if defined HAVE_QMI_SERVICE_WDS
1954 return __qmi_message_wds_is_abortable (self, context);
1955 #else
1956 return FALSE;
1957 #endif
1958 case QMI_SERVICE_NAS:
1959 #if defined HAVE_QMI_SERVICE_NAS
1960 return __qmi_message_nas_is_abortable (self, context);
1961 #else
1962 return FALSE;
1963 #endif
1964
1965 case QMI_SERVICE_UNKNOWN:
1966 g_assert_not_reached ();
1967
1968 case QMI_SERVICE_CTL:
1969 case QMI_SERVICE_DMS:
1970 case QMI_SERVICE_WMS:
1971 case QMI_SERVICE_PDS:
1972 case QMI_SERVICE_PBM:
1973 case QMI_SERVICE_UIM:
1974 case QMI_SERVICE_OMA:
1975 case QMI_SERVICE_GAS:
1976 case QMI_SERVICE_WDA:
1977 case QMI_SERVICE_LOC:
1978 case QMI_SERVICE_AUTH:
1979 case QMI_SERVICE_AT:
1980 case QMI_SERVICE_CAT2:
1981 case QMI_SERVICE_QCHAT:
1982 case QMI_SERVICE_RMTFS:
1983 case QMI_SERVICE_TEST:
1984 case QMI_SERVICE_SAR:
1985 case QMI_SERVICE_IMS:
1986 case QMI_SERVICE_ADC:
1987 case QMI_SERVICE_CSD:
1988 case QMI_SERVICE_MFS:
1989 case QMI_SERVICE_TIME:
1990 case QMI_SERVICE_TS:
1991 case QMI_SERVICE_TMD:
1992 case QMI_SERVICE_SAP:
1993 case QMI_SERVICE_TSYNC:
1994 case QMI_SERVICE_RFSA:
1995 case QMI_SERVICE_CSVT:
1996 case QMI_SERVICE_QCMAP:
1997 case QMI_SERVICE_IMSP:
1998 case QMI_SERVICE_IMSVT:
1999 case QMI_SERVICE_IMSA:
2000 case QMI_SERVICE_COEX:
2001 case QMI_SERVICE_STX:
2002 case QMI_SERVICE_BIT:
2003 case QMI_SERVICE_IMSRTP:
2004 case QMI_SERVICE_RFRPE:
2005 case QMI_SERVICE_SSCTL:
2006 case QMI_SERVICE_DPM:
2007 case QMI_SERVICE_CAT:
2008 case QMI_SERVICE_RMS:
2009 case QMI_SERVICE_FOTA:
2010 case QMI_SERVICE_GMS:
2011 case QMI_SERVICE_VOICE:
2012 case QMI_SERVICE_PDC:
2013 case QMI_SERVICE_DSD:
2014 case QMI_SERVICE_QOS:
2015 case QMI_SERVICE_FOX:
2016 case QMI_SERVICE_ATR:
2017 case QMI_SERVICE_SSC:
2018 case QMI_SERVICE_IMSDCM:
2019 default:
2020 return FALSE;
2021 }
2022 }