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 void __qmi_request_complete(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg) 66 { 67 void *tlv_buf; 68 int tlv_len; 69 70 if (!req->pending) 71 return; 72 73 req->pending = false; 74 list_del(&req->list); 75 76 if (msg) { 77 tlv_buf = qmi_msg_get_tlv_buf(msg, &tlv_len); 78 req->ret = qmi_check_message_status(tlv_buf, tlv_len); 79 if (req->ret) 80 msg = NULL; 81 } else { 82 req->ret = QMI_ERROR_CANCELLED; 83 } 84 85 if (req->cb && (msg || !req->no_error_cb)) 86 req->cb(qmi, req, msg); 87 88 if (req->complete) { 89 *req->complete = true; 90 uloop_cancelled = true; 91 } 92 } 93 94 static void qmi_process_msg(struct qmi_dev *qmi, struct qmi_msg *msg) 95 { 96 struct qmi_request *req; 97 uint16_t tid; 98 99 if (msg->flags != QMI_CTL_FLAG_RESPONSE && msg->flags != QMI_SERVICE_FLAG_RESPONSE) 100 return; 101 102 if (msg->qmux.service == QMI_SERVICE_CTL) 103 tid = msg->ctl.transaction; 104 else 105 tid = le16_to_cpu(msg->svc.transaction); 106 107 list_for_each_entry(req, &qmi->req, list) { 108 if (req->service != msg->qmux.service) 109 continue; 110 111 if (req->tid != tid) 112 continue; 113 114 __qmi_request_complete(qmi, req, msg); 115 return; 116 } 117 } 118 119 static void qmi_notify_read(struct ustream *us, int bytes) 120 { 121 struct qmi_dev *qmi = container_of(us, struct qmi_dev, sf.stream); 122 struct qmi_msg *msg; 123 char *buf; 124 int len, msg_len; 125 126 127 while (1) { 128 buf = ustream_get_read_buf(us, &len); 129 if (!buf || !len) 130 return; 131 132 dump_packet("Received packet", buf, len); 133 if (qmi->is_mbim) { 134 struct mbim_command_message *mbim = (void *) buf; 135 136 if (len < sizeof(*mbim)) 137 return; 138 msg = (struct qmi_msg *) (buf + sizeof(*mbim)); 139 msg_len = le32_to_cpu(mbim->header.length); 140 if (!is_mbim_qmi(mbim)) { 141 /* must consume other MBIM packets */ 142 ustream_consume(us, msg_len); 143 return; 144 } 145 } else { 146 if (len < offsetof(struct qmi_msg, flags)) 147 return; 148 msg = (struct qmi_msg *) buf; 149 msg_len = le16_to_cpu(msg->qmux.len) + 1; 150 } 151 152 if (len < msg_len) 153 return; 154 155 qmi_process_msg(qmi, msg); 156 ustream_consume(us, msg_len); 157 } 158 } 159 160 int qmi_request_start(struct qmi_dev *qmi, struct qmi_request *req, request_cb cb) 161 { 162 struct qmi_msg *msg = qmi->buf; 163 int len = qmi_complete_request_message(msg); 164 uint16_t tid; 165 void *buf = (void *) qmi->buf; 166 167 memset(req, 0, sizeof(*req)); 168 req->ret = -1; 169 req->service = msg->qmux.service; 170 if (req->service == QMI_SERVICE_CTL) { 171 tid = qmi->ctl_tid++; 172 msg->ctl.transaction = tid; 173 } else { 174 int idx = qmi_get_service_idx(req->service); 175 176 if (idx < 0) 177 return -1; 178 179 tid = qmi->service_data[idx].tid++; 180 msg->svc.transaction = cpu_to_le16(tid); 181 msg->qmux.client = qmi->service_data[idx].client_id; 182 } 183 184 req->tid = tid; 185 req->cb = cb; 186 req->pending = true; 187 list_add(&req->list, &qmi->req); 188 189 if (qmi->is_mbim) { 190 buf -= sizeof(struct mbim_command_message); 191 mbim_qmi_cmd((struct mbim_command_message *) buf, len, tid); 192 len += sizeof(struct mbim_command_message); 193 } 194 195 dump_packet("Send packet", buf, len); 196 ustream_write(&qmi->sf.stream, buf, len, false); 197 return 0; 198 } 199 200 void qmi_request_cancel(struct qmi_dev *qmi, struct qmi_request *req) 201 { 202 req->cb = NULL; 203 __qmi_request_complete(qmi, req, NULL); 204 } 205 206 int qmi_request_wait(struct qmi_dev *qmi, struct qmi_request *req) 207 { 208 bool complete = false; 209 bool cancelled; 210 211 if (!req->pending) 212 return req->ret; 213 214 if (req->complete) 215 *req->complete = true; 216 217 req->complete = &complete; 218 while (!complete) { 219 cancelled = uloop_cancelled; 220 uloop_cancelled = false; 221 uloop_run(); 222 223 if (cancel_all_requests) 224 qmi_request_cancel(qmi, req); 225 226 uloop_cancelled = cancelled; 227 } 228 229 if (req->complete == &complete) 230 req->complete = NULL; 231 232 return req->ret; 233 } 234 235 struct qmi_connect_request { 236 struct qmi_request req; 237 int cid; 238 }; 239 240 static void qmi_connect_service_cb(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_msg *msg) 241 { 242 struct qmi_ctl_allocate_cid_response res; 243 struct qmi_connect_request *creq = container_of(req, struct qmi_connect_request, req); 244 245 if (!msg) 246 return; 247 248 qmi_parse_ctl_allocate_cid_response(msg, &res); 249 creq->cid = res.data.allocation_info.cid; 250 } 251 252 int qmi_service_connect(struct qmi_dev *qmi, QmiService svc, int client_id) 253 { 254 struct qmi_ctl_allocate_cid_request creq = { 255 QMI_INIT(service, svc) 256 }; 257 struct qmi_connect_request req; 258 int idx = qmi_get_service_idx(svc); 259 struct qmi_msg *msg = qmi->buf; 260 261 if (idx < 0) 262 return -1; 263 264 if (qmi->service_connected & (1 << idx)) 265 return 0; 266 267 if (client_id < 0) { 268 qmi_set_ctl_allocate_cid_request(msg, &creq); 269 qmi_request_start(qmi, &req.req, qmi_connect_service_cb); 270 qmi_request_wait(qmi, &req.req); 271 272 if (req.req.ret) 273 return req.req.ret; 274 275 client_id = req.cid; 276 } else { 277 qmi->service_keep_cid |= (1 << idx); 278 } 279 280 qmi->service_data[idx].connected = true; 281 qmi->service_data[idx].client_id = client_id; 282 qmi->service_data[idx].tid = 1; 283 qmi->service_connected |= (1 << idx); 284 285 return 0; 286 } 287 288 static void __qmi_service_disconnect(struct qmi_dev *qmi, int idx) 289 { 290 int client_id = qmi->service_data[idx].client_id; 291 struct qmi_ctl_release_cid_request creq = { 292 QMI_INIT_SEQUENCE(release_info, 293 .service = qmi_services[idx], 294 .cid = client_id, 295 ) 296 }; 297 struct qmi_request req; 298 struct qmi_msg *msg = qmi->buf; 299 300 qmi->service_connected &= ~(1 << idx); 301 qmi->service_data[idx].client_id = -1; 302 qmi->service_data[idx].tid = 0; 303 304 qmi_set_ctl_release_cid_request(msg, &creq); 305 qmi_request_start(qmi, &req, NULL); 306 qmi_request_wait(qmi, &req); 307 } 308 309 int qmi_service_release_client_id(struct qmi_dev *qmi, QmiService svc) 310 { 311 int idx = qmi_get_service_idx(svc); 312 qmi->service_release_cid |= 1 << idx; 313 return 0; 314 } 315 316 static void qmi_close_all_services(struct qmi_dev *qmi) 317 { 318 uint32_t connected = qmi->service_connected; 319 int idx; 320 321 qmi->service_keep_cid &= ~qmi->service_release_cid; 322 for (idx = 0; connected; idx++, connected >>= 1) { 323 if (!(connected & 1)) 324 continue; 325 326 if (qmi->service_keep_cid & (1 << idx)) 327 continue; 328 329 __qmi_service_disconnect(qmi, idx); 330 } 331 } 332 333 int qmi_service_get_client_id(struct qmi_dev *qmi, QmiService svc) 334 { 335 int idx = qmi_get_service_idx(svc); 336 337 if (idx < 0) 338 return -1; 339 340 qmi->service_keep_cid |= (1 << idx); 341 return qmi->service_data[idx].client_id; 342 } 343 344 int qmi_device_open(struct qmi_dev *qmi, const char *path) 345 { 346 static struct { 347 struct mbim_command_message mbim; 348 union { 349 char buf[2048]; 350 struct qmi_msg msg; 351 } u; 352 } __packed msgbuf; 353 struct ustream *us = &qmi->sf.stream; 354 int fd; 355 356 uloop_init(); 357 358 fd = open(path, O_RDWR | O_EXCL | O_NONBLOCK | O_NOCTTY); 359 if (fd < 0) 360 return -1; 361 362 us->notify_read = qmi_notify_read; 363 ustream_fd_init(&qmi->sf, fd); 364 INIT_LIST_HEAD(&qmi->req); 365 qmi->ctl_tid = 1; 366 qmi->buf = msgbuf.u.buf; 367 368 return 0; 369 } 370 371 void qmi_device_close(struct qmi_dev *qmi) 372 { 373 struct qmi_request *req; 374 375 qmi_close_all_services(qmi); 376 ustream_free(&qmi->sf.stream); 377 close(qmi->sf.fd.fd); 378 379 while (!list_empty(&qmi->req)) { 380 req = list_first_entry(&qmi->req, struct qmi_request, list); 381 qmi_request_cancel(qmi, req); 382 } 383 } 384 385 QmiService qmi_service_get_by_name(const char *str) 386 { 387 static const struct { 388 const char *name; 389 QmiService svc; 390 } services[] = { 391 { "dms", QMI_SERVICE_DMS }, 392 { "nas", QMI_SERVICE_NAS }, 393 { "pds", QMI_SERVICE_PDS }, 394 { "wds", QMI_SERVICE_WDS }, 395 { "wms", QMI_SERVICE_WMS }, 396 { "wda", QMI_SERVICE_WDA }, 397 { "uim", QMI_SERVICE_UIM }, 398 }; 399 int i; 400 401 for (i = 0; i < ARRAY_SIZE(services); i++) { 402 if (!strcasecmp(str, services[i].name)) 403 return services[i].svc; 404 } 405 406 return -1; 407 } 408 409 const char *qmi_get_error_str(int code) 410 { 411 int i; 412 413 for (i = 0; i < ARRAY_SIZE(qmi_errors); i++) { 414 if (qmi_errors[i].code == code) 415 return qmi_errors[i].text; 416 } 417 418 return "Unknown error"; 419 } 420
This page was automatically generated by LXR 0.3.1. • OpenWrt