Debian Codegraph @

utils/qmitest.c

raw
1 /* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
2 /*
3 * This program is free software; you can redistribute it and/or modify
4 * it under the terms of the GNU General Public License version 2 and
5 * only version 2 as published by the Free Software Foundation.
6 *
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
11 *
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software
14 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
15 * 02110-1301, USA.
16 *
17 * Copyright (C) 2011 - 2012 Red Hat, Inc.
18 * Copyright (C) 2010 - 2012 Google, Inc.
19 */
20
21 #include <glib.h>
22 #include <string.h>
23 #include <errno.h>
24 #include <sys/ioctl.h>
25 #include <fcntl.h>
26 #include <termios.h>
27 #include <unistd.h>
28 #include <stdlib.h>
29 #include <sys/types.h>
30
31 typedef u_int16_t u16;
32 typedef u_int8_t u8;
33 typedef u_int32_t u32;
34 typedef u_int64_t u64;
35
36 typedef unsigned int qbool;
37 #ifndef TRUE
38 #define TRUE 1
39 #endif
40 #ifndef FALSE
41 #define FALSE 0
42 #endif
43
44 /***********************************************************/
45
46 static void
47 print_buf (const char *detail, const char *buf, size_t len)
48 {
49 int i = 0, z;
50 qbool newline = FALSE, indent = FALSE;
51 char *f;
52 guint flen;
53
54 f = g_strdup_printf ("%s (%zu) ", detail, len);
55 flen = strlen (f);
56 g_print ("%s", f);
57 for (i = 0; i < len; i++) {
58 if (indent) {
59 z = flen;
60 while (z--)
61 g_print (" ");
62 indent = FALSE;
63 }
64 g_print ("%02x ", buf[i] & 0xFF);
65 if (((i + 1) % 16) == 0) {
66 g_print ("\n");
67 newline = TRUE;
68 indent = TRUE;
69 } else
70 newline = FALSE;
71 }
72
73 if (!newline)
74 g_print ("\n");
75 }
76
77 /**************************************************************/
78
79 typedef enum {
80 QMI_SVC_CTL = 0x00,
81 QMI_SVC_WDS = 0x01,
82 QMI_SVC_DMS = 0x02,
83 QMI_SVC_NAS = 0x03,
84 QMI_SVC_QOS = 0x04,
85 QMI_SVC_WMS = 0x05,
86 QMI_SVC_PDS = 0x06,
87 QMI_SVC_AUTH = 0x07,
88 QMI_SVC_AT = 0x08,
89 QMI_SVC_VOICE = 0x09,
90 QMI_SVC_CAT2 = 0x0A,
91 QMI_SVC_UIM = 0x0B,
92 QMI_SVC_PBM = 0x0C,
93 QMI_SVC_LOC = 0x10,
94 QMI_SVC_SAR = 0x11,
95 QMI_SVC_RMTFS = 0x14,
96 QMI_SVC_CAT = 0xE0,
97 QMI_SVC_RMS = 0xE1,
98 QMI_SVC_OMA = 0xE2
99 } qmi_service_type;
100
101 struct qmux {
102 u8 tf; /* always 1 */
103 u16 len;
104 u8 ctrl;
105 u8 service;
106 u8 qmicid;
107 } __attribute__((__packed__));
108
109 struct qmi_tlv_status {
110 u16 status;
111 u16 error;
112 } __attribute__((__packed__));
113
114 struct getcid_req {
115 struct qmux header;
116 u8 req;
117 u8 tid;
118 u16 msgid;
119 u16 tlvsize;
120 u8 service;
121 u16 size;
122 u8 qmisvc;
123 } __attribute__((__packed__));
124
125 struct releasecid_req {
126 struct qmux header;
127 u8 req;
128 u8 tid;
129 u16 msgid;
130 u16 tlvsize;
131 u8 rlscid;
132 u16 size;
133 u16 cid;
134 } __attribute__((__packed__));
135
136 struct version_info_req {
137 struct qmux header;
138 u8 req;
139 u8 tid;
140 u16 msgid;
141 u16 tlvsize;
142 } __attribute__((__packed__));
143
144 struct qmi_ctl_version_info_list_service {
145 u8 service_type; /* QMI_SVC_xxx */
146 u16 major_version;
147 u16 minor_version;
148 } __attribute__((__packed__));
149
150 struct qmi_tlv_ctl_version_info_list {
151 u8 count;
152 struct qmi_ctl_version_info_list_service services[0];
153 }__attribute__((__packed__));
154
155 struct seteventreport_req {
156 struct qmux header;
157 u8 req;
158 u16 tid;
159 u16 msgid;
160 u16 tlvsize;
161 u8 reportchanrate;
162 u16 size;
163 u8 period;
164 u32 mask;
165 } __attribute__((__packed__));
166
167 struct getpkgsrvcstatus_req {
168 struct qmux header;
169 u8 req;
170 u16 tid;
171 u16 msgid;
172 u16 tlvsize;
173 } __attribute__((__packed__));
174
175 struct getmeid_req {
176 struct qmux header;
177 u8 req;
178 u16 tid;
179 u16 msgid;
180 u16 tlvsize;
181 } __attribute__((__packed__));
182
183 struct qmiwds_stats {
184 u32 txok;
185 u32 rxok;
186 u32 txerr;
187 u32 rxerr;
188 u32 txofl;
189 u32 rxofl;
190 u64 txbytesok;
191 u64 rxbytesok;
192 qbool linkstate;
193 qbool reconfigure;
194 };
195
196 struct nas_signal_req {
197 struct qmux header;
198 u8 req;
199 u16 tid;
200 u16 msgid;
201 u16 tlvsize;
202 } __attribute__((__packed__));
203
204 const size_t qmux_size = sizeof(struct qmux);
205
206 static void
207 qmux_fill(struct qmux *qmux, u8 service, u16 cid, u16 size)
208 {
209 qmux->tf = 1;
210 qmux->len = size - 1;
211 qmux->ctrl = 0;
212 qmux->service = cid & 0xff;
213 qmux->qmicid = cid >> 8;
214 }
215
216 static void *
217 qmictl_new_getcid(u8 tid, u8 svctype, size_t *size)
218 {
219 struct getcid_req *req;
220
221 req = g_malloc0 (sizeof (*req));
222 req->req = 0x00;
223 req->tid = tid;
224 req->msgid = 0x0022;
225 req->tlvsize = 0x0004;
226 req->service = 0x01;
227 req->size = 0x0001;
228 req->qmisvc = svctype;
229 *size = sizeof(*req);
230 qmux_fill (&req->header, QMI_SVC_CTL, 0, *size);
231 return req;
232 }
233
234 static void *
235 qmictl_new_releasecid(u8 tid, u16 cid, size_t *size)
236 {
237 struct releasecid_req *req;
238
239 req = g_malloc0 (sizeof (*req));
240 req->req = 0x00;
241 req->tid = tid;
242 req->msgid = 0x0023;
243 req->tlvsize = 0x05;
244 req->rlscid = 0x01;
245 req->size = 0x0002;
246 req->cid = cid;
247 *size = sizeof(*req);
248 qmux_fill (&req->header, QMI_SVC_CTL, 0, *size);
249 return req;
250 }
251
252 static void *
253 qmictl_new_version_info(u8 tid, size_t *size)
254 {
255 struct version_info_req *req;
256
257 req = g_malloc0 (sizeof (*req));
258 req->req = 0x00;
259 req->tid = tid;
260 req->msgid = 0x21;
261 req->tlvsize = 0;
262 *size = sizeof(*req);
263 qmux_fill (&req->header, QMI_SVC_CTL, 0, *size);
264 return req;
265 }
266
267 static void *
268 qmiwds_new_seteventreport(u8 tid, size_t *size)
269 {
270 struct seteventreport_req *req;
271
272 req = g_malloc0 (sizeof (*req));
273 req->req = 0x00;
274 req->tid = tid;
275 req->msgid = 0x0001;
276 req->tlvsize = 0x0008;
277 req->reportchanrate = 0x11;
278 req->size = 0x0005;
279 req->period = 0x01;
280 req->mask = 0x000000ff;
281 *size = sizeof(*req);
282 return req;
283 }
284
285 static void *
286 qmiwds_new_getpkgsrvcstatus(u8 tid, size_t *size)
287 {
288 struct getpkgsrvcstatus_req *req;
289
290 req = g_malloc0 (sizeof (*req));
291 req->req = 0x00;
292 req->tid = tid;
293 req->msgid = 0x22;
294 req->tlvsize = 0x0000;
295 *size = sizeof(*req);
296 return req;
297 }
298
299 static void *
300 qmidms_new_getmeid(u16 cid, u8 tid, size_t *size)
301 {
302 struct getmeid_req *req;
303
304 req = g_malloc0 (sizeof (*req));
305 req->req = 0x00;
306 req->tid = tid;
307 req->msgid = 0x25;
308 req->tlvsize = 0x0000;
309 *size = sizeof(*req);
310 qmux_fill (&req->header, QMI_SVC_WDS, cid, *size);
311 return req;
312 }
313
314 static int
315 qmux_parse(u16 *cid, void *buf, size_t size)
316 {
317 struct qmux *qmux = buf;
318
319 if (!buf || size < 12)
320 return -ENOMEM;
321
322 if (qmux->tf != 1 || qmux->len != size - 1 || qmux->ctrl != 0x80)
323 return -EINVAL;
324
325 *cid = (qmux->qmicid << 8) + qmux->service;
326 return sizeof(*qmux);
327 }
328
329 static u16
330 tlv_get(void *msg, u16 msgsize, u8 type, void *buf, u16 bufsize)
331 {
332 u16 pos;
333 u16 msize = 0;
334
335 if (!msg || !buf)
336 return -ENOMEM;
337
338 for (pos = 4; pos + 3 < msgsize; pos += msize + 3) {
339 msize = *(u16 *)(msg + pos + 1);
340 if (*(u8 *)(msg + pos) == type) {
341 if (bufsize < msize)
342 return -ENOMEM;
343
344 memcpy(buf, msg + pos + 3, msize);
345 return msize;
346 }
347 }
348
349 return -ENOMSG;
350 }
351
352 static int
353 qmi_msgisvalid(void *msg, u16 size)
354 {
355 struct qmi_tlv_status status;
356
357 if (tlv_get(msg, size, 2, &status, sizeof (status)) == sizeof (status)) {
358 if (le16toh (status.status != 0))
359 return le16toh (status.error);
360 else
361 return 0;
362 }
363 return -ENOMSG;
364 }
365
366 static int
367 qmi_msgid(void *msg, u16 size)
368 {
369 return size < 2 ? -ENODATA : *(u16 *)msg;
370 }
371
372 static int
373 qmictl_version_info_resp(void *buf, u16 size)
374 {
375 int result, i;
376 u8 offset = sizeof(struct qmux) + 2;
377 u8 svcbuf[100];
378 struct qmi_tlv_ctl_version_info_list *service_list;
379 struct qmi_ctl_version_info_list_service *svc;
380
381 if (!buf || size < offset)
382 return -ENOMEM;
383
384 buf = buf + offset;
385 size -= offset;
386
387 result = qmi_msgid(buf, size);
388 if (result != 0x21)
389 return -EFAULT;
390
391 result = qmi_msgisvalid(buf, size);
392 if (result != 0)
393 return -EFAULT;
394
395 /* Get the services TLV */
396 result = tlv_get(buf, size, 0x01, svcbuf, sizeof (svcbuf));
397 if (result < 0)
398 return -EFAULT;
399
400 service_list = (struct qmi_tlv_ctl_version_info_list *) svcbuf;
401 if (result < (service_list->count * sizeof (struct qmi_ctl_version_info_list_service)))
402 return -EFAULT;
403
404 svc = &(service_list->services[0]);
405 for (i = 0; i < service_list->count; i++, svc++) {
406 g_message ("SVC: %d v%d.%d",
407 svc->service_type,
408 le16toh (svc->major_version),
409 le16toh (svc->minor_version));
410 }
411
412 return 0;
413 }
414
415 static int
416 qmictl_getcid_resp(void *buf, u16 size, u16 *cid)
417 {
418 int result;
419 u8 offset = sizeof(struct qmux) + 2;
420
421 if (!buf || size < offset)
422 return -ENOMEM;
423
424 buf = buf + offset;
425 size -= offset;
426
427 result = qmi_msgid(buf, size);
428 if (result != 0x22)
429 return -EFAULT;
430
431 result = qmi_msgisvalid(buf, size);
432 if (result != 0)
433 return -EFAULT;
434
435 result = tlv_get(buf, size, 0x01, cid, 2);
436 if (result != 2)
437 return -EFAULT;
438
439 return 0;
440 }
441
442 static int
443 qmictl_releasecid_resp(void *buf, u16 size)
444 {
445 int result;
446 u8 offset = sizeof(struct qmux) + 2;
447
448 if (!buf || size < offset)
449 return -ENOMEM;
450
451 buf = buf + offset;
452 size -= offset;
453
454 result = qmi_msgid(buf, size);
455 if (result != 0x23)
456 return -EFAULT;
457
458 result = qmi_msgisvalid(buf, size);
459 if (result != 0)
460 return -EFAULT;
461
462 return 0;
463 }
464
465 static int
466 qmiwds_event_resp(void *buf, u16 size, struct qmiwds_stats *stats)
467 {
468 int result;
469 u8 status[2];
470
471 u8 offset = sizeof(struct qmux) + 3;
472
473 if (!buf || size < offset || !stats)
474 return -ENOMEM;
475
476 buf = buf + offset;
477 size -= offset;
478
479 result = qmi_msgid(buf, size);
480 if (result == 0x01) {
481 tlv_get(buf, size, 0x10, &stats->txok, 4);
482 tlv_get(buf, size, 0x11, &stats->rxok, 4);
483 tlv_get(buf, size, 0x12, &stats->txerr, 4);
484 tlv_get(buf, size, 0x13, &stats->rxerr, 4);
485 tlv_get(buf, size, 0x14, &stats->txofl, 4);
486 tlv_get(buf, size, 0x15, &stats->rxofl, 4);
487 tlv_get(buf, size, 0x19, &stats->txbytesok, 8);
488 tlv_get(buf, size, 0x1A, &stats->rxbytesok, 8);
489 } else if (result == 0x22) {
490 result = tlv_get(buf, size, 0x01, &status[0], 2);
491 if (result >= 1)
492 stats->linkstate = status[0] == 0x02;
493 if (result == 2)
494 stats->reconfigure = status[1] == 0x01;
495
496 if (result < 0)
497 return result;
498 } else {
499 return -EFAULT;
500 }
501
502 return 0;
503 }
504
505 static int
506 qmidms_meid_resp(void *buf, u16 size, char *meid, int meidsize)
507 {
508 int result;
509
510 u8 offset = sizeof(struct qmux) + 3;
511
512 if (!buf || size < offset || meidsize < 14)
513 return -ENOMEM;
514
515 buf = buf + offset;
516 size -= offset;
517
518 result = qmi_msgid(buf, size);
519 if (result != 0x25)
520 return -EFAULT;
521
522 result = qmi_msgisvalid(buf, size);
523 if (result)
524 return -EFAULT;
525
526 result = tlv_get(buf, size, 0x12, meid, 14);
527 if (result != 14)
528 return -EFAULT;
529
530 return 0;
531 }
532
533 static void *
534 qminas_new_signal(u16 cid, u8 tid, size_t *size)
535 {
536 struct nas_signal_req *req;
537
538 req = g_malloc0 (sizeof (*req));
539 req->req = 0x00;
540 req->tid = tid;
541 req->msgid = 0x20;
542 req->tlvsize = 0x0000;
543 *size = sizeof(*req);
544 qmux_fill (&req->header, QMI_SVC_NAS, cid, *size);
545 return req;
546 }
547
548 /* NAS/Get Signal Strength TLV 0x01 */
549 struct qminas_resp_signalstrength {
550 u8 dbm;
551 u8 act;
552 } __attribute__((__packed__));
553
554 static int
555 qminas_signal_resp(void *buf, u16 size, u8 *dbm, u8 *act)
556 {
557 int result;
558 struct qminas_resp_signalstrength signal;
559
560 u8 offset = sizeof(struct qmux) + 3;
561
562 if (!buf || size < offset)
563 return -ENOMEM;
564
565 buf = buf + offset;
566 size -= offset;
567
568 result = qmi_msgid(buf, size);
569 if (result != 0x20)
570 return -EFAULT;
571
572 result = qmi_msgisvalid(buf, size);
573 if (result)
574 return -EFAULT;
575
576 result = tlv_get(buf, size, 0x01, &signal, sizeof (signal));
577 if (result != sizeof (signal))
578 return -EFAULT;
579
580 *dbm = signal.dbm;
581 *act = signal.act;
582 return 0;
583 }
584
585 /*****************************************************/
586
587 static size_t
588 send_and_wait_reply (int fd, void *b, size_t blen, char *reply, size_t rlen)
589 {
590 ssize_t num;
591 fd_set in;
592 int result;
593 struct timeval timeout = { 1, 0 };
594
595 print_buf (">>>", b, blen);
596 num = write (fd, b, blen);
597 if (num != blen) {
598 g_warning ("Failed to write: wrote %zd err %d", num, errno);
599 return 0;
600 }
601
602 FD_ZERO (&in);
603 FD_SET (fd, &in);
604 result = select (fd + 1, &in, NULL, NULL, &timeout);
605 if (result != 1 || !FD_ISSET (fd, &in)) {
606 g_warning ("No data pending");
607 return 0;
608 }
609
610 errno = 0;
611 num = read (fd, reply, rlen);
612 if (num < 0) {
613 g_warning ("read error %d", errno);
614 return 0;
615 }
616 print_buf ("<<<", reply, num);
617 return num;
618 }
619
620 /* CTL service transaction ID */
621 static u8 ctl_tid = 1;
622
623 static int
624 get_meid (int fd)
625 {
626 void *b;
627 size_t blen;
628 u8 dms_tid = 1;
629 u16 dms_cid = 0;
630 char reply[2048];
631 size_t rlen;
632 char meid[16];
633 int err;
634
635 /* Allocate a DMS client ID */
636 b = qmictl_new_getcid (ctl_tid++, QMI_SVC_DMS, &blen);
637 g_assert (b);
638
639 rlen = send_and_wait_reply (fd, b, blen, reply, sizeof (reply));
640 g_free (b);
641 if (rlen <= 0)
642 return 1;
643
644 err = qmictl_getcid_resp (reply, rlen, &dms_cid);
645 if (err < 0) {
646 g_warning ("Failed to get DMS client ID: %d", err);
647 return 1;
648 }
649
650 g_message ("DMS CID %d 0x%X", dms_cid, dms_cid);
651
652 /* Get the MEID */
653 b = qmidms_new_getmeid(dms_cid, dms_tid++, &blen);
654 g_assert (b);
655
656 rlen = send_and_wait_reply (fd, b, blen, reply, sizeof (reply));
657 g_free (b);
658 if (rlen <= 0)
659 return 1;
660
661 memset (meid, 0, sizeof (meid));
662 err = qmidms_meid_resp (reply, rlen, meid, sizeof (meid) - 1);
663 if (err < 0)
664 g_warning ("Failed to get MEID: %d", err);
665 else
666 g_message ("MEID: %s", meid);
667
668 /* Relese the DMS client ID */
669 b = qmictl_new_releasecid (ctl_tid++, dms_cid, &blen);
670 g_assert (b);
671
672 rlen = send_and_wait_reply (fd, b, blen, reply, sizeof (reply));
673 g_free (b);
674 if (rlen <= 0)
675 return 1;
676
677 err = qmictl_releasecid_resp (reply, rlen);
678 if (err < 0) {
679 g_warning ("Failed to release DMS client ID: %d", err);
680 return 1;
681 }
682
683 return 0;
684 }
685
686 static const char *
687 act_to_string (u8 act)
688 {
689 switch (act) {
690 case 0:
691 return "no service";
692 case 1:
693 return "CDMA2000 1x";
694 case 2:
695 return "CDMA2000 HRPD/EVDO";
696 case 3:
697 return "AMPS";
698 case 4:
699 return "GSM";
700 case 5:
701 return "UMTS";
702 case 8:
703 return "LTE";
704 default:
705 break;
706 }
707 return "unknown";
708 }
709
710 static int
711 get_signal (int fd)
712 {
713 void *b;
714 size_t blen;
715 u8 nas_tid = 1;
716 u16 nas_cid = 0;
717 char reply[2048];
718 size_t rlen;
719 int err;
720 u8 dbm = 0, act = 0;
721
722 /* Allocate a NAS client ID */
723 b = qmictl_new_getcid (ctl_tid++, QMI_SVC_NAS, &blen);
724 g_assert (b);
725
726 rlen = send_and_wait_reply (fd, b, blen, reply, sizeof (reply));
727 g_free (b);
728 if (rlen <= 0)
729 return 1;
730
731 err = qmictl_getcid_resp (reply, rlen, &nas_cid);
732 if (err < 0) {
733 g_warning ("Failed to get NAS client ID: %d", err);
734 return 1;
735 }
736
737 g_message ("NAS CID %d 0x%X", nas_cid, nas_cid);
738
739 /* Get the signal strength */
740 b = qminas_new_signal(nas_cid, nas_tid++, &blen);
741 g_assert (b);
742
743 rlen = send_and_wait_reply (fd, b, blen, reply, sizeof (reply));
744 g_free (b);
745 if (rlen <= 0)
746 return 1;
747
748 err = qminas_signal_resp (reply, rlen, &dbm, &act);
749 if (err < 0)
750 g_warning ("Failed to get signal: %d", err);
751 else {
752 g_message ("dBm: -%d", 0x100 - (dbm & 0xFF));
753 g_message ("AcT: %s", act_to_string (act));
754 }
755
756 /* Relese the NAS client ID */
757 b = qmictl_new_releasecid (ctl_tid++, nas_cid, &blen);
758 g_assert (b);
759
760 rlen = send_and_wait_reply (fd, b, blen, reply, sizeof (reply));
761 g_free (b);
762 if (rlen <= 0)
763 return 1;
764
765 err = qmictl_releasecid_resp (reply, rlen);
766 if (err < 0) {
767 g_warning ("Failed to release NAS client ID: %d", err);
768 return 1;
769 }
770
771 return 0;
772 }
773
774 int main(int argc, char *argv[])
775 {
776 int fd;
777 void *b;
778 size_t blen;
779 u8 ctl_tid = 1;
780 char reply[2048];
781 size_t rlen;
782 int err;
783
784 if (argc != 2) {
785 g_warning ("usage: %s <port>", argv[0]);
786 return 1;
787 }
788
789 errno = 0;
790 fd = open (argv[1], O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY);
791 if (fd < 0) {
792 g_warning ("%s: open failed: %d", argv[1], errno);
793 return 1;
794 }
795
796 /* Send the ready request */
797 b = qmictl_new_version_info (ctl_tid++, &blen);
798 g_assert (b);
799
800 rlen = send_and_wait_reply (fd, b, blen, reply, sizeof (reply));
801 g_free (b);
802 if (rlen <= 0)
803 goto out;
804
805 err = qmictl_version_info_resp (reply, rlen);
806 if (err < 0) {
807 g_warning ("Failed to get version info: %d", err);
808 goto out;
809 }
810
811 get_meid (fd);
812 get_signal (fd);
813
814 out:
815 close (fd);
816 return 0;
817 }
818