drivers/qmimodem/qmi.c

   1 /*
   2  *
   3  *  oFono - Open Source Telephony
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation. All rights reserved.
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License version 2 as
   9  *  published by the Free Software Foundation.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25
  26 #define _GNU_SOURCE
  27 #include <stdio.h>
  28 #include <ctype.h>
  29 #include <fcntl.h>
  30 #include <unistd.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33
  34 #include <glib.h>
  35
  36 #include "qmi.h"
  37 #include "ctl.h"
  38
  39 typedef void (*qmi_message_func_t)(uint16_t message, uint16_t length,
  40                                         const void *buffer, void *user_data);
  41
  42 struct qmi_device {
  43         int ref_count;
  44         int fd;
  45         GIOChannel *io;
  46         bool close_on_unref;
  47         guint read_watch;
  48         guint write_watch;
  49         GQueue *req_queue;
  50         GQueue *control_queue;
  51         GQueue *service_queue;
  52         uint8_t next_control_tid;
  53         uint16_t next_service_tid;
  54         qmi_debug_func_t debug_func;
  55         void *debug_data;
  56         uint16_t control_major;
  57         uint16_t control_minor;
  58         char *version_str;
  59         struct qmi_version *version_list;
  60         uint8_t version_count;
  61         GHashTable *service_list;
  62         unsigned int release_users;
  63 };
  64
  65 struct qmi_service {
  66         int ref_count;
  67         struct qmi_device *device;
  68         bool shared;
  69         uint8_t type;
  70         uint16_t major;
  71         uint16_t minor;
  72         uint8_t client_id;
  73         uint16_t next_notify_id;
  74         GList *notify_list;
  75 };
  76
  77 struct qmi_param {
  78         void *data;
  79         uint16_t length;
  80 };
  81
  82 struct qmi_result {
  83         uint16_t message;
  84         uint16_t result;
  85         uint16_t error;
  86         const void *data;
  87         uint16_t length;
  88 };
  89
  90 struct qmi_request {
  91         uint16_t tid;
  92         uint8_t client;
  93         void *buf;
  94         size_t len;
  95         qmi_message_func_t callback;
  96         void *user_data;
  97 };
  98
  99 struct qmi_notify {
 100         uint16_t id;
 101         uint16_t message;
 102         qmi_result_func_t callback;
 103         void *user_data;
 104         qmi_destroy_func_t destroy;
 105 };
 106
 107 struct qmi_mux_hdr {
 108         uint8_t  frame;         /* Always 0x01 */
 109         uint16_t length;        /* Packet size without frame byte */
 110         uint8_t  flags;         /* Either 0x00 or 0x80 */
 111         uint8_t  service;       /* Service type (0x00 for control) */
 112         uint8_t  client;        /* Client identifier (0x00 for control) */
 113 } __attribute__ ((packed));
 114 #define QMI_MUX_HDR_SIZE 6
 115
 116 struct qmi_control_hdr {
 117         uint8_t  type;          /* Bit 1 = response, Bit 2 = indication */
 118         uint8_t  transaction;   /* Transaction identifier */
 119 } __attribute__ ((packed));
 120 #define QMI_CONTROL_HDR_SIZE 2
 121
 122 struct qmi_service_hdr {
 123         uint8_t  type;          /* Bit 2 = response, Bit 3 = indication */
 124         uint16_t transaction;   /* Transaction identifier */
 125 } __attribute__ ((packed));
 126 #define QMI_SERVICE_HDR_SIZE 3
 127
 128 struct qmi_message_hdr {
 129         uint16_t message;       /* Message identifier */
 130         uint16_t length;        /* Message size without header */
 131         uint8_t data[0];
 132 } __attribute__ ((packed));
 133 #define QMI_MESSAGE_HDR_SIZE 4
 134
 135 struct qmi_tlv_hdr {
 136         uint8_t type;
 137         uint16_t length;
 138         uint8_t value[0];
 139 } __attribute__ ((packed));
 140 #define QMI_TLV_HDR_SIZE 3
 141
 142 void qmi_free(void *ptr)
 143 {
 144         free(ptr);
 145 }
 146
 147 static struct qmi_request *__request_alloc(uint8_t service,
 148                                 uint8_t client, uint16_t message,
 149                                 uint16_t headroom, const void *data,
 150                                 uint16_t length, qmi_message_func_t func,
 151                                 void *user_data, void **head)
 152 {
 153         struct qmi_request *req;
 154         struct qmi_mux_hdr *hdr;
 155         struct qmi_message_hdr *msg;
 156
 157         req = g_try_new0(struct qmi_request, 1);
 158         if (!req)
 159                 return NULL;
 160
 161         req->len = QMI_MUX_HDR_SIZE + headroom + QMI_MESSAGE_HDR_SIZE + length;
 162
 163         req->buf = g_try_malloc(req->len);
 164         if (!req->buf) {
 165                 g_free(req);
 166                 return NULL;
 167         }
 168
 169         req->client = client;
 170
 171         hdr = req->buf;
 172
 173         hdr->frame = 0x01;
 174         hdr->length = GUINT16_TO_LE(req->len - 1);
 175         hdr->flags = 0x00;
 176         hdr->service = service;
 177         hdr->client = client;
 178
 179         msg = req->buf + QMI_MUX_HDR_SIZE + headroom;
 180
 181         msg->message = GUINT16_TO_LE(message);
 182         msg->length = GUINT16_TO_LE(length);
 183
 184         if (data && length > 0)
 185                 memcpy(req->buf + QMI_MUX_HDR_SIZE + headroom +
 186                                         QMI_MESSAGE_HDR_SIZE, data, length);
 187
 188         req->callback = func;
 189         req->user_data = user_data;
 190
 191         *head = req->buf + QMI_MUX_HDR_SIZE;
 192
 193         return req;
 194 }
 195
 196 static void __request_free(gpointer data, gpointer user_data)
 197 {
 198         struct qmi_request *req = data;
 199
 200         g_free(req->buf);
 201         g_free(req);
 202 }
 203
 204 static gint __request_compare(gconstpointer a, gconstpointer b)
 205 {
 206         const struct qmi_request *req = a;
 207         uint16_t tid = GPOINTER_TO_UINT(b);
 208
 209         return req->tid - tid;
 210 }
 211
 212 static void __notify_free(gpointer data, gpointer user_data)
 213 {
 214         struct qmi_notify *notify = data;
 215
 216         if (notify->destroy)
 217                 notify->destroy(notify->user_data);
 218
 219         g_free(notify);
 220 }
 221
 222 static gint __notify_compare(gconstpointer a, gconstpointer b)
 223 {
 224         const struct qmi_notify *notify = a;
 225         uint16_t id = GPOINTER_TO_UINT(b);
 226
 227         return notify->id - id;
 228 }
 229
 230 static gboolean __service_compare_shared(gpointer key, gpointer value,
 231                                                         gpointer user_data)
 232 {
 233         struct qmi_service *service = value;
 234         uint8_t type = GPOINTER_TO_UINT(user_data);
 235
 236         if (!service->shared)
 237                 return FALSE;
 238
 239         if (service->type == type)
 240                 return TRUE;
 241
 242         return FALSE;
 243 }
 244
 245 static void __hexdump(const char dir, const unsigned char *buf, size_t len,
 246                                 qmi_debug_func_t function, void *user_data)
 247 {
 248         static const char hexdigits[] = "0123456789abcdef";
 249         char str[68];
 250         size_t i;
 251
 252         if (!function || !len)
 253                 return;
 254
 255         str[0] = dir;
 256
 257         for (i = 0; i < len; i++) {
 258                 str[((i % 16) * 3) + 1] = ' ';
 259                 str[((i % 16) * 3) + 2] = hexdigits[buf[i] >> 4];
 260                 str[((i % 16) * 3) + 3] = hexdigits[buf[i] & 0xf];
 261                 str[(i % 16) + 51] = isprint(buf[i]) ? buf[i] : '.';
 262
 263                 if ((i + 1) % 16 == 0) {
 264                         str[49] = ' ';
 265                         str[50] = ' ';
 266                         str[67] = '\0';
 267                         function(str, user_data);
 268                         str[0] = ' ';
 269                 }
 270         }
 271
 272         if (i % 16 > 0) {
 273                 size_t j;
 274                 for (j = (i % 16); j < 16; j++) {
 275                         str[(j * 3) + 1] = ' ';
 276                         str[(j * 3) + 2] = ' ';
 277                         str[(j * 3) + 3] = ' ';
 278                         str[j + 51] = ' ';
 279                 }
 280                 str[49] = ' ';
 281                 str[50] = ' ';
 282                 str[67] = '\0';
 283                 function(str, user_data);
 284         }
 285 }
 286
 287 static const char *__service_type_to_string(uint8_t type)
 288 {
 289         switch (type) {
 290         case QMI_SERVICE_CONTROL:
 291                 return "CTL";
 292         case QMI_SERVICE_WDS:
 293                 return "WDS";
 294         case QMI_SERVICE_DMS:
 295                 return "DMS";
 296         case QMI_SERVICE_NAS:
 297                 return "NAS";
 298         case QMI_SERVICE_QOS:
 299                 return "QOS";
 300         case QMI_SERVICE_WMS:
 301                 return "WMS";
 302         case QMI_SERVICE_PDS:
 303                 return "PDS";
 304         case QMI_SERVICE_AUTH:
 305                 return "AUTH";
 306         case QMI_SERVICE_AT:
 307                 return "AT";
 308         case QMI_SERVICE_VOICE:
 309                 return "VOICE";
 310         case QMI_SERVICE_CAT:
 311                 return "CAT";
 312         case QMI_SERVICE_UIM:
 313                 return "UIM";
 314         case QMI_SERVICE_PBM:
 315                 return "PBM";
 316         case QMI_SERVICE_RMTFS:
 317                 return "RMTFS";
 318         case QMI_SERVICE_LOC:
 319                 return "LOC";
 320         case QMI_SERVICE_SAR:
 321                 return "SAR";
 322         case QMI_SERVICE_CSD:
 323                 return "CSD";
 324         case QMI_SERVICE_EFS:
 325                 return "EFS";
 326         case QMI_SERVICE_TS:
 327                 return "TS";
 328         case QMI_SERVICE_TMD:
 329                 return "TMS";
 330         case QMI_SERVICE_PDC:
 331                 return "PDC";
 332         case QMI_SERVICE_CAT_OLD:
 333                 return "CAT";
 334         case QMI_SERVICE_RMS:
 335                 return "RMS";
 336         case QMI_SERVICE_OMA:
 337                 return "OMA";
 338         }
 339
 340         return NULL;
 341 }
 342
 343 static const struct {
 344         uint16_t err;
 345         const char *str;
 346 } __error_table[] = {
 347         { 0x0000, "NONE"                        },
 348         { 0x0001, "MALFORMED_MSG"               },
 349         { 0x0002, "NO_MEMORY"                   },
 350         { 0x0003, "INTERNAL"                    },
 351         { 0x0004, "ABORTED"                     },
 352         { 0x0005, "CLIENT_IDS_EXHAUSTED"        },
 353         { 0x0006, "UNABORTABLE_TRANSACTION"     },
 354         { 0x0007, "INVALID_CLIENT_ID"           },
 355         { 0x0008, "NO_THRESHOLDS"               },
 356         { 0x0009, "INVALID_HANDLE"              },
 357         { 0x000a, "INVALID_PROFILE"             },
 358         { 0x000b, "INVALID_PINID"               },
 359         { 0x000c, "INCORRECT_PIN"               },
 360         { 0x000d, "NO_NETWORK_FOUND"            },
 361         { 0x000e, "CALL_FAILED"                 },
 362         { 0x000f, "OUT_OF_CALL"                 },
 363         { 0x0010, "NOT_PROVISIONED"             },
 364         { 0x0011, "MISSING_ARG"                 },
 365         { 0x0013, "ARG_TOO_LONG"                },
 366         { 0x0016, "INVALID_TX_ID"               },
 367         { 0x0017, "DEVICE_IN_USE"               },
 368         { 0x0018, "OP_NETWORK_UNSUPPORTED"      },
 369         { 0x0019, "OP_DEVICE_UNSUPPORTED"       },
 370         { 0x001a, "NO_EFFECT"                   },
 371         { 0x001b, "NO_FREE_PROFILE"             },
 372         { 0x001c, "INVALID_PDP_TYPE"            },
 373         { 0x001d, "INVALID_TECH_PREF"           },
 374         { 0x001e, "INVALID_PROFILE_TYPE"        },
 375         { 0x001f, "INVALID_SERVICE_TYPE"        },
 376         { 0x0020, "INVALID_REGISTER_ACTION"     },
 377         { 0x0021, "INVALID_PS_ATTACH_ACTION"    },
 378         { 0x0022, "AUTHENTICATION_FAILED"       },
 379         { 0x0023, "PIN_BLOCKED"                 },
 380         { 0x0024, "PIN_PERM_BLOCKED"            },
 381         { 0x0025, "UIM_NOT_INITIALIZED"         },
 382         { 0x0026, "MAX_QOS_REQUESTS_IN_USE"     },
 383         { 0x0027, "INCORRECT_FLOW_FILTER"       },
 384         { 0x0028, "NETWORK_QOS_UNAWARE"         },
 385         { 0x0029, "INVALID_QOS_ID/INVALID_ID"   },
 386         { 0x002a, "REQUESTED_NUM_UNSUPPORTED"   },
 387         { 0x002b, "INTERFACE_NOT_FOUND"         },
 388         { 0x002c, "FLOW_SUSPENDED"              },
 389         { 0x002d, "INVALID_DATA_FORMAT"         },
 390         { 0x002e, "GENERAL"                     },
 391         { 0x002f, "UNKNOWN"                     },
 392         { 0x0030, "INVALID_ARG"                 },
 393         { 0x0031, "INVALID_INDEX"               },
 394         { 0x0032, "NO_ENTRY"                    },
 395         { 0x0033, "DEVICE_STORAGE_FULL"         },
 396         { 0x0034, "DEVICE_NOT_READY"            },
 397         { 0x0035, "NETWORK_NOT_READY"           },
 398         { 0x0036, "CAUSE_CODE"                  },
 399         { 0x0037, "MESSAGE_NOT_SENT"            },
 400         { 0x0038, "MESSAGE_DELIVERY_FAILURE"    },
 401         { 0x0039, "INVALID_MESSAGE_ID"          },
 402         { 0x003a, "ENCODING"                    },
 403         { 0x003b, "AUTHENTICATION_LOCK"         },
 404         { 0x003c, "INVALID_TRANSACTION"         },
 405         { 0x0041, "SESSION_INACTIVE"            },
 406         { 0x0042, "SESSION_INVALID"             },
 407         { 0x0043, "SESSION_OWNERSHIP"           },
 408         { 0x0044, "INSUFFICIENT_RESOURCES"      },
 409         { 0x0045, "DISABLED"                    },
 410         { 0x0046, "INVALID_OPERATION"           },
 411         { 0x0047, "INVALID_QMI_CMD"             },
 412         { 0x0048, "TPDU_TYPE"                   },
 413         { 0x0049, "SMSC_ADDR"                   },
 414         { 0x004a, "INFO_UNAVAILABLE"            },
 415         { 0x004b, "SEGMENT_TOO_LONG"            },
 416         { 0x004c, "SEGEMENT_ORDER"              },
 417         { 0x004d, "BUNDLING_NOT_SUPPORTED"      },
 418         { 0x004f, "POLICY_MISMATCH"             },
 419         { 0x0050, "SIM_FILE_NOT_FOUND"          },
 420         { 0x0051, "EXTENDED_INTERNAL"           },
 421         { 0x0052, "ACCESS_DENIED"               },
 422         { 0x0053, "HARDWARE_RESTRICTED"         },
 423         { 0x0054, "ACK_NOT_SENT"                },
 424         { 0x0055, "INJECT_TIMEOUT"              },
 425         { }
 426 };
 427
 428 static const char *__error_to_string(uint16_t error)
 429 {
 430         int i;
 431
 432         for (i = 0; __error_table[i].str; i++) {
 433                 if (__error_table[i].err == error)
 434                         return __error_table[i].str;
 435         }
 436
 437         return NULL;
 438 }
 439
 440 static void __debug_msg(const char dir, const void *buf, size_t len,
 441                                 qmi_debug_func_t function, void *user_data)
 442 {
 443         const struct qmi_mux_hdr *hdr;
 444         const struct qmi_message_hdr *msg;
 445         const char *service;
 446         const void *ptr;
 447         uint16_t offset;
 448         char strbuf[72 + 16], *str;
 449         bool pending_print = false;
 450
 451         if (!function || !len)
 452                 return;
 453
 454         hdr = buf;
 455
 456         str = strbuf;
 457         service = __service_type_to_string(hdr->service);
 458         if (service)
 459                 str += sprintf(str, "%c   %s", dir, service);
 460         else
 461                 str += sprintf(str, "%c   %d", dir, hdr->service);
 462
 463         if (hdr->service == QMI_SERVICE_CONTROL) {
 464                 const struct qmi_control_hdr *ctl;
 465                 const char *type;
 466
 467                 ctl = buf + QMI_MUX_HDR_SIZE;
 468                 msg = buf + QMI_MUX_HDR_SIZE + QMI_CONTROL_HDR_SIZE;
 469                 ptr = buf + QMI_MUX_HDR_SIZE + QMI_CONTROL_HDR_SIZE +
 470                                                         QMI_MESSAGE_HDR_SIZE;
 471
 472                 switch (ctl->type) {
 473                 case 0x00:
 474                         type = "_req";
 475                         break;
 476                 case 0x01:
 477                         type = "_resp";
 478                         break;
 479                 case 0x02:
 480                         type = "_ind";
 481                         break;
 482                 default:
 483                         type = "";
 484                         break;
 485                 }
 486
 487                 str += sprintf(str, "%s msg=%d len=%d", type,
 488                                         GUINT16_FROM_LE(msg->message),
 489                                         GUINT16_FROM_LE(msg->length));
 490
 491                 str += sprintf(str, " [client=%d,type=%d,tid=%d,len=%d]",
 492                                         hdr->client, ctl->type,
 493                                         ctl->transaction,
 494                                         GUINT16_FROM_LE(hdr->length));
 495         } else {
 496                 const struct qmi_service_hdr *srv;
 497                 const char *type;
 498
 499                 srv = buf + QMI_MUX_HDR_SIZE;
 500                 msg = buf + QMI_MUX_HDR_SIZE + QMI_SERVICE_HDR_SIZE;
 501                 ptr = buf + QMI_MUX_HDR_SIZE + QMI_SERVICE_HDR_SIZE +
 502                                                         QMI_MESSAGE_HDR_SIZE;
 503
 504                 switch (srv->type) {
 505                 case 0x00:
 506                         type = "_req";
 507                         break;
 508                 case 0x02:
 509                         type = "_resp";
 510                         break;
 511                 case 0x04:
 512                         type = "_ind";
 513                         break;
 514                 default:
 515                         type = "";
 516                         break;
 517                 }
 518
 519                 str += sprintf(str, "%s msg=%d len=%d", type,
 520                                         GUINT16_FROM_LE(msg->message),
 521                                         GUINT16_FROM_LE(msg->length));
 522
 523                 str += sprintf(str, " [client=%d,type=%d,tid=%d,len=%d]",
 524                                         hdr->client, srv->type,
 525                                         GUINT16_FROM_LE(srv->transaction),
 526                                         GUINT16_FROM_LE(hdr->length));
 527         }
 528
 529         function(strbuf, user_data);
 530
 531         if (!msg->length)
 532                 return;
 533
 534         str = strbuf;
 535         str += sprintf(str, "      ");
 536         offset = 0;
 537
 538         while (offset + QMI_TLV_HDR_SIZE < GUINT16_FROM_LE(msg->length)) {
 539                 const struct qmi_tlv_hdr *tlv = ptr + offset;
 540                 uint16_t tlv_length = GUINT16_FROM_LE(tlv->length);
 541
 542                 if (tlv->type == 0x02 && tlv_length == QMI_RESULT_CODE_SIZE) {
 543                         const struct qmi_result_code *result = ptr + offset +
 544                                                         QMI_TLV_HDR_SIZE;
 545                         uint16_t error = GUINT16_FROM_LE(result->error);
 546                         const char *error_str;
 547
 548                         error_str = __error_to_string(error);
 549                         if (error_str)
 550                                 str += sprintf(str, " {type=%d,error=%s}",
 551                                                         tlv->type, error_str);
 552                         else
 553                                 str += sprintf(str, " {type=%d,error=%d}",
 554                                                         tlv->type, error);
 555                 } else {
 556                         str += sprintf(str, " {type=%d,len=%d}", tlv->type,
 557                                                                 tlv_length);
 558                 }
 559
 560                 if (str - strbuf > 60) {
 561                         function(strbuf, user_data);
 562
 563                         str = strbuf;
 564                         str += sprintf(str, "      ");
 565
 566                         pending_print = false;
 567                 } else
 568                         pending_print = true;
 569
 570                 offset += QMI_TLV_HDR_SIZE + tlv_length;
 571         }
 572
 573         if (pending_print)
 574                 function(strbuf, user_data);
 575 }
 576
 577 static void __debug_device(struct qmi_device *device,
 578                                         const char *format, ...)
 579 {
 580         char strbuf[72 + 16];
 581         va_list ap;
 582
 583         if (!device->debug_func)
 584                 return;
 585
 586         va_start(ap, format);
 587         vsnprintf(strbuf, sizeof(strbuf), format, ap);
 588         va_end(ap);
 589
 590         device->debug_func(strbuf, device->debug_data);
 591 }
 592
 593 static gboolean can_write_data(GIOChannel *channel, GIOCondition cond,
 594                                                         gpointer user_data)
 595 {
 596         struct qmi_device *device = user_data;
 597         struct qmi_mux_hdr *hdr;
 598         struct qmi_request *req;
 599         ssize_t bytes_written;
 600
 601         req = g_queue_pop_head(device->req_queue);
 602         if (!req)
 603                 return FALSE;
 604
 605         bytes_written = write(device->fd, req->buf, req->len);
 606         if (bytes_written < 0)
 607                 return FALSE;
 608
 609         __hexdump('>', req->buf, bytes_written,
 610                                 device->debug_func, device->debug_data);
 611
 612         __debug_msg(' ', req->buf, bytes_written,
 613                                 device->debug_func, device->debug_data);
 614
 615         hdr = req->buf;
 616
 617         if (hdr->service == QMI_SERVICE_CONTROL)
 618                 g_queue_push_tail(device->control_queue, req);
 619         else
 620                 g_queue_push_tail(device->service_queue, req);
 621
 622         g_free(req->buf);
 623         req->buf = NULL;
 624
 625         if (g_queue_get_length(device->req_queue) > 0)
 626                 return TRUE;
 627
 628         return FALSE;
 629 }
 630
 631 static void write_watch_destroy(gpointer user_data)
 632 {
 633         struct qmi_device *device = user_data;
 634
 635         device->write_watch = 0;
 636 }
 637
 638 static void wakeup_writer(struct qmi_device *device)
 639 {
 640         if (device->write_watch > 0)
 641                 return;
 642
 643         device->write_watch = g_io_add_watch_full(device->io, G_PRIORITY_HIGH,
 644                                 G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 645                                 can_write_data, device, write_watch_destroy);
 646 }
 647
 648 static void __request_submit(struct qmi_device *device,
 649                                 struct qmi_request *req, uint16_t transaction)
 650 {
 651         req->tid = transaction;
 652
 653         g_queue_push_tail(device->req_queue, req);
 654
 655         wakeup_writer(device);
 656 }
 657
 658 static void service_notify(gpointer key, gpointer value, gpointer user_data)
 659 {
 660         struct qmi_service *service = value;
 661         struct qmi_result *result = user_data;
 662         GList *list;
 663
 664         for (list = g_list_first(service->notify_list); list;
 665                                                 list = g_list_next(list)) {
 666                 struct qmi_notify *notify = list->data;
 667
 668                 if (notify->message == result->message)
 669                         notify->callback(result, notify->user_data);
 670         }
 671 }
 672
 673 static void handle_indication(struct qmi_device *device,
 674                         uint8_t service_type, uint8_t client_id,
 675                         uint16_t message, uint16_t length, const void *data)
 676 {
 677         struct qmi_service *service;
 678         struct qmi_result result;
 679         unsigned int hash_id;
 680
 681         if (service_type == QMI_SERVICE_CONTROL)
 682                 return;
 683
 684         result.result = 0;
 685         result.error = 0;
 686         result.message = message;
 687         result.data = data;
 688         result.length = length;
 689
 690         if (client_id == 0xff) {
 691                 g_hash_table_foreach(device->service_list,
 692                                                 service_notify, &result);
 693                 return;
 694         }
 695
 696         hash_id = service_type | (client_id << 8);
 697
 698         service = g_hash_table_lookup(device->service_list,
 699                                         GUINT_TO_POINTER(hash_id));
 700         if (!service)
 701                 return;
 702
 703         service_notify(NULL, service, &result);
 704 }
 705
 706 static void handle_packet(struct qmi_device *device,
 707                                 const struct qmi_mux_hdr *hdr, const void *buf)
 708 {
 709         struct qmi_request *req;
 710         uint16_t message, length;
 711         const void *data;
 712
 713         if (hdr->service == QMI_SERVICE_CONTROL) {
 714                 const struct qmi_control_hdr *control = buf;
 715                 const struct qmi_message_hdr *msg;
 716                 unsigned int tid;
 717                 GList *list;
 718
 719                 /* Ignore control messages with client identifier */
 720                 if (hdr->client != 0x00)
 721                         return;
 722
 723                 msg = buf + QMI_CONTROL_HDR_SIZE;
 724
 725                 message = GUINT16_FROM_LE(msg->message);
 726                 length = GUINT16_FROM_LE(msg->length);
 727
 728                 data = buf + QMI_CONTROL_HDR_SIZE + QMI_MESSAGE_HDR_SIZE;
 729
 730                 tid = control->transaction;
 731
 732                 if (control->type == 0x02 && control->transaction == 0x00) {
 733                         handle_indication(device, hdr->service, hdr->client,
 734                                                         message, length, data);
 735                         return;
 736                 }
 737
 738                 list = g_queue_find_custom(device->control_queue,
 739                                 GUINT_TO_POINTER(tid), __request_compare);
 740                 if (!list)
 741                         return;
 742
 743                 req = list->data;
 744
 745                 g_queue_delete_link(device->control_queue, list);
 746         } else {
 747                 const struct qmi_service_hdr *service = buf;
 748                 const struct qmi_message_hdr *msg;
 749                 unsigned int tid;
 750                 GList *list;
 751
 752                 msg = buf + QMI_SERVICE_HDR_SIZE;
 753
 754                 message = GUINT16_FROM_LE(msg->message);
 755                 length = GUINT16_FROM_LE(msg->length);
 756
 757                 data = buf + QMI_SERVICE_HDR_SIZE + QMI_MESSAGE_HDR_SIZE;
 758
 759                 tid = GUINT16_FROM_LE(service->transaction);
 760
 761                 if (service->type == 0x04 && tid == 0x0000) {
 762                         handle_indication(device, hdr->service, hdr->client,
 763                                                         message, length, data);
 764                         return;
 765                 }
 766
 767                 list = g_queue_find_custom(device->service_queue,
 768                                 GUINT_TO_POINTER(tid), __request_compare);
 769                 if (!list)
 770                         return;
 771
 772                 req = list->data;
 773
 774                 g_queue_delete_link(device->service_queue, list);
 775         }
 776
 777         if (req->callback)
 778                 req->callback(message, length, data, req->user_data);
 779
 780         __request_free(req, NULL);
 781 }
 782
 783 static gboolean received_data(GIOChannel *channel, GIOCondition cond,
 784                                                         gpointer user_data)
 785 {
 786         struct qmi_device *device = user_data;
 787         struct qmi_mux_hdr *hdr;
 788         unsigned char buf[2048];
 789         ssize_t bytes_read;
 790         uint16_t offset;
 791
 792         if (cond & G_IO_NVAL)
 793                 return FALSE;
 794
 795         bytes_read = read(device->fd, buf, sizeof(buf));
 796         if (bytes_read < 0)
 797                 return TRUE;
 798
 799         __hexdump('<', buf, bytes_read,
 800                                 device->debug_func, device->debug_data);
 801
 802         offset = 0;
 803
 804         while (offset < bytes_read) {
 805                 uint16_t len;
 806
 807                 /* Check if QMI mux header fits into packet */
 808                 if (bytes_read - offset < QMI_MUX_HDR_SIZE)
 809                         break;
 810
 811                 hdr = (void *) (buf + offset);
 812
 813                 /* Check for fixed frame and flags value */
 814                 if (hdr->frame != 0x01 || hdr->flags != 0x80)
 815                         break;
 816
 817                 len = GUINT16_FROM_LE(hdr->length) + 1;
 818
 819                 /* Check that packet size matches frame size */
 820                 if (bytes_read - offset < len)
 821                         break;
 822
 823                 __debug_msg(' ', buf + offset, len,
 824                                 device->debug_func, device->debug_data);
 825
 826                 handle_packet(device, hdr, buf + offset + QMI_MUX_HDR_SIZE);
 827
 828                 offset += len;
 829         }
 830
 831         return TRUE;
 832 }
 833
 834 static void read_watch_destroy(gpointer user_data)
 835 {
 836         struct qmi_device *device = user_data;
 837
 838         device->read_watch = 0;
 839 }
 840
 841 static void service_destroy(gpointer data)
 842 {
 843         struct qmi_service *service = data;
 844
 845         if (!service->device)
 846                 return;
 847
 848         service->device = NULL;
 849 }
 850
 851 struct qmi_device *qmi_device_new(int fd)
 852 {
 853         struct qmi_device *device;
 854         long flags;
 855
 856         device = g_try_new0(struct qmi_device, 1);
 857         if (!device)
 858                 return NULL;
 859
 860         __debug_device(device, "device %p new", device);
 861
 862         device->ref_count = 1;
 863
 864         device->fd = fd;
 865         device->close_on_unref = false;
 866
 867         flags = fcntl(device->fd, F_GETFL, NULL);
 868         if (flags < 0) {
 869                 g_free(device);
 870                 return NULL;
 871         }
 872
 873         if (!(flags & O_NONBLOCK)) {
 874                 if (fcntl(device->fd, F_SETFL, flags | O_NONBLOCK) < 0) {
 875                         g_free(device);
 876                         return NULL;
 877                 }
 878         }
 879
 880         device->io = g_io_channel_unix_new(device->fd);
 881
 882         g_io_channel_set_encoding(device->io, NULL, NULL);
 883         g_io_channel_set_buffered(device->io, FALSE);
 884
 885         device->read_watch = g_io_add_watch_full(device->io, G_PRIORITY_DEFAULT,
 886                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 887                                 received_data, device, read_watch_destroy);
 888
 889         g_io_channel_unref(device->io);
 890
 891         device->req_queue = g_queue_new();
 892         device->control_queue = g_queue_new();
 893         device->service_queue = g_queue_new();
 894
 895         device->service_list = g_hash_table_new_full(g_direct_hash,
 896                                         g_direct_equal, NULL, service_destroy);
 897
 898         return device;
 899 }
 900
 901 struct qmi_device *qmi_device_ref(struct qmi_device *device)
 902 {
 903         if (!device)
 904                 return NULL;
 905
 906         __sync_fetch_and_add(&device->ref_count, 1);
 907
 908         return device;
 909 }
 910
 911 void qmi_device_unref(struct qmi_device *device)
 912 {
 913         if (!device)
 914                 return;
 915
 916         if (__sync_sub_and_fetch(&device->ref_count, 1))
 917                 return;
 918
 919         __debug_device(device, "device %p free", device);
 920
 921         g_queue_foreach(device->control_queue, __request_free, NULL);
 922         g_queue_free(device->control_queue);
 923
 924         g_queue_foreach(device->service_queue, __request_free, NULL);
 925         g_queue_free(device->service_queue);
 926
 927         g_queue_foreach(device->req_queue, __request_free, NULL);
 928         g_queue_free(device->req_queue);
 929
 930         if (device->write_watch > 0)
 931                 g_source_remove(device->write_watch);
 932
 933         if (device->read_watch > 0)
 934                 g_source_remove(device->read_watch);
 935
 936         if (device->close_on_unref)
 937                 close(device->fd);
 938
 939         g_hash_table_destroy(device->service_list);
 940
 941         g_free(device->version_str);
 942         g_free(device->version_list);
 943
 944         g_free(device);
 945 }
 946
 947 void qmi_device_set_debug(struct qmi_device *device,
 948                                 qmi_debug_func_t func, void *user_data)
 949 {
 950         if (device == NULL)
 951                 return;
 952
 953         device->debug_func = func;
 954         device->debug_data = user_data;
 955 }
 956
 957 void qmi_device_set_close_on_unref(struct qmi_device *device, bool do_close)
 958 {
 959         if (!device)
 960                 return;
 961
 962         device->close_on_unref = do_close;
 963 }
 964
 965 static const void *tlv_get(const void *data, uint16_t size,
 966                                         uint8_t type, uint16_t *length)
 967 {
 968         const void *ptr = data;
 969         uint16_t len = size;
 970
 971         while (len > QMI_TLV_HDR_SIZE) {
 972                 const struct qmi_tlv_hdr *tlv = ptr;
 973                 uint16_t tlv_length = GUINT16_FROM_LE(tlv->length);
 974
 975                 if (tlv->type == type) {
 976                         if (length)
 977                                 *length = tlv_length;
 978
 979                         return ptr + QMI_TLV_HDR_SIZE;
 980                 }
 981
 982                 ptr += QMI_TLV_HDR_SIZE + tlv_length;
 983                 len -= QMI_TLV_HDR_SIZE + tlv_length;
 984         }
 985
 986         return NULL;
 987 }
 988
 989 struct discover_data {
 990         struct qmi_device *device;
 991         qmi_discover_func_t func;
 992         void *user_data;
 993         qmi_destroy_func_t destroy;
 994         guint timeout;
 995 };
 996
 997 static void discover_callback(uint16_t message, uint16_t length,
 998                                         const void *buffer, void *user_data)
 999 {
1000         struct discover_data *data = user_data;
1001         struct qmi_device *device = data->device;
1002         const struct qmi_result_code *result_code;
1003         const struct qmi_service_list *service_list;
1004         const void *ptr;
1005         uint16_t len;
1006         struct qmi_version *list;
1007         uint8_t count;
1008         unsigned int i;
1009
1010         g_source_remove(data->timeout);
1011
1012         count = 0;
1013         list = NULL;
1014
1015         result_code = tlv_get(buffer, length, 0x02, &len);
1016         if (!result_code)
1017                 goto done;
1018
1019         if (len != QMI_RESULT_CODE_SIZE)
1020                 goto done;
1021
1022         service_list = tlv_get(buffer, length, 0x01, &len);
1023         if (!service_list)
1024                 goto done;
1025
1026         if (len < QMI_SERVICE_LIST_SIZE)
1027                 goto done;
1028
1029         list = g_try_malloc(sizeof(struct qmi_version) * service_list->count);
1030         if (!list)
1031                 goto done;
1032
1033         for (i = 0; i < service_list->count; i++) {
1034                 uint16_t major =
1035                         GUINT16_FROM_LE(service_list->services[i].major);
1036                 uint16_t minor =
1037                         GUINT16_FROM_LE(service_list->services[i].minor);
1038                 uint8_t type = service_list->services[i].type;
1039                 const char *name = __service_type_to_string(type);
1040
1041                 if (type == QMI_SERVICE_CONTROL) {
1042                         device->control_major = major;
1043                         device->control_minor = minor;
1044                         continue;
1045                 }
1046
1047                 list[count].type = type;
1048                 list[count].major = major;
1049                 list[count].minor = minor;
1050                 list[count].name = name;
1051
1052                 count++;
1053
1054                 if (name)
1055                         __debug_device(device, "found service [%s %d.%d]",
1056                                                         name, major, minor);
1057                 else
1058                         __debug_device(device, "found service [%d %d.%d]",
1059                                                         type, major, minor);
1060         }
1061
1062         ptr = tlv_get(buffer, length, 0x10, &len);
1063         if (!ptr)
1064                 goto done;
1065
1066         device->version_str = strndup(ptr + 1, *((uint8_t *) ptr));
1067
1068         service_list = ptr + *((uint8_t *) ptr) + 1;
1069
1070         for (i = 0; i < service_list->count; i++) {
1071                 if (service_list->services[i].type == QMI_SERVICE_CONTROL)
1072                         continue;
1073         }
1074
1075 done:
1076         device->version_list = list;
1077         device->version_count = count;
1078
1079         if (data->func)
1080                 data->func(count, list, data->user_data);
1081
1082         if (data->destroy)
1083                 data->destroy(data->user_data);
1084
1085         g_free(data);
1086 }
1087
1088 static gboolean discover_reply(gpointer user_data)
1089 {
1090         struct discover_data *data = user_data;
1091         struct qmi_device *device = data->device;
1092
1093         data->timeout = 0;
1094
1095         if (data->func)
1096                 data->func(device->version_count,
1097                                 device->version_list, data->user_data);
1098
1099         if (data->destroy)
1100                 data->destroy(data->user_data);
1101
1102         g_free(data);
1103
1104         return FALSE;
1105 }
1106
1107 bool qmi_device_discover(struct qmi_device *device, qmi_discover_func_t func,
1108                                 void *user_data, qmi_destroy_func_t destroy)
1109 {
1110         struct discover_data *data;
1111         struct qmi_request *req;
1112         struct qmi_control_hdr *hdr;
1113
1114         if (!device)
1115                 return false;
1116
1117         __debug_device(device, "device %p discover", device);
1118
1119         data = g_try_new0(struct discover_data, 1);
1120         if (!data)
1121                 return false;
1122
1123         data->device = device;
1124         data->func = func;
1125         data->user_data = user_data;
1126         data->destroy = destroy;
1127
1128         if (device->version_list) {
1129                 g_timeout_add_seconds(0, discover_reply, data);
1130                 return true;
1131         }
1132
1133         req = __request_alloc(QMI_SERVICE_CONTROL, 0x00,
1134                         QMI_CTL_GET_VERSION_INFO, QMI_CONTROL_HDR_SIZE,
1135                         NULL, 0, discover_callback, data, (void **) &hdr);
1136         if (!req) {
1137                 g_free(data);
1138                 return false;
1139         }
1140
1141         if (device->next_control_tid < 1)
1142                 device->next_control_tid = 1;
1143
1144         hdr->type = 0x00;
1145         hdr->transaction = device->next_control_tid++;
1146
1147         __request_submit(device, req, hdr->transaction);
1148
1149         data->timeout = g_timeout_add_seconds(5, discover_reply, data);
1150
1151         return true;
1152 }
1153
1154 static void release_client(struct qmi_device *device,
1155                                 uint8_t type, uint8_t client_id,
1156                                 qmi_message_func_t func, void *user_data)
1157 {
1158         unsigned char release_req[] = { 0x01, 0x02, 0x00, type, client_id };
1159         struct qmi_request *req;
1160         struct qmi_control_hdr *hdr;
1161
1162         req = __request_alloc(QMI_SERVICE_CONTROL, 0x00,
1163                         QMI_CTL_RELEASE_CLIENT_ID, QMI_CONTROL_HDR_SIZE,
1164                         release_req, sizeof(release_req),
1165                         func, user_data, (void **) &hdr);
1166         if (!req) {
1167                 func(0x0000, 0x0000, NULL, user_data);
1168                 return;
1169         }
1170
1171         if (device->next_control_tid < 1)
1172                 device->next_control_tid = 1;
1173
1174         hdr->type = 0x00;
1175         hdr->transaction = device->next_control_tid++;
1176
1177         __request_submit(device, req, hdr->transaction);
1178 }
1179
1180 struct shutdown_data {
1181         struct qmi_device *device;
1182         qmi_shutdown_func_t func;
1183         void *user_data;
1184         qmi_destroy_func_t destroy;
1185 };
1186
1187 static gboolean shutdown_reply(gpointer user_data)
1188 {
1189         struct shutdown_data *data = user_data;
1190
1191         if (data->func)
1192                 data->func(data->user_data);
1193
1194         g_free(data);
1195
1196         return FALSE;
1197 }
1198
1199 static gboolean shutdown_timeout(gpointer user_data)
1200 {
1201         struct shutdown_data *data = user_data;
1202         struct qmi_device *device = data->device;
1203
1204         if (device->release_users > 0)
1205                 return TRUE;
1206
1207         return shutdown_reply(data);
1208 }
1209
1210 bool qmi_device_shutdown(struct qmi_device *device, qmi_shutdown_func_t func,
1211                                 void *user_data, qmi_destroy_func_t destroy)
1212 {
1213         struct shutdown_data *data;
1214
1215         if (!device)
1216                 return false;
1217
1218         __debug_device(device, "device %p shutdown", device);
1219
1220         data = g_try_new0(struct shutdown_data, 1);
1221         if (!data)
1222                 return false;
1223
1224         data->device = device;
1225         data->func = func;
1226         data->user_data = user_data;
1227         data->destroy = destroy;
1228
1229         if (device->release_users > 0)
1230                 g_timeout_add_seconds(0, shutdown_timeout, data);
1231         else
1232                 g_timeout_add_seconds(0, shutdown_reply, data);
1233
1234         return true;
1235 }
1236
1237 struct qmi_param *qmi_param_new(void)
1238 {
1239         struct qmi_param *param;
1240
1241         param = g_try_new0(struct qmi_param, 1);
1242         if (!param)
1243                 return NULL;
1244
1245         return param;
1246 }
1247
1248 void qmi_param_free(struct qmi_param *param)
1249 {
1250         if (!param)
1251                 return;
1252
1253         g_free(param->data);
1254         g_free(param);
1255 }
1256
1257 bool qmi_param_append(struct qmi_param *param, uint8_t type,
1258                                         uint16_t length, const void *data)
1259 {
1260         struct qmi_tlv_hdr *tlv;
1261         void *ptr;
1262
1263         if (!param || !type)
1264                 return false;
1265
1266         if (!length)
1267                 return true;
1268
1269         if (!data)
1270                 return false;
1271
1272         if (param->data)
1273                 ptr = g_try_realloc(param->data,
1274                                 param->length + QMI_TLV_HDR_SIZE + length);
1275         else
1276                 ptr = g_try_malloc(QMI_TLV_HDR_SIZE + length);
1277
1278         if (!ptr)
1279                 return false;
1280
1281         tlv = ptr + param->length;
1282
1283         tlv->type = type;
1284         tlv->length = GUINT16_TO_LE(length);
1285         memcpy(tlv->value, data, length);
1286
1287         param->data = ptr;
1288         param->length += QMI_TLV_HDR_SIZE + length;
1289
1290         return true;
1291 }
1292
1293 bool qmi_param_append_uint8(struct qmi_param *param, uint8_t type,
1294                                                         uint8_t value)
1295 {
1296         unsigned char buf[1] = { value };
1297
1298         return qmi_param_append(param, type, sizeof(buf), buf);
1299 }
1300
1301 bool qmi_param_append_uint16(struct qmi_param *param, uint8_t type,
1302                                                         uint16_t value)
1303 {
1304         unsigned char buf[2] = { value & 0xff, (value & 0xff00) >> 8 };
1305
1306         return qmi_param_append(param, type, sizeof(buf), buf);
1307 }
1308
1309 bool qmi_param_append_uint32(struct qmi_param *param, uint8_t type,
1310                                                         uint32_t value)
1311 {
1312         unsigned char buf[4] = { value & 0xff, (value & 0xff00) >> 8,
1313                                         (value & 0xff0000) >> 16,
1314                                         (value & 0xff000000) >> 24 };
1315
1316         return qmi_param_append(param, type, sizeof(buf), buf);
1317 }
1318
1319 struct qmi_param *qmi_param_new_uint8(uint8_t type, uint8_t value)
1320 {
1321         struct qmi_param *param;
1322
1323         param = qmi_param_new();
1324         if (!param)
1325                 return NULL;
1326
1327         if (!qmi_param_append_uint8(param, type, value)) {
1328                 qmi_param_free(param);
1329                 return NULL;
1330         }
1331
1332         return param;
1333 }
1334
1335 struct qmi_param *qmi_param_new_uint16(uint8_t type, uint16_t value)
1336 {
1337         struct qmi_param *param;
1338
1339         param = qmi_param_new();
1340         if (!param)
1341                 return NULL;
1342
1343         if (!qmi_param_append_uint16(param, type, value)) {
1344                 qmi_param_free(param);
1345                 return NULL;
1346         }
1347
1348         return param;
1349 }
1350
1351 struct qmi_param *qmi_param_new_uint32(uint8_t type, uint32_t value)
1352 {
1353         struct qmi_param *param;
1354
1355         param = qmi_param_new();
1356         if (!param)
1357                 return NULL;
1358
1359         if (!qmi_param_append_uint32(param, type, value)) {
1360                 qmi_param_free(param);
1361                 return NULL;
1362         }
1363
1364         return param;
1365 }
1366
1367 bool qmi_result_set_error(struct qmi_result *result, uint16_t *error)
1368 {
1369         if (!result) {
1370                 if (error)
1371                         *error = 0xffff;
1372                 return true;
1373         }
1374
1375         if (result->result == 0x0000)
1376                 return false;
1377
1378         if (error)
1379                 *error = result->error;
1380
1381         return true;
1382 }
1383
1384 const char *qmi_result_get_error(struct qmi_result *result)
1385 {
1386         if (!result)
1387                 return NULL;
1388
1389         if (result->result == 0x0000)
1390                 return NULL;
1391
1392         return __error_to_string(result->error);
1393 }
1394
1395 const void *qmi_result_get(struct qmi_result *result, uint8_t type,
1396                                                         uint16_t *length)
1397 {
1398         if (!result || !type)
1399                 return NULL;
1400
1401         return tlv_get(result->data, result->length, type, length);
1402 }
1403
1404 char *qmi_result_get_string(struct qmi_result *result, uint8_t type)
1405 {
1406         const void *ptr;
1407         uint16_t len;
1408
1409         if (!result || !type)
1410                 return NULL;
1411
1412         ptr = tlv_get(result->data, result->length, type, &len);
1413         if (!ptr)
1414                 return NULL;
1415
1416         return strndup(ptr, len);
1417 }
1418
1419 bool qmi_result_get_uint8(struct qmi_result *result, uint8_t type,
1420                                                         uint8_t *value)
1421 {
1422         const unsigned char *ptr;
1423         uint16_t len;
1424
1425         if (!result || !type)
1426                 return false;
1427
1428         ptr = tlv_get(result->data, result->length, type, &len);
1429         if (!ptr)
1430                 return false;
1431
1432         if (value)
1433                 *value = *ptr;
1434
1435         return true;
1436 }
1437
1438 bool qmi_result_get_uint16(struct qmi_result *result, uint8_t type,
1439                                                         uint16_t *value)
1440 {
1441         const unsigned char *ptr;
1442         uint16_t len, tmp;
1443
1444         if (!result || !type)
1445                 return false;
1446
1447         ptr = tlv_get(result->data, result->length, type, &len);
1448         if (!ptr)
1449                 return false;
1450
1451         memcpy(&tmp, ptr, 2);
1452
1453         if (value)
1454                 *value = GUINT16_FROM_LE(tmp);
1455
1456         return true;
1457 }
1458
1459 bool qmi_result_get_uint32(struct qmi_result *result, uint8_t type,
1460                                                         uint32_t *value)
1461 {
1462         const unsigned char *ptr;
1463         uint16_t len;
1464         uint32_t tmp;
1465
1466         if (!result || !type)
1467                 return false;
1468
1469         ptr = tlv_get(result->data, result->length, type, &len);
1470         if (!ptr)
1471                 return false;
1472
1473         memcpy(&tmp, ptr, 4);
1474
1475         if (value)
1476                 *value = GUINT32_FROM_LE(tmp);
1477
1478         return true;
1479 }
1480
1481 bool qmi_result_get_uint64(struct qmi_result *result, uint8_t type,
1482                                                         uint64_t *value)
1483 {
1484         const unsigned char *ptr;
1485         uint16_t len;
1486         uint64_t tmp;
1487
1488         if (!result || !type)
1489                 return false;
1490
1491         ptr = tlv_get(result->data, result->length, type, &len);
1492         if (!ptr)
1493                 return false;
1494
1495         memcpy(&tmp, ptr, 8);
1496
1497         if (value)
1498                 *value = GUINT64_FROM_LE(tmp);
1499
1500         return true;
1501 }
1502
1503 struct service_create_data {
1504         struct qmi_device *device;
1505         bool shared;
1506         uint8_t type;
1507         uint16_t major;
1508         uint16_t minor;
1509         qmi_create_func_t func;
1510         void *user_data;
1511         qmi_destroy_func_t destroy;
1512         guint timeout;
1513 };
1514
1515 static gboolean service_create_reply(gpointer user_data)
1516 {
1517         struct service_create_data *data = user_data;
1518
1519         data->func(NULL, data->user_data);
1520
1521         if (data->destroy)
1522                 data->destroy(data->user_data);
1523
1524         g_free(data);
1525
1526         return FALSE;
1527 }
1528
1529 static void service_create_callback(uint16_t message, uint16_t length,
1530                                         const void *buffer, void *user_data)
1531 {
1532         struct service_create_data *data = user_data;
1533         struct qmi_device *device = data->device;
1534         struct qmi_service *service = NULL;
1535         const struct qmi_result_code *result_code;
1536         const struct qmi_client_id *client_id;
1537         uint16_t len;
1538         unsigned int hash_id;
1539
1540         g_source_remove(data->timeout);
1541
1542         result_code = tlv_get(buffer, length, 0x02, &len);
1543         if (!result_code)
1544                 goto done;
1545
1546         if (len != QMI_RESULT_CODE_SIZE)
1547                 goto done;
1548
1549         client_id = tlv_get(buffer, length, 0x01, &len);
1550         if (!client_id)
1551                 goto done;
1552
1553         if (len != QMI_CLIENT_ID_SIZE)
1554                 goto done;
1555
1556         if (client_id->service != data->type)
1557                 goto done;
1558
1559         service = g_try_new0(struct qmi_service, 1);
1560         if (!service)
1561                 goto done;
1562
1563         service->ref_count = 1;
1564         service->device = data->device;
1565         service->shared = data->shared;
1566
1567         service->type = data->type;
1568         service->major = data->major;
1569         service->minor = data->minor;
1570
1571         service->client_id = client_id->client;
1572
1573         __debug_device(device, "service created [client=%d,type=%d]",
1574                                         service->client_id, service->type);
1575
1576         hash_id = service->type | (service->client_id << 8);
1577
1578         g_hash_table_replace(device->service_list,
1579                                 GUINT_TO_POINTER(hash_id), service);
1580
1581 done:
1582         data->func(service, data->user_data);
1583
1584         qmi_service_unref(service);
1585
1586         if (data->destroy)
1587                 data->destroy(data->user_data);
1588
1589         g_free(data);
1590 }
1591
1592 static void service_create_discover(uint8_t count,
1593                         const struct qmi_version *list, void *user_data)
1594 {
1595         struct service_create_data *data = user_data;
1596         struct qmi_device *device = data->device;
1597         struct qmi_request *req;
1598         struct qmi_control_hdr *hdr;
1599         unsigned char client_req[] = { 0x01, 0x01, 0x00, data->type };
1600         unsigned int i;
1601
1602         __debug_device(device, "service create [type=%d]", data->type);
1603
1604         for (i = 0; i < count; i++) {
1605                 if (list[i].type == data->type) {
1606                         data->major = list[i].major;
1607                         data->minor = list[i].minor;
1608                         break;
1609                 }
1610         }
1611
1612         req = __request_alloc(QMI_SERVICE_CONTROL, 0x00,
1613                         QMI_CTL_GET_CLIENT_ID, QMI_CONTROL_HDR_SIZE,
1614                         client_req, sizeof(client_req),
1615                         service_create_callback, data, (void **) &hdr);
1616         if (!req) {
1617                 if (data->timeout > 0)
1618                         g_source_remove(data->timeout);
1619
1620                 g_timeout_add_seconds(0, service_create_reply, data);
1621                 return;
1622         }
1623
1624         if (device->next_control_tid < 1)
1625                 device->next_control_tid = 1;
1626
1627         hdr->type = 0x00;
1628         hdr->transaction = device->next_control_tid++;
1629
1630         __request_submit(device, req, hdr->transaction);
1631 }
1632
1633 static bool service_create(struct qmi_device *device, bool shared,
1634                                 uint8_t type, qmi_create_func_t func,
1635                                 void *user_data, qmi_destroy_func_t destroy)
1636 {
1637         struct service_create_data *data;
1638
1639         data = g_try_new0(struct service_create_data, 1);
1640         if (!data)
1641                 return false;
1642
1643         data->device = device;
1644         data->shared = shared;
1645         data->type = type;
1646         data->func = func;
1647         data->user_data = user_data;
1648         data->destroy = destroy;
1649
1650         if (device->version_list) {
1651                 service_create_discover(device->version_count,
1652                                                 device->version_list, data);
1653                 goto done;
1654         }
1655
1656         if (qmi_device_discover(device, service_create_discover, data, NULL))
1657                 goto done;
1658
1659         g_free(data);
1660
1661         return false;
1662
1663 done:
1664         data->timeout = g_timeout_add_seconds(8, service_create_reply, data);
1665
1666         return true;
1667 }
1668
1669 bool qmi_service_create(struct qmi_device *device,
1670                                 uint8_t type, qmi_create_func_t func,
1671                                 void *user_data, qmi_destroy_func_t destroy)
1672 {
1673         if (!device || !func)
1674                 return false;
1675
1676         if (type == QMI_SERVICE_CONTROL)
1677                 return false;
1678
1679         return service_create(device, false, type, func, user_data, destroy);
1680 }
1681
1682 struct service_create_shared_data {
1683         struct qmi_service *service;
1684         qmi_create_func_t func;
1685         void *user_data;
1686         qmi_destroy_func_t destroy;
1687 };
1688
1689 static gboolean service_create_shared_reply(gpointer user_data)
1690 {
1691         struct service_create_shared_data *data = user_data;
1692
1693         data->func(data->service, data->user_data);
1694
1695         qmi_service_unref(data->service);
1696
1697         if (data->destroy)
1698                 data->destroy(data->user_data);
1699
1700         g_free(data);
1701
1702         return FALSE;
1703 }
1704
1705 bool qmi_service_create_shared(struct qmi_device *device,
1706                                 uint8_t type, qmi_create_func_t func,
1707                                 void *user_data, qmi_destroy_func_t destroy)
1708 {
1709         struct qmi_service *service;
1710         unsigned int type_val = type;
1711
1712         if (!device || !func)
1713                 return false;
1714
1715         if (type == QMI_SERVICE_CONTROL)
1716                 return false;
1717
1718         service = g_hash_table_find(device->service_list,
1719                         __service_compare_shared, GUINT_TO_POINTER(type_val));
1720         if (service) {
1721                 struct service_create_shared_data *data;
1722
1723                 data = g_try_new0(struct service_create_shared_data, 1);
1724                 if (!data)
1725                         return false;
1726
1727                 data->service = qmi_service_ref(service);
1728
1729                 data->func = func;
1730                 data->user_data = user_data;
1731                 data->destroy = destroy;
1732
1733                 g_timeout_add(0, service_create_shared_reply, data);
1734
1735                 return 0;
1736         }
1737
1738         return service_create(device, true, type, func, user_data, destroy);
1739 }
1740
1741 static void service_release_callback(uint16_t message, uint16_t length,
1742                                         const void *buffer, void *user_data)
1743 {
1744         struct qmi_service *service = user_data;
1745
1746         if (service->device)
1747                 service->device->release_users--;
1748
1749         g_free(service);
1750 }
1751
1752 struct qmi_service *qmi_service_ref(struct qmi_service *service)
1753 {
1754         if (!service)
1755                 return NULL;
1756
1757         __sync_fetch_and_add(&service->ref_count, 1);
1758
1759         return service;
1760 }
1761
1762 void qmi_service_unref(struct qmi_service *service)
1763 {
1764         unsigned int hash_id;
1765
1766         if (!service)
1767                 return;
1768
1769         if (__sync_sub_and_fetch(&service->ref_count, 1))
1770                 return;
1771
1772         if (!service->device) {
1773                 g_free(service);
1774                 return;
1775         }
1776
1777         qmi_service_cancel_all(service);
1778         qmi_service_unregister_all(service);
1779
1780         hash_id = service->type | (service->client_id << 8);
1781
1782         g_hash_table_steal(service->device->service_list,
1783                                         GUINT_TO_POINTER(hash_id));
1784
1785         service->device->release_users++;
1786
1787         release_client(service->device, service->type, service->client_id,
1788                                         service_release_callback, service);
1789 }
1790
1791 const char *qmi_service_get_identifier(struct qmi_service *service)
1792 {
1793         if (!service)
1794                 return NULL;
1795
1796         return __service_type_to_string(service->type);
1797 }
1798
1799 bool qmi_service_get_version(struct qmi_service *service,
1800                                         uint16_t *major, uint16_t *minor)
1801 {
1802         if (!service)
1803                 return false;
1804
1805         if (major)
1806                 *major = service->major;
1807
1808         if (minor)
1809                 *minor = service->minor;
1810
1811         return true;
1812 }
1813
1814 struct service_send_data {
1815         struct qmi_service *service;
1816         struct qmi_param *param;
1817         qmi_result_func_t func;
1818         void *user_data;
1819         qmi_destroy_func_t destroy;
1820 };
1821
1822 static void service_send_free(struct service_send_data *data)
1823 {
1824         if (data->destroy)
1825                 data->destroy(data->user_data);
1826
1827         qmi_param_free(data->param);
1828
1829         g_free(data);
1830 }
1831
1832 static void service_send_callback(uint16_t message, uint16_t length,
1833                                         const void *buffer, void *user_data)
1834 {
1835         struct service_send_data *data = user_data;
1836         const struct qmi_result_code *result_code;
1837         uint16_t len;
1838         struct qmi_result result;
1839
1840         result.message = message;
1841         result.data = buffer;
1842         result.length = length;
1843
1844         result_code = tlv_get(buffer, length, 0x02, &len);
1845         if (!result_code)
1846                 goto done;
1847
1848         if (len != QMI_RESULT_CODE_SIZE)
1849                 goto done;
1850
1851         result.result = GUINT16_FROM_LE(result_code->result);
1852         result.error = GUINT16_FROM_LE(result_code->error);
1853
1854 done:
1855         if (data->func)
1856                 data->func(&result, data->user_data);
1857
1858         service_send_free(data);
1859 }
1860
1861 uint16_t qmi_service_send(struct qmi_service *service,
1862                                 uint16_t message, struct qmi_param *param,
1863                                 qmi_result_func_t func,
1864                                 void *user_data, qmi_destroy_func_t destroy)
1865 {
1866         struct qmi_device *device;
1867         struct service_send_data *data;
1868         struct qmi_request *req;
1869         struct qmi_service_hdr *hdr;
1870
1871         if (!service)
1872                 return 0;
1873
1874         if (!service->client_id)
1875                 return 0;
1876
1877         device = service->device;
1878         if (!device)
1879                 return 0;
1880
1881         data = g_try_new0(struct service_send_data, 1);
1882         if (!data)
1883                 return 0;
1884
1885         data->service = service;
1886         data->param = param;
1887         data->func = func;
1888         data->user_data = user_data;
1889         data->destroy = destroy;
1890
1891         req = __request_alloc(service->type, service->client_id,
1892                                 message, QMI_SERVICE_HDR_SIZE,
1893                                 data->param ? data->param->data : NULL,
1894                                 data->param ? data->param->length : 0,
1895                                 service_send_callback, data, (void **) &hdr);
1896         if (!req) {
1897                 g_free(data);
1898                 return 0;
1899         }
1900
1901         if (device->next_service_tid < 256)
1902                 device->next_service_tid = 256;
1903
1904         hdr->type = 0x00;
1905         hdr->transaction = device->next_service_tid++;
1906
1907         __request_submit(device, req, hdr->transaction);
1908
1909         return hdr->transaction;
1910 }
1911
1912 bool qmi_service_cancel(struct qmi_service *service, uint16_t id)
1913 {
1914         unsigned int tid = id;
1915         struct qmi_device *device;
1916         struct qmi_request *req;
1917         GList *list;
1918
1919         if (!service || !tid)
1920                 return false;
1921
1922         if (!service->client_id)
1923                 return false;
1924
1925         device = service->device;
1926         if (!device)
1927                 return false;
1928
1929         list = g_queue_find_custom(device->req_queue,
1930                                 GUINT_TO_POINTER(tid), __request_compare);
1931         if (list) {
1932                 req = list->data;
1933
1934                 g_queue_delete_link(device->req_queue, list);
1935         } else {
1936                 list = g_queue_find_custom(device->service_queue,
1937                                 GUINT_TO_POINTER(tid), __request_compare);
1938                 if (!list)
1939                         return false;
1940
1941                 req = list->data;
1942
1943                 g_queue_delete_link(device->service_queue, list);
1944         }
1945
1946         service_send_free(req->user_data);
1947
1948         __request_free(req, NULL);
1949
1950         return true;
1951 }
1952
1953 static GQueue *remove_client(GQueue *queue, uint8_t client)
1954 {
1955         GQueue *new_queue;
1956         GList *list;
1957
1958         new_queue = g_queue_new();
1959
1960         while (1) {
1961                 struct qmi_request *req;
1962
1963                 list = g_queue_pop_head_link(queue);
1964                 if (!list)
1965                         break;
1966
1967                 req = list->data;
1968
1969                 if (!req->client || req->client != client) {
1970                         g_queue_push_tail_link(new_queue, list);
1971                         continue;
1972                 }
1973
1974                 service_send_free(req->user_data);
1975
1976                 __request_free(req, NULL);
1977         }
1978
1979         g_queue_free(queue);
1980
1981         return new_queue;
1982 }
1983
1984 bool qmi_service_cancel_all(struct qmi_service *service)
1985 {
1986         struct qmi_device *device;
1987
1988         if (!service)
1989                 return false;
1990
1991         if (!service->client_id)
1992                 return false;
1993
1994         device = service->device;
1995         if (!device)
1996                 return false;
1997
1998         device->req_queue = remove_client(device->req_queue,
1999                                                 service->client_id);
2000
2001         device->service_queue = remove_client(device->service_queue,
2002                                                         service->client_id);
2003
2004         return true;
2005 }
2006
2007 uint16_t qmi_service_register(struct qmi_service *service,
2008                                 uint16_t message, qmi_result_func_t func,
2009                                 void *user_data, qmi_destroy_func_t destroy)
2010 {
2011         struct qmi_notify *notify;
2012
2013         if (!service || !func)
2014                 return 0;
2015
2016         notify = g_try_new0(struct qmi_notify, 1);
2017         if (!notify)
2018                 return 0;
2019
2020         if (service->next_notify_id < 1)
2021                 service->next_notify_id = 1;
2022
2023         notify->id = service->next_notify_id++;
2024         notify->message = message;
2025         notify->callback = func;
2026         notify->user_data = user_data;
2027         notify->destroy = destroy;
2028
2029         service->notify_list = g_list_append(service->notify_list, notify);
2030
2031         return notify->id;
2032 }
2033
2034 bool qmi_service_unregister(struct qmi_service *service, uint16_t id)
2035 {
2036         unsigned int nid = id;
2037         struct qmi_notify *notify;
2038         GList *list;
2039
2040         if (!service || !id)
2041                 return false;
2042
2043         list = g_list_find_custom(service->notify_list,
2044                                 GUINT_TO_POINTER(nid), __notify_compare);
2045         if (!list)
2046                 return false;
2047
2048         notify = list->data;
2049
2050         service->notify_list = g_list_delete_link(service->notify_list, list);
2051
2052         __notify_free(notify, NULL);
2053
2054         return true;
2055 }
2056
2057 bool qmi_service_unregister_all(struct qmi_service *service)
2058 {
2059         if (!service)
2060                 return false;
2061
2062         g_list_foreach(service->notify_list, __notify_free, NULL);
2063         g_list_free(service->notify_list);
2064
2065         service->notify_list = NULL;
2066
2067         return true;
2068 }