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
;