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