dev.c

   1 /*
   2  * uqmi -- tiny QMI support implementation
   3  *
   4  * Copyright (C) 2014-2015 Felix Fietkau <nbd@openwrt.org>
   5  *
   6  * This library is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public
   8  * License as published by the Free Software Foundation; either
   9  * version 2 of the License, or (at your option) any later version.
  10  *
  11  * This library 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 GNU
  14  * Lesser General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General Public
  17  * License along with this library; if not, write to the
  18  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  19  * Boston, MA 02110-1301 USA.
  20  */
  21
  22 #include <fcntl.h>
  23 #include <unistd.h>
  24 #include <stdlib.h>
  25 #include <stdio.h>
  26 #include <string.h>
  27 #include <strings.h>
  28 #include "uqmi.h"
  29 #include "qmi-errors.h"
  30 #include "qmi-errors.c"
  31 #include "mbim.h"
  32
  33 bool cancel_all_requests = false;
  34
  35 #define __qmi_service(_n) [__##_n] = _n
  36 static const uint8_t qmi_services[__QMI_SERVICE_LAST] = {
  37         __qmi_services
  38 };
  39 #undef __qmi_service
  40
  41 #ifdef DEBUG_PACKET
  42 void dump_packet(const char *prefix, void *ptr, int len)
  43 {
  44         unsigned char *data = ptr;
  45         int i;
  46
  47         fprintf(stderr, "%s:", prefix);
  48         for (i = 0; i < len; i++)
  49                 fprintf(stderr, " %02x", data[i]);
  50         fprintf(stderr, "\n");
  51 }
  52 #endif
  53
  54 static int
  55 qmi_get_service_idx(QmiService svc)
  56 {
  57         int i;
  58
  59         for (i = 0; i < ARRAY_SIZE(qmi_services); i++)
  60                 if (qmi_services[i] == svc)
  61                         return i;
  62
  63         return -1;
  64 }
  65
  66 static bool qmi_message_is_response(struct qmi_msg *msg)
  67 {
  68         if (msg->qmux.service == QMI_SERVICE_CTL) {
  69                 if (msg->flags & QMI_CTL_FLAG_RESPONSE)
  70                         return true;
  71         }
  72         else {
  73                 if (msg->flags & QMI_SERVICE_FLAG_RESPONSE)
  74                         return true;
  75         }
  76
  77         return false;
  78 }
  79
  80 static bool qmi_message_is_indication(struct qmi_msg *msg)
  81 {
  82         if (msg->qmux.service == QMI_SERVICE_CTL) {
  83                 if (msg->flags & QMI_CTL_FLAG_INDICATION)
  84                         return true;
  85         }
  86         else {
  87                 if (msg->flags & QMI_SERVICE_FLAG_INDICATION)
  88                         return true;
  89         }
  90
  91         return false;
  92 }
  93
  94 static void __qmi_request_complete(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg)
  95 {
  96         void *tlv_buf;
  97         int tlv_len;
  98
  99         if (!req->pending)
 100                 return;
 101
 102         req->pending = false;
 103         list_del(&req->list);
 104
 105         if (msg) {
 106                 tlv_buf = qmi_msg_get_tlv_buf(msg, &tlv_len);
 107                 req->ret = qmi_check_message_status(tlv_buf, tlv_len);
 108                 if (req->ret)
 109                         msg = NULL;
 110         } else {
 111                 req->ret = QMI_ERROR_CANCELLED;
 112         }
 113
 114         if (req->cb && (msg || !req->no_error_cb))
 115                 req->cb(qmi, req, msg);
 116
 117         if (req->complete) {
 118                 *req->complete = true;
 119                 uloop_cancelled = true;
 120         }
 121 }
 122
 123 static void qmi_process_msg(struct qmi_dev *qmi, struct qmi_msg *msg)
 124 {
 125         struct qmi_request *req;
 126         uint16_t tid;
 127
 128         if (qmi_message_is_indication(msg)) {
 129                 if (msg->qmux.service == QMI_SERVICE_CTL) {
 130                         struct qmi_msg sync_msg = {0};
 131                         qmi_set_ctl_sync_request(&sync_msg);
 132                         /* A SYNC indication might be sent on boot in order to indicate
 133                          * that all Client IDs have been deallocated by the modem:
 134                          * cancel all requests, as they will not be answered. */
 135                         if (msg->ctl.message == sync_msg.ctl.message) {
 136                                 while (!list_empty(&qmi->req)) {
 137                                         req = list_first_entry(&qmi->req, struct qmi_request, list);
 138                                         qmi_request_cancel(qmi, req);
 139                                 }
 140                         }
 141                 }
 142
 143                 return;
 144         }
 145
 146         if (!qmi_message_is_response(msg))
 147                 return;
 148
 149         if (msg->qmux.service == QMI_SERVICE_CTL)
 150                 tid = msg->ctl.transaction;
 151         else
 152                 tid = le16_to_cpu(msg->svc.transaction);
 153
 154         list_for_each_entry(req, &qmi->req, list) {
 155                 if (req->service != msg->qmux.service)
 156                         continue;
 157
 158                 if (req->tid != tid)
 159                         continue;
 160
 161                 __qmi_request_complete(qmi, req, msg);
 162                 return;
 163         }
 164 }
 165
 166 static void qmi_notify_read(struct ustream *us, int bytes)
 167 {
 168         struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream);
 169         struct qmi_msg *msg;
 170         char *buf;
 171         int len, msg_len;
 172
 173
 174         while (1) {
 175                 buf = ustream_get_read_buf(us, &len);
 176                 if (!buf || !len)
 177                         return;
 178
 179                 dump_packet("Received packet", buf, len);
 180                 if (qmi->is_mbim) {
 181                         struct mbim_command_message *mbim = (void *) buf;
 182
 183                         if (len < sizeof(*mbim))
 184                                 return;
 185                         msg = (struct qmi_msg *) (buf + sizeof(*mbim));
 186                         msg_len = le32_to_cpu(mbim->header.length);
 187                         if (!is_mbim_qmi(mbim)) {
 188                                 /* must consume other MBIM packets */
 189                                 ustream_consume(us, msg_len);
 190                                 return;
 191                         }
 192                 } else {
 193                         if (len < offsetof(struct qmi_msg, flags))
 194                                 return;
 195                         msg = (struct qmi_msg *) buf;
 196                         msg_len = le16_to_cpu(msg->qmux.len) + 1;
 197                 }
 198
 199                 if (len < msg_len)
 200                         return;
 201
 202                 qmi_process_msg(qmi, msg);
 203                 ustream_consume(us, msg_len);
 204         }
 205 }
 206
 207 int qmi_request_start(struct qmi_dev *qmi, struct qmi_request *req, request_cb cb)
 208 {
 209         struct qmi_msg *msg = qmi->buf;
 210         int len = qmi_complete_request_message(msg);
 211         uint16_t tid;
 212         void *buf = (void *) qmi->buf;
 213
 214         memset(req, 0, sizeof(*req));
 215         req->ret = -1;
 216         req->service = msg->qmux.service;
 217         if (req->service == QMI_SERVICE_CTL) {
 218                 tid = qmi->ctl_tid++;
 219                 msg->ctl.transaction = tid;
 220         } else {
 221                 int idx = qmi_get_service_idx(req->service);
 222
 223                 if (idx < 0)
 224                         return -1;
 225
 226                 tid = qmi->service_data[idx].tid++;
 227                 msg->svc.transaction = cpu_to_le16(tid);
 228                 msg->qmux.client = qmi->service_data[idx].client_id;
 229         }
 230
 231         req->tid = tid;
 232         req->cb = cb;
 233         req->pending = true;
 234         list_add(&req->list, &qmi->req);
 235
 236         if (qmi->is_mbim) {
 237                 buf -= sizeof(struct mbim_command_message);
 238                 mbim_qmi_cmd((struct mbim_command_message *) buf, len, tid);
 239                 len += sizeof(struct mbim_command_message);
 240         }
 241
 242         dump_packet("Send packet", buf, len);
 243         ustream_write(&qmi->sf.stream, buf, len, false);
 244         return 0;
 245 }
 246
 247 void qmi_request_cancel(struct qmi_dev *qmi, struct qmi_request *req)
 248 {
 249         req->cb = NULL;
 250         __qmi_request_complete(qmi, req, NULL);
 251 }
 252
 253 /*! Run uloop until the request has been completed
 254  *
 255  * \param qmi the qmi device
 256  * \param req the request to wait for
 257  * \return req->ret (0 on success)
 258  */
 259 int qmi_request_wait(struct qmi_dev *qmi, struct qmi_request *req)
 260 {
 261         bool complete = false;
 262         bool cancelled;
 263
 264         if (!req->pending)
 265                 return req->ret;
 266
 267         if (req->complete)
 268                 *req->complete = true;
 269
 270         req->complete = &complete;
 271         while (!complete) {
 272                 cancelled = uloop_cancelled;
 273                 uloop_cancelled = false;
 274                 uloop_run();
 275
 276                 if (cancel_all_requests)
 277                         qmi_request_cancel(qmi, req);
 278
 279                 uloop_cancelled = cancelled;
 280         }
 281
 282         if (req->complete == &complete)
 283                 req->complete = NULL;
 284
 285         return req->ret;
 286 }
 287
 288 struct qmi_connect_request {
 289         struct qmi_request req;
 290         int cid;
 291 };
 292
 293 static void qmi_connect_service_cb(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg)
 294 {
 295         struct qmi_ctl_allocate_cid_response res;
 296         struct qmi_connect_request *creq = container_of(req, struct qmi_connect_request, req);
 297
 298         if (!msg)
 299                 return;
 300
 301         qmi_parse_ctl_allocate_cid_response(msg, &res);
 302         creq->cid = res.data.allocation_info.cid;
 303 }
 304
 305 int qmi_service_connect(struct qmi_dev *qmi, QmiService svc, int client_id)
 306 {
 307         struct qmi_ctl_allocate_cid_request creq = {
 308                 QMI_INIT(service, svc)
 309         };
 310         struct qmi_connect_request req;
 311         int idx = qmi_get_service_idx(svc);
 312         struct qmi_msg *msg = qmi->buf;
 313
 314         if (idx < 0)
 315                 return -1;
 316
 317         if (qmi->service_connected & (1 << idx))
 318                 return 0;
 319
 320         if (client_id < 0) {
 321                 qmi_set_ctl_allocate_cid_request(msg, &creq);
 322                 qmi_request_start(qmi, &req.req, qmi_connect_service_cb);
 323                 qmi_request_wait(qmi, &req.req);
 324
 325                 if (req.req.ret)
 326                         return req.req.ret;
 327
 328                 client_id = req.cid;
 329         } else {
 330                 qmi->service_keep_cid |= (1 << idx);
 331         }
 332
 333         qmi->service_data[idx].connected = true;
 334         qmi->service_data[idx].client_id = client_id;
 335         qmi->service_data[idx].tid = 1;
 336         qmi->service_connected |= (1 << idx);
 337
 338         return 0;
 339 }
 340
 341 static void __qmi_service_disconnect(struct qmi_dev *qmi, int idx)
 342 {
 343         int client_id = qmi->service_data[idx].client_id;
 344         struct qmi_ctl_release_cid_request creq = {
 345                 QMI_INIT_SEQUENCE(release_info,
 346                         .service = qmi_services[idx],
 347                         .cid = client_id,
 348                 )
 349         };
 350         struct qmi_request req;
 351         struct qmi_msg *msg = qmi->buf;
 352
 353         qmi->service_connected &= ~(1 << idx);
 354         qmi->service_data[idx].client_id = -1;
 355         qmi->service_data[idx].tid = 0;
 356
 357         qmi_set_ctl_release_cid_request(msg, &creq);
 358         qmi_request_start(qmi, &req, NULL);
 359         qmi_request_wait(qmi, &req);
 360 }
 361
 362 int qmi_service_release_client_id(struct qmi_dev *qmi, QmiService svc)
 363 {
 364         int idx = qmi_get_service_idx(svc);
 365         qmi->service_release_cid |= 1 << idx;
 366         return 0;
 367 }
 368
 369 static void qmi_close_all_services(struct qmi_dev *qmi)
 370 {
 371         uint32_t connected = qmi->service_connected;
 372         int idx;
 373
 374         qmi->service_keep_cid &= ~qmi->service_release_cid;
 375         for (idx = 0; connected; idx++, connected >>= 1) {
 376                 if (!(connected & 1))
 377                         continue;
 378
 379                 if (qmi->service_keep_cid & (1 << idx))
 380                         continue;
 381
 382                 __qmi_service_disconnect(qmi, idx);
 383         }
 384 }
 385
 386 int qmi_service_get_client_id(struct qmi_dev *qmi, QmiService svc)
 387 {
 388         int idx = qmi_get_service_idx(svc);
 389
 390         if (idx < 0)
 391                 return -1;
 392
 393         qmi->service_keep_cid |= (1 << idx);
 394         return qmi->service_data[idx].client_id;
 395 }
 396
 397 int qmi_device_open(struct qmi_dev *qmi, const char *path)
 398 {
 399         static struct {
 400                 struct mbim_command_message mbim;
 401                 union {
 402                         char buf[2048];
 403                         struct qmi_msg msg;
 404                 } u;
 405         } __packed msgbuf;
 406         struct ustream *us = &qmi->sf.stream;
 407         int fd;
 408
 409         uloop_init();
 410
 411         fd = open(path, O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY);
 412         if (fd < 0)
 413                 return -1;
 414
 415         us->notify_read = qmi_notify_read;
 416         ustream_fd_init(&qmi->sf, fd);
 417         INIT_LIST_HEAD(&qmi->req);
 418         qmi->ctl_tid = 1;
 419         qmi->buf = msgbuf.u.buf;
 420
 421         return 0;
 422 }
 423
 424 void qmi_device_close(struct qmi_dev *qmi)
 425 {
 426         struct qmi_request *req;
 427
 428         qmi_close_all_services(qmi);
 429         ustream_free(&qmi->sf.stream);
 430         close(qmi->sf.fd.fd);
 431
 432         while (!list_empty(&qmi->req)) {
 433                 req = list_first_entry(&qmi->req, struct qmi_request, list);
 434                 qmi_request_cancel(qmi, req);
 435         }
 436 }
 437
 438 QmiService qmi_service_get_by_name(const char *str)
 439 {
 440         static const struct {
 441                 const char *name;
 442                 QmiService svc;
 443         } services[] = {
 444                 { "dms", QMI_SERVICE_DMS },
 445                 { "nas", QMI_SERVICE_NAS },
 446                 { "pds", QMI_SERVICE_PDS },
 447                 { "wds", QMI_SERVICE_WDS },
 448                 { "wms", QMI_SERVICE_WMS },
 449                 { "wda", QMI_SERVICE_WDA },
 450                 { "uim", QMI_SERVICE_UIM },
 451         };
 452         int i;
 453
 454         for (i = 0; i < ARRAY_SIZE(services); i++) {
 455                 if (!strcasecmp(str, services[i].name))
 456                         return services[i].svc;
 457         }
 458
 459         return -1;
 460 }