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