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         }
 234
 235         qmi->service_data[idx].connected = true;
 236         qmi->service_data[idx].client_id = client_id;
 237         qmi->service_data[idx].tid = 1;
 238         qmi->service_connected |= (1 << idx);
 239
 240         return 0;
 241 }
 242
 243 static void __qmi_service_disconnect(struct qmi_dev *qmi, int idx)
 244 {
 245         int client_id = qmi->service_data[idx].client_id;
 246         struct qmi_ctl_release_cid_request creq = {
 247                 QMI_INIT_SEQUENCE(release_info,
 248                         .service = qmi_services[idx],
 249                         .cid = client_id,
 250                 )
 251         };
 252         struct qmi_request req;
 253         struct qmi_msg *msg = &msgbuf.msg;
 254
 255         qmi->service_connected &= ~(1 << idx);
 256         qmi->service_data[idx].client_id = -1;
 257         qmi->service_data[idx].tid = 0;
 258
 259         qmi_set_ctl_release_cid_request(msg, &creq);
 260         qmi_request_start(qmi, &req, msg, NULL);
 261         qmi_request_wait(qmi, &req);
 262 }
 263
 264 int qmi_service_release_client_id(struct qmi_dev *qmi, QmiService svc)
 265 {
 266         int idx = qmi_get_service_idx(svc);
 267         qmi->service_keep_cid &= ~(1 << idx);
 268         return 0;
 269 }
 270
 271 static void qmi_close_all_services(struct qmi_dev *qmi)
 272 {
 273         uint32_t connected = qmi->service_connected;
 274         int idx;
 275
 276         for (idx = 0; connected; idx++, connected >>= 1) {
 277                 if (!(connected & 1))
 278                         continue;
 279
 280                 if (qmi->service_keep_cid & (1 << idx))
 281                         continue;
 282
 283                 __qmi_service_disconnect(qmi, idx);
 284         }
 285 }
 286
 287 int qmi_service_get_client_id(struct qmi_dev *qmi, QmiService svc)
 288 {
 289         int idx = qmi_get_service_idx(svc);
 290
 291         if (idx < 0)
 292                 return -1;
 293
 294         qmi->service_keep_cid |= (1 << idx);
 295         return qmi->service_data[idx].client_id;
 296 }
 297
 298 int qmi_device_open(struct qmi_dev *qmi, const char *path)
 299 {
 300         struct ustream *us = &qmi->sf.stream;
 301         int fd;
 302
 303         uloop_init();
 304
 305         fd = open(path, O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY);
 306         if (fd < 0)
 307                 return -1;
 308
 309         us->notify_read = qmi_notify_read;
 310         ustream_fd_init(&qmi->sf, fd);
 311         INIT_LIST_HEAD(&qmi->req);
 312         qmi->ctl_tid = 1;
 313
 314         return 0;
 315 }
 316
 317 void qmi_device_close(struct qmi_dev *qmi)
 318 {
 319         struct qmi_request *req;
 320
 321         qmi_close_all_services(qmi);
 322         ustream_free(&qmi->sf.stream);
 323         close(qmi->sf.fd.fd);
 324
 325         while (!list_empty(&qmi->req)) {
 326                 req = list_first_entry(&qmi->req, struct qmi_request, list);
 327                 qmi_request_cancel(qmi, req);
 328         }
 329 }
 330
 331 QmiService qmi_service_get_by_name(const char *str)
 332 {
 333         static const struct {
 334                 const char *name;
 335                 QmiService svc;
 336         } services[] = {
 337                 { "dms", QMI_SERVICE_DMS },
 338                 { "nas", QMI_SERVICE_NAS },
 339                 { "pds", QMI_SERVICE_PDS },
 340                 { "wds", QMI_SERVICE_WDS },
 341                 { "wms", QMI_SERVICE_WMS },
 342         };
 343         int i;
 344
 345         for (i = 0; i < ARRAY_SIZE(services); i++) {
 346                 if (!strcasecmp(str, services[i].name))
 347                         return services[i].svc;
 348         }
 349
 350         return -1;
 351 }
 352
 353 const char *qmi_get_error_str(int code)
 354 {
 355         int i;
 356
 357         for (i = 0; i < ARRAY_SIZE(qmi_errors); i++) {
 358                 if (qmi_errors[i].code == code)
 359                         return qmi_errors[i].text;
 360         }
 361
 362         return "Unknown error";
 363 }