diff options
| -rw-r--r-- | CMakeLists.txt | 2 | ||||
| -rw-r--r-- | dev.c | 52 | ||||
| -rw-r--r-- | main.c | 7 | ||||
| -rw-r--r-- | mbim.c | 51 | ||||
| -rw-r--r-- | mbim.h | 55 | ||||
| -rw-r--r-- | uqmi.h | 2 |
6 files changed, 154 insertions, 15 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index 3bb0230..d91411a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ ADD_DEFINITIONS(-Os -ggdb -Wall -Werror --std=gnu99 -Wmissing-declarations -Wno- SET(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") -SET(SOURCES main.c dev.c commands.c qmi-message.c) +SET(SOURCES main.c dev.c commands.c qmi-message.c mbim.c) FIND_PATH(ubox_include_dir libubox/usock.h) FIND_PATH(blobmsg_json_include_dir libubox/blobmsg_json.h) @@ -27,6 +27,7 @@ #include "uqmi.h" #include "qmi-errors.h" #include "qmi-errors.c" +#include "mbim.h" bool cancel_all_requests = false; @@ -36,10 +37,13 @@ static const uint8_t qmi_services[__QMI_SERVICE_LAST] = { }; #undef __qmi_service -static union { - char buf[512]; - struct qmi_msg msg; -} msgbuf; +static struct { + struct mbim_command_message mbim; + union { + char buf[512]; + struct qmi_msg msg; + } u; +} __attribute__((packed)) msgbuf; #ifdef DEBUG_PACKET void dump_packet(const char *prefix, void *ptr, int len) @@ -124,20 +128,35 @@ static void qmi_notify_read(struct ustream *us, int bytes) char *buf; int len, msg_len; + while (1) { buf = ustream_get_read_buf(us, &len); if (!buf || !len) return; - if (len < offsetof(struct qmi_msg, flags)) - return; + dump_packet("Received packet", buf, len); + if (qmi->is_mbim) { + struct mbim_command_message *mbim = (void *) buf; + + if (len < sizeof(*mbim)) + return; + msg = (struct qmi_msg *) (buf + sizeof(*mbim)); + msg_len = le32toh(mbim->header.length); + if (!is_mbim_qmi(mbim)) { + /* must consume other MBIM packets */ + ustream_consume(us, msg_len); + return; + } + } else { + if (len < offsetof(struct qmi_msg, flags)) + return; + msg = (struct qmi_msg *) buf; + msg_len = le16_to_cpu(msg->qmux.len) + 1; + } - msg = (struct qmi_msg *) buf; - msg_len = le16_to_cpu(msg->qmux.len) + 1; if (len < msg_len) return; - dump_packet("Received packet", msg, msg_len); qmi_process_msg(qmi, msg); ustream_consume(us, msg_len); } @@ -147,6 +166,7 @@ int qmi_request_start(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_m { int len = qmi_complete_request_message(msg); uint16_t tid; + char *buf = (void *) msg; memset(req, 0, sizeof(*req)); req->ret = -1; @@ -170,8 +190,14 @@ int qmi_request_start(struct qmi_dev *qmi, struct qmi_request *req, struct qmi_m req->pending = true; list_add(&req->list, &qmi->req); - dump_packet("Send packet", msg, len); - ustream_write(&qmi->sf.stream, (void *) msg, len, false); + if (qmi->is_mbim) { + buf -= sizeof(struct mbim_command_message); + mbim_qmi_cmd((struct mbim_command_message *) buf, len, tid); + len += sizeof(struct mbim_command_message); + } + + dump_packet("Send packet", buf, len); + ustream_write(&qmi->sf.stream, buf, len, false); return 0; } @@ -234,7 +260,7 @@ int qmi_service_connect(struct qmi_dev *qmi, QmiService svc, int client_id) }; struct qmi_connect_request req; int idx = qmi_get_service_idx(svc); - struct qmi_msg *msg = &msgbuf.msg; + struct qmi_msg *msg = &msgbuf.u.msg; if (idx < 0) return -1; @@ -273,7 +299,7 @@ static void __qmi_service_disconnect(struct qmi_dev *qmi, int idx) ) }; struct qmi_request req; - struct qmi_msg *msg = &msgbuf.msg; + struct qmi_msg *msg = &msgbuf.u.msg; qmi->service_connected &= ~(1 << idx); qmi->service_data[idx].client_id = -1; @@ -43,6 +43,7 @@ static const struct option uqmi_getopt[] = { { "device", required_argument, NULL, 'd' }, { "keep-client-id", required_argument, NULL, 'k' }, { "release-client-id", required_argument, NULL, 'r' }, + { "device-is-mbim", no_argument, NULL, 'm' }, { NULL, 0, NULL, 0 } }; #undef __uqmi_command @@ -55,6 +56,7 @@ static int usage(const char *progname) " --device=NAME, -d NAME: Set device name to NAME (required)\n" " --keep-client-id <name>: Keep Client ID for service <name>\n" " --release-client-id <name>: Release Client ID after exiting\n" + " --device-is-mbim, -m NAME is an MBIM device with EXT_QMUX support\n" "\n" "Services: dms, nas, pds, wds, wms\n" "\n" @@ -108,7 +110,7 @@ int main(int argc, char **argv) signal(SIGINT, handle_exit_signal); signal(SIGTERM, handle_exit_signal); - while ((ch = getopt_long(argc, argv, "d:k:s", uqmi_getopt, NULL)) != -1) { + while ((ch = getopt_long(argc, argv, "d:k:sm", uqmi_getopt, NULL)) != -1) { int cmd_opt = CMD_OPT(ch); if (ch < 0 && cmd_opt >= 0 && cmd_opt < __UQMI_COMMAND_LAST) { @@ -129,6 +131,9 @@ int main(int argc, char **argv) case 's': single_line = true; break; + case 'm': + dev.is_mbim = true; + break; default: return usage(argv[0]); } @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2016 Bjørn Mork <bjorn@mork.no> + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#include <endian.h> +#include <stdbool.h> +#include <stdint.h> +#include <string.h> + +#include <stdio.h> + +#include "mbim.h" + +static const uint8_t qmiuuid[16] = { 0xd1, 0xa3, 0x0b, 0xc2, 0xf9, 0x7a, 0x6e, 0x43, + 0xbf, 0x65, 0xc7, 0xe2, 0x4f, 0xb0, 0xf0, 0xd3 }; + +bool is_mbim_qmi(struct mbim_command_message *msg) +{ + return msg->header.type == htole32(MBIM_MESSAGE_TYPE_COMMAND_DONE) && + msg->command_id == htole32(MBIM_CID_QMI_MSG) && + !msg->command_type && /* actually 'status' here */ + !memcmp(msg->service_id, qmiuuid, 16); + } + +void mbim_qmi_cmd(struct mbim_command_message *msg, int len, uint16_t tid) +{ + msg->header.type = htole32(MBIM_MESSAGE_TYPE_COMMAND); + msg->header.length = sizeof(*msg) + len; + msg->header.transaction_id = htole32(tid); + msg->fragment_header.total = 1; + msg->fragment_header.current = 0; + memcpy(msg->service_id, qmiuuid, 16); + msg->command_id = htole32(MBIM_CID_QMI_MSG); + msg->command_type = htole32(MBIM_MESSAGE_COMMAND_TYPE_SET); + msg->buffer_length = len; +} @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 John Crispin <blogic@openwrt.org> + * Copyright (C) 2016 Bjørn Mork <bjorn@mork.no> + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + */ + +#ifndef _MBIM_H__ +#define _MBIM_H__ + +#include <stdbool.h> +#include <stdint.h> + +#define MBIM_MESSAGE_TYPE_COMMAND 0x00000003 +#define MBIM_MESSAGE_TYPE_COMMAND_DONE 0x80000003 +#define MBIM_MESSAGE_COMMAND_TYPE_SET 1 +#define MBIM_CID_QMI_MSG 1 + +struct mbim_message_header { + uint32_t type; + uint32_t length; + uint32_t transaction_id; +} __attribute__((packed)); + +struct mbim_fragment_header { + uint32_t total; + uint32_t current; +} __attribute__((packed)); + +struct mbim_command_message { + struct mbim_message_header header; + struct mbim_fragment_header fragment_header; + uint8_t service_id[16]; + uint32_t command_id; + uint32_t command_type; + uint32_t buffer_length; +} __attribute__((packed)); + +bool is_mbim_qmi(struct mbim_command_message *msg); +void mbim_qmi_cmd(struct mbim_command_message *msg, int len, uint16_t tid); + +#endif @@ -87,6 +87,8 @@ struct qmi_dev { uint32_t service_release_cid; uint8_t ctl_tid; + + bool is_mbim; }; struct qmi_request { |