1 /* 2 * uqmi -- tiny QMI support implementation 3 * 4 * Copyright (C) 2014-2015 Felix Fietkau <nbd@openwrt.org> 5 * 6 * This library is free software; you can redistribute it and/or 7 * modify it under the terms of the GNU Lesser General Public 8 * License as published by the Free Software Foundation; either 9 * version 2 of the License, or (at your option) any later version. 10 * 11 * This library is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 * Lesser General Public License for more details. 15 * 16 * You should have received a copy of the GNU Lesser General Public 17 * License along with this library; if not, write to the 18 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, 19 * Boston, MA 02110-1301 USA. 20 */ 21 22 #include <fcntl.h> 23 #include <unistd.h> 24 #include <stdlib.h> 25 #include <stdio.h> 26 #include <string.h> 27 #include <strings.h> 28 #include "uqmi.h" 29 #include "qmi-errors.h" 30 #include "qmi-errors.c" 31 #include "mbim.h" 32 33 bool cancel_all_requests = false; 34 35 #define __qmi_service(_n) [__##_n] = _n 36 static const uint8_t qmi_services[__QMI_SERVICE_LAST] = { 37 __qmi_services 38 }; 39 #undef __qmi_service 40 41 #ifdef DEBUG_PACKET 42 void dump_packet(const char *prefix, void *ptr, int len) 43 { 44 unsigned char *data = ptr; 45 int i; 46 47 fprintf(stderr, "%s:", prefix); 48 for (i = 0; i < len; i++) 49 fprintf(stderr, " %02x", data[i]); 50 fprintf(stderr, "\n"); 51 } 52 #endif 53 54 static int 55 qmi_get_service_idx(QmiService svc) 56 { 57 int i; 58 59 for (i = 0; i < ARRAY_SIZE(qmi_services); i++) 60 if (qmi_services[i] == svc) 61 return i; 62 63 return -1; 64 } 65 66 static bool qmi_message_is_response(struct qmi_msg *msg) 67 { 68 if (msg->qmux.service == QMI_SERVICE_CTL) { 69 if (msg->flags & QMI_CTL_FLAG_RESPONSE) 70 return true; 71 } 72 else { 73 if (msg->flags & QMI_SERVICE_FLAG_RESPONSE) 74 return true; 75 } 76 77 return false; 78 } 79 80 static bool qmi_message_is_indication(struct qmi_msg *msg) 81 { 82 if (msg->qmux.service == QMI_SERVICE_CTL) { 83 if (msg->flags & QMI_CTL_FLAG_INDICATION) 84 return true; 85 } 86 else { 87 if (msg->flags & QMI_SERVICE_FLAG_INDICATION) 88 return true; 89 } 90 91 return false; 92 } 93 94 static void __qmi_request_complete(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg) 95 { 96 void *tlv_buf; 97 int tlv_len; 98 99 if (!req->pending) 100 return; 101 102 req->pending = false; 103 list_del(&req->list); 104 105 if (msg) { 106 tlv_buf = qmi_msg_get_tlv_buf(msg, &tlv_len); 107 req->ret = qmi_check_message_status(tlv_buf, tlv_len); 108 if (req->ret) 109 msg = NULL; 110 } else { 111 req->ret = QMI_ERROR_CANCELLED; 112 } 113 114 if (req->cb && (msg || !req->no_error_cb)) 115 req->cb(qmi, req, msg); 116 117 if (req->complete) { 118 *req->complete = true; 119 uloop_cancelled = true; 120 } 121 } 122 123 static void qmi_process_msg(struct qmi_dev *qmi, struct qmi_msg *msg) 124 { 125 struct qmi_request *req; 126 uint16_t tid; 127 128 if (qmi_message_is_indication(msg)) { 129 if (msg->qmux.service == QMI_SERVICE_CTL) { 130 struct qmi_msg sync_msg = {0}; 131 qmi_set_ctl_sync_request(&sync_msg); 132 /* A SYNC indication might be sent on boot in order to indicate 133 * that all Client IDs have been deallocated by the modem: 134 * cancel all requests, as they will not be answered. */ 135 if (msg->ctl.message == sync_msg.ctl.message) { 136 while (!list_empty(&qmi->req)) { 137 req = list_first_entry(&qmi->req, struct qmi_request, list); 138 qmi_request_cancel(qmi, req); 139 } 140 } 141 } 142 143 return; 144 } 145 146 if (!qmi_message_is_response(msg)) 147 return; 148 149 if (msg->qmux.service == QMI_SERVICE_CTL) 150 tid = msg->ctl.transaction; 151 else 152 tid = le16_to_cpu(msg->svc.transaction); 153 154 list_for_each_entry(req, &qmi->req, list) { 155 if (req->service != msg->qmux.service) 156 continue; 157 158 if (req->tid != tid) 159 continue; 160 161 __qmi_request_complete(qmi, req, msg); 162 return; 163 } 164 } 165 166 static void qmi_notify_read(struct ustream *us, int bytes) 167 { 168 struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream); 169 struct qmi_msg *msg; 170 char *buf; 171 int len, msg_len; 172 173 174 while (1) { 175 buf = ustream_get_read_buf(us, &len); 176 if (!buf || !len) 177 return; 178 179 dump_packet("Received packet", buf, len); 180 if (qmi->is_mbim) { 181 struct mbim_command_message *mbim = (void *) buf; 182 183 if (len < sizeof(*mbim)) 184 return; 185 msg = (struct qmi_msg *) (buf + sizeof(*mbim)); 186 msg_len = le32_to_cpu(mbim->header.length); 187 if (!is_mbim_qmi(mbim)) { 188 /* must consume other MBIM packets */ 189 ustream_consume(us, msg_len); 190 return; 191 } 192 } else { 193 if (len < offsetof(struct qmi_msg, flags)) 194 return; 195 msg = (struct qmi_msg *) buf; 196 msg_len = le16_to_cpu(msg->qmux.len) + 1; 197 } 198 199 if (len < msg_len) 200 return; 201 202 qmi_process_msg(qmi, msg); 203 ustream_consume(us, msg_len); 204 } 205 } 206 207 int qmi_request_start(struct qmi_dev *qmi, struct qmi_request *req, request_cb cb) 208 { 209 struct qmi_msg *msg = qmi->buf; 210 int len = qmi_complete_request_message(msg); 211 uint16_t tid; 212 void *buf = (void *) qmi->buf; 213 214 memset(req, 0, sizeof(*req)); 215 req->ret = -1; 216 req->service = msg->qmux.service; 217 if (req->service == QMI_SERVICE_CTL) { 218 tid = qmi->ctl_tid++; 219 msg->ctl.transaction = tid; 220 } else { 221 int idx = qmi_get_service_idx(req->service); 222 223 if (idx < 0) 224 return -1; 225 226 tid = qmi->service_data[idx].tid++; 227 msg->svc.transaction = cpu_to_le16(tid); 228 msg->qmux.client = qmi->service_data[idx].client_id; 229 } 230 231 req->tid = tid; 232 req->cb = cb; 233 req->pending = true; 234 list_add(&req->list, &qmi->req); 235 236 if (qmi->is_mbim) { 237 buf -= sizeof(struct mbim_command_message); 238 mbim_qmi_cmd((struct mbim_command_message *) buf, len, tid); 239 len += sizeof(struct mbim_command_message); 240 } 241 242 dump_packet("Send packet", buf, len); 243 ustream_write(&qmi->sf.stream, buf, len, false); 244 return 0; 245 } 246 247 void qmi_request_cancel(struct qmi_dev *qmi, struct qmi_request *req) 248 { 249 req->cb = NULL; 250 __qmi_request_complete(qmi, req, NULL); 251 } 252 253 /*! Run uloop until the request has been completed 254 * 255 * \param qmi the qmi device 256 * \param req the request to wait for 257 * \return req->ret (0 on success) 258 */ 259 int qmi_request_wait(struct qmi_dev *qmi, struct qmi_request *req) 260 { 261 bool complete = false; 262 bool cancelled; 263 264 if (!req->pending) 265 return req->ret; 266 267 if (req->complete) 268 *req->complete = true; 269 270 req->complete = &complete; 271 while (!complete) { 272 cancelled = uloop_cancelled; 273 uloop_cancelled = false; 274 uloop_run(); 275 276 if (cancel_all_requests) 277 qmi_request_cancel(qmi, req); 278 279 uloop_cancelled = cancelled; 280 } 281 282 if (req->complete == &complete) 283 req->complete = NULL; 284 285 return req->ret; 286 } 287 288 struct qmi_connect_request { 289 struct qmi_request req; 290 int cid; 291 }; 292 293 static void qmi_connect_service_cb(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg) 294 { 295 struct qmi_ctl_allocate_cid_response res; 296 struct qmi_connect_request *creq = container_of(req, struct qmi_connect_request, req); 297 298 if (!msg) 299 return; 300 301 qmi_parse_ctl_allocate_cid_response(msg, &res); 302 creq->cid = res.data.allocation_info.cid; 303 } 304 305 int qmi_service_connect(struct qmi_dev *qmi, QmiService svc, int client_id) 306 { 307 struct qmi_ctl_allocate_cid_request creq = { 308 QMI_INIT(service, svc) 309 }; 310 struct qmi_connect_request req; 311 int idx = qmi_get_service_idx(svc); 312 struct qmi_msg *msg = qmi->buf; 313 314 if (idx < 0) 315 return -1; 316 317 if (qmi->service_connected & (1 << idx)) 318 return 0; 319 320 if (client_id < 0) { 321 qmi_set_ctl_allocate_cid_request(msg, &creq); 322 qmi_request_start(qmi, &req.req, qmi_connect_service_cb); 323 qmi_request_wait(qmi, &req.req); 324 325 if (req.req.ret) 326 return req.req.ret; 327 328 client_id = req.cid; 329 } else { 330 qmi->service_keep_cid |= (1 << idx); 331 } 332 333 qmi->service_data[idx].connected = true; 334 qmi->service_data[idx].client_id = client_id; 335 qmi->service_data[idx].tid = 1; 336 qmi->service_connected |= (1 << idx); 337 338 return 0; 339 } 340 341 static void __qmi_service_disconnect(struct qmi_dev *qmi, int idx) 342 { 343 int client_id = qmi->service_data[idx].client_id; 344 struct qmi_ctl_release_cid_request creq = { 345 QMI_INIT_SEQUENCE(release_info, 346 .service = qmi_services[idx], 347 .cid = client_id, 348 ) 349 }; 350 struct qmi_request req; 351 struct qmi_msg *msg = qmi->buf; 352 353 qmi->service_connected &= ~(1 << idx); 354 qmi->service_data[idx].client_id = -1; 355 qmi->service_data[idx].tid = 0; 356 357 qmi_set_ctl_release_cid_request(msg, &creq); 358 qmi_request_start(qmi, &req, NULL); 359 qmi_request_wait(qmi, &req); 360 } 361 362 int qmi_service_release_client_id(struct qmi_dev *qmi, QmiService svc) 363 { 364 int idx = qmi_get_service_idx(svc); 365 qmi->service_release_cid |= 1 << idx; 366 return 0; 367 } 368 369 static void qmi_close_all_services(struct qmi_dev *qmi) 370 { 371 uint32_t connected = qmi->service_connected; 372 int idx; 373 374 qmi->service_keep_cid &= ~qmi->service_release_cid; 375 for (idx = 0; connected; idx++, connected >>= 1) { 376 if (!(connected & 1)) 377 continue; 378 379 if (qmi->service_keep_cid & (1 << idx)) 380 continue; 381 382 __qmi_service_disconnect(qmi, idx); 383 } 384 } 385 386 int qmi_service_get_client_id(struct qmi_dev *qmi, QmiService svc) 387 { 388 int idx = qmi_get_service_idx(svc); 389 390 if (idx < 0) 391 return -1; 392 393 qmi->service_keep_cid |= (1 << idx); 394 return qmi->service_data[idx].client_id; 395 } 396 397 int qmi_device_open(struct qmi_dev *qmi, const char *path) 398 { 399 static struct { 400 struct mbim_command_message mbim; 401 union { 402 char buf[2048]; 403 struct qmi_msg msg; 404 } u; 405 } __packed msgbuf; 406 struct ustream *us = &qmi->sf.stream; 407 int fd; 408 409 uloop_init(); 410 411 fd = open(path, O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY); 412 if (fd < 0) 413 return -1; 414 415 us->notify_read = qmi_notify_read; 416 ustream_fd_init(&qmi->sf, fd); 417 INIT_LIST_HEAD(&qmi->req); 418 qmi->ctl_tid = 1; 419 qmi->buf = msgbuf.u.buf; 420 421 return 0; 422 } 423 424 void qmi_device_close(struct qmi_dev *qmi) 425 { 426 struct qmi_request *req; 427 428 qmi_close_all_services(qmi); 429 ustream_free(&qmi->sf.stream); 430 close(qmi->sf.fd.fd); 431 432 while (!list_empty(&qmi->req)) { 433 req = list_first_entry(&qmi->req, struct qmi_request, list); 434 qmi_request_cancel(qmi, req); 435 } 436 } 437 438 QmiService qmi_service_get_by_name(const char *str) 439 { 440 static const struct { 441 const char *name; 442 QmiService svc; 443 } services[] = { 444 { "dms", QMI_SERVICE_DMS }, 445 { "nas", QMI_SERVICE_NAS }, 446 { "pds", QMI_SERVICE_PDS }, 447 { "wds", QMI_SERVICE_WDS }, 448 { "wms", QMI_SERVICE_WMS }, 449 { "wda", QMI_SERVICE_WDA }, 450 { "uim", QMI_SERVICE_UIM }, 451 }; 452 int i; 453 454 for (i = 0; i < ARRAY_SIZE(services); i++) { 455 if (!strcasecmp(str, services[i].name)) 456 return services[i].svc; 457 } 458 459 return -1; 460 } 461
This page was automatically generated by LXR 0.3.1. • OpenWrt