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