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 "uqmi.h"
  28 #include "qmi-errors.h"
  29 #include "qmi-errors.c"
  30
  31 bool cancel_all_requests = false;
  32
  33 #define __qmi_service(_n) [__##_n] = _n
  34 static const uint8_t qmi_services[__QMI_SERVICE_LAST] = {
  35         __qmi_services
  36 };
  37 #undef __qmi_service
  38
  39 static union {
  40         char buf[512];
  41         struct qmi_msg msg;
  42 } msgbuf;
  43
  44 #ifdef DEBUG_PACKET
  45 void dump_packet(const char *prefix, void *ptr, int len)
  46 {
  47         unsigned char *data = ptr;
  48         int i;
  49
  50         fprintf(stderr, "%s:", prefix);
  51         for (i = 0; i < len; i++)
  52                 fprintf(stderr, " %02x", data[i]);
  53         fprintf(stderr, "\n");
  54 }
  55 #endif
  56
  57 static int
  58 qmi_get_service_idx(QmiService svc)
  59 {
  60         int i;
  61
  62         for (i = 0; i < ARRAY_SIZE(qmi_services); i++)
  63                 if (qmi_services[i] == svc)
  64                         return i;
  65
  66         return -1;
  67 }
  68
  69 static void __qmi_request_complete(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg)
  70 {
  71         void *tlv_buf;
  72         int tlv_len;
  73
  74         if (!req->pending)
  75                 return;
  76
  77         req->pending = false;
  78         list_del(&req->list);
  79
  80         if (msg) {
  81                 tlv_buf = qmi_msg_get_tlv_buf(msg, &tlv_len);
  82                 req->ret = qmi_check_message_status(tlv_buf, tlv_len);
  83                 if (req->ret)
  84                         msg = NULL;
  85         } else {
  86                 req->ret = QMI_ERROR_CANCELLED;
  87         }
  88
  89         if (req->cb && (msg || !req->no_error_cb))
  90                 req->cb(qmi, req, msg);
  91
  92         if (req->complete) {
  93                 *req->complete = true;
  94                 uloop_cancelled = true;
  95         }
  96 }
  97
  98 static void qmi_process_msg(struct qmi_dev *qmi, struct qmi_msg *msg)
  99 {
 100         struct qmi_request *req;
 101         uint16_t tid;
 102
 103         if (msg->qmux.service == QMI_SERVICE_CTL)
 104                 tid = msg->ctl.transaction;
 105         else
 106                 tid = le16_to_cpu(msg->svc.transaction);
 107
 108         list_for_each_entry(req, &qmi->req, list) {
 109                 if (req->service != msg->qmux.service)
 110                         continue;
 111
 112                 if (req->tid != tid)
 113                         continue;
 114
 115                 __qmi_request_complete(qmi, req, msg);
 116                 return;
 117         }
 118 }
 119
 120 static void qmi_notify_read(struct ustream *us, int bytes)
 121 {
 122         struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream);
 123         struct qmi_msg *msg;
 124         char *buf;
 125         int len, msg_len;
 126
 127         while (1) {
 128                 buf = ustream_get_read_buf(us, &len);
 129                 if (!buf || !len)
 130                         return;
 131
 132                 if (len < offsetof(struct qmi_msg, flags))
 133                         return;
 134
 135                 msg = (struct qmi_msg *) buf;
 136                 msg_len = le16_to_cpu(msg->qmux.len) + 1;
 137                 if (len < msg_len)
 138                         return;
 139
 140                 dump_packet("Received packet", msg, msg_len);
 141                 qmi_process_msg(qmi, msg);
 142                 ustream_consume(us, msg_len);
 143         }
 144 }
 145
 146 int qmi_request_start(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg, request_cb cb)
 147 {
 148         int len = qmi_complete_request_message(msg);
 149         uint16_t tid;
 150
 151         memset(req, 0, sizeof(*req));
 152         req->ret = -1;
 153         req->service = msg->qmux.service;
 154         if (req->service == QMI_SERVICE_CTL) {
 155                 tid = qmi->ctl_tid++;
 156                 msg->ctl.transaction = tid;
 157         } else {
 158                 int idx = qmi_get_service_idx(req->service);
 159
 160                 if (idx < 0)
 161                         return -1;
 162
 163                 tid = qmi->service_data[idx].tid++;
 164                 msg->svc.transaction = cpu_to_le16(tid);
 165                 msg->qmux.client = qmi->service_data[idx].client_id;
 166         }
 167
 168         req->tid = tid;
 169         req->cb = cb;
 170         req->pending = true;
 171         list_add(&req->list, &qmi->req);
 172
 173         dump_packet("Send packet", msg, len);
 174         ustream_write(&qmi->sf.stream, (void *) msg, len, false);
 175         return 0;
 176 }
 177
 178 void qmi_request_cancel(struct qmi_dev *qmi, struct qmi_request *req)
 179 {
 180         req->cb = NULL;
 181         __qmi_request_complete(qmi, req, NULL);
 182 }
 183
 184 int qmi_request_wait(struct qmi_dev *qmi, struct qmi_request *req)
 185 {
 186         bool complete = false;
 187         bool cancelled;
 188
 189         if (!req->pending)
 190                 return req->ret;
 191
 192         if (req->complete)
 193                 *req->complete = true;
 194
 195         req->complete = &complete;
 196         while (!complete) {
 197                 cancelled = uloop_cancelled;
 198                 uloop_cancelled = false;
 199                 uloop_run();
 200
 201                 if (cancel_all_requests)
 202                         qmi_request_cancel(qmi, req);
 203
 204                 uloop_cancelled = cancelled;
 205         }
 206
 207         if (req->complete == &complete)
 208                 req->complete = NULL;
 209
 210         return req->ret;
 211 }
 212
 213 struct qmi_connect_request {
 214         struct qmi_request req;
 215         int cid;
 216 };
 217
 218 static void qmi_connect_service_cb(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg)
 219 {
 220         struct qmi_ctl_allocate_cid_response res;
 221         struct qmi_connect_request *creq = container_of(req, struct qmi_connect_request, req);
 222
 223         if (!msg)
 224                 return;
 225
 226         qmi_parse_ctl_allocate_cid_response(msg, &res);
 227         creq->cid = res.data.allocation_info.cid;
 228 }
 229
 230 int qmi_service_connect(struct qmi_dev *qmi, QmiService svc, int client_id)
 231 {
 232         struct qmi_ctl_allocate_cid_request creq = {
 233                 QMI_INIT(service, svc)
 234         };
 235         struct qmi_connect_request req;
 236         int idx = qmi_get_service_idx(svc);
 237         struct qmi_msg *msg = &msgbuf.msg;
 238
 239         if (idx < 0)
 240                 return -1;
 241
 242         if (qmi->service_connected & (1 << idx))
 243                 return 0;
 244
 245         if (client_id < 0) {
 246                 qmi_set_ctl_allocate_cid_request(msg, &creq);
 247                 qmi_request_start(qmi, &req.req, msg, qmi_connect_service_cb);
 248                 qmi_request_wait(qmi, &req.req);
 249
 250                 if (req.req.ret)
 251                         return req.req.ret;
 252
 253                 client_id = req.cid;
 254         } else {
 255                 qmi->service_keep_cid |= (1 << idx);
 256         }
 257
 258         qmi->service_data[idx].connected = true;
 259         qmi->service_data[idx].client_id = client_id;
 260         qmi->service_data[idx].tid = 1;
 261         qmi->service_connected |= (1 << idx);
 262
 263         return 0;
 264 }
 265
 266 static void __qmi_service_disconnect(struct qmi_dev *qmi, int idx)
 267 {
 268         int client_id = qmi->service_data[idx].client_id;
 269         struct qmi_ctl_release_cid_request creq = {
 270                 QMI_INIT_SEQUENCE(release_info,
 271                         .service = qmi_services[idx],
 272                         .cid = client_id,
 273                 )
 274         };
 275         struct qmi_request req;
 276         struct qmi_msg *msg = &msgbuf.msg;
 277
 278         qmi->service_connected &= ~(1 << idx);
 279         qmi->service_data[idx].client_id = -1;
 280         qmi->service_data[idx].tid = 0;
 281
 282         qmi_set_ctl_release_cid_request(msg, &creq);
 283         qmi_request_start(qmi, &req, msg, NULL);
 284         qmi_request_wait(qmi, &req);
 285 }
 286
 287 int qmi_service_release_client_id(struct qmi_dev *qmi, QmiService svc)
 288 {
 289         int idx = qmi_get_service_idx(svc);
 290         qmi->service_release_cid |= 1 << idx;
 291         return 0;
 292 }
 293
 294 static void qmi_close_all_services(struct qmi_dev *qmi)
 295 {
 296         uint32_t connected = qmi->service_connected;
 297         int idx;
 298
 299         qmi->service_keep_cid &= ~qmi->service_release_cid;
 300         for (idx = 0; connected; idx++, connected >>= 1) {
 301                 if (!(connected & 1))
 302                         continue;
 303
 304                 if (qmi->service_keep_cid & (1 << idx))
 305                         continue;
 306
 307                 __qmi_service_disconnect(qmi, idx);
 308         }
 309 }
 310
 311 int qmi_service_get_client_id(struct qmi_dev *qmi, QmiService svc)
 312 {
 313         int idx = qmi_get_service_idx(svc);
 314
 315         if (idx < 0)
 316                 return -1;
 317
 318         qmi->service_keep_cid |= (1 << idx);
 319         return qmi->service_data[idx].client_id;
 320 }
 321
 322 int qmi_device_open(struct qmi_dev *qmi, const char *path)
 323 {
 324         struct ustream *us = &qmi->sf.stream;
 325         int fd;
 326
 327         uloop_init();
 328
 329         fd = open(path, O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY);
 330         if (fd < 0)
 331                 return -1;
 332
 333         us->notify_read = qmi_notify_read;
 334         ustream_fd_init(&qmi->sf, fd);
 335         INIT_LIST_HEAD(&qmi->req);
 336         qmi->ctl_tid = 1;
 337
 338         return 0;
 339 }
 340
 341 void qmi_device_close(struct qmi_dev *qmi)
 342 {
 343         struct qmi_request *req;
 344
 345         qmi_close_all_services(qmi);
 346         ustream_free(&qmi->sf.stream);
 347         close(qmi->sf.fd.fd);
 348
 349         while (!list_empty(&qmi->req)) {
 350                 req = list_first_entry(&qmi->req, struct qmi_request, list);
 351                 qmi_request_cancel(qmi, req);
 352         }
 353 }
 354
 355 QmiService qmi_service_get_by_name(const char *str)
 356 {
 357         static const struct {
 358                 const char *name;
 359                 QmiService svc;
 360         } services[] = {
 361                 { "dms", QMI_SERVICE_DMS },
 362                 { "nas", QMI_SERVICE_NAS },
 363                 { "pds", QMI_SERVICE_PDS },
 364                 { "wds", QMI_SERVICE_WDS },
 365                 { "wms", QMI_SERVICE_WMS },
 366                 { "wda", QMI_SERVICE_WDA },
 367         };
 368         int i;
 369
 370         for (i = 0; i < ARRAY_SIZE(services); i++) {
 371                 if (!strcasecmp(str, services[i].name))
 372                         return services[i].svc;
 373         }
 374
 375         return -1;
 376 }
 377
 378 const char *qmi_get_error_str(int code)
 379 {
 380         int i;
 381
 382         for (i = 0; i < ARRAY_SIZE(qmi_errors); i++) {
 383                 if (qmi_errors[i].code == code)
 384                         return qmi_errors[i].text;
 385         }
 386
 387         return "Unknown error";
 388 }