dev.c

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