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