2 * uqmi -- tiny QMI support implementation
4 * Copyright (C) 2014-2015 Felix Fietkau <nbd@openwrt.org>
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.
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.
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.
26 #include "qmi-message.h"
28 static uint8_t buf[QMI_BUFFER_LEN];
29 static unsigned int buf_ofs;
31 uint8_t *__qmi_get_buf(unsigned int *ofs)
37 void __qmi_alloc_reset(void)
42 void *__qmi_alloc_static(unsigned int len)
46 if (buf_ofs + len > sizeof(buf)) {
47 fprintf(stderr, "ERROR: static buffer for message data too small\n");
57 char *__qmi_copy_string(void *data, unsigned int len)
61 res = (char *) &buf[buf_ofs];
63 memcpy(res, data, len);
68 struct tlv *tlv_get_next(void **buf, unsigned int *buflen)
70 struct tlv *tlv = NULL;
73 if (*buflen < sizeof(*tlv))
77 tlv_len = le16_to_cpu(tlv->len) + sizeof(*tlv);
78 if (tlv_len > *buflen)
86 static struct tlv *qmi_msg_next_tlv(struct qmi_msg *qm, int add)
91 if (qm->qmux.service == QMI_SERVICE_CTL) {
93 tlv_len = le16_to_cpu(qm->ctl.tlv_len);
94 qm->ctl.tlv_len = cpu_to_le16(tlv_len + add);
97 tlv_len = le16_to_cpu(qm->svc.tlv_len);
98 qm->svc.tlv_len = cpu_to_le16(tlv_len + add);
106 void tlv_new(struct qmi_msg *qm, uint8_t type, uint16_t len, void *data)
110 tlv = qmi_msg_next_tlv(qm, sizeof(*tlv) + len);
112 tlv->len = cpu_to_le16(len);
113 memcpy(tlv->data, data, len);
116 void qmi_init_request_message(struct qmi_msg *qm, QmiService service)
118 memset(qm, 0, sizeof(*qm));
120 qm->qmux.service = service;
123 int qmi_complete_request_message(struct qmi_msg *qm)
125 void *tlv_end = qmi_msg_next_tlv(qm, 0);
126 void *msg_start = &qm->qmux;
128 qm->qmux.len = cpu_to_le16(tlv_end - msg_start);
129 return tlv_end - msg_start + 1;
132 int qmi_check_message_status(void *tlv_buf, unsigned int len)
140 while ((tlv = tlv_get_next(&tlv_buf, &len)) != NULL) {
144 if (tlv_data_len(tlv) != sizeof(*status))
145 return QMI_ERROR_INVALID_DATA;
147 status = (void *) tlv->data;
151 return le16_to_cpu(status->code);
154 return QMI_ERROR_NO_DATA;
157 void *qmi_msg_get_tlv_buf(struct qmi_msg *qm, int *tlv_len)
162 if (qm->qmux.service == QMI_SERVICE_CTL) {
164 len = qm->ctl.tlv_len;
167 len = qm->svc.tlv_len;