5 #include "qmi-message.h"
7 static uint8_t buf[QMI_BUFFER_LEN];
10 uint8_t *__qmi_get_buf(int *ofs)
16 void __qmi_alloc_reset(void)
21 void *__qmi_alloc_static(int len)
25 if (buf_ofs + len > sizeof(buf)) {
26 fprintf(stderr, "ERROR: static buffer for message data too small\n");
36 char *__qmi_copy_string(void *data, int len)
40 res = (char *) &buf[buf_ofs];
42 memcpy(res, data, len);
47 struct tlv *tlv_get_next(void **buf, int *buflen)
49 struct tlv *tlv = NULL;
52 if (*buflen < sizeof(*tlv))
56 tlv_len = le16_to_cpu(tlv->len) + sizeof(*tlv);
57 if (tlv_len > *buflen)
65 static struct tlv *qmi_msg_next_tlv(struct qmi_msg *qm, int add)
70 if (qm->qmux.service == QMI_SERVICE_CTL) {
72 tlv_len = le16_to_cpu(qm->ctl.tlv_len);
73 qm->ctl.tlv_len = cpu_to_le16(tlv_len + add);
76 tlv_len = le16_to_cpu(qm->svc.tlv_len);
77 qm->svc.tlv_len = cpu_to_le16(tlv_len + add);
85 void tlv_new(struct qmi_msg *qm, uint8_t type, uint16_t len, void *data)
89 tlv = qmi_msg_next_tlv(qm, sizeof(*tlv) + len);
91 tlv->len = cpu_to_le16(len);
92 memcpy(tlv->data, data, len);
95 void qmi_init_request_message(struct qmi_msg *qm, QmiService service)
97 memset(qm, 0, sizeof(*qm));
99 qm->qmux.service = service;
102 int qmi_complete_request_message(struct qmi_msg *qm)
104 void *tlv_end = qmi_msg_next_tlv(qm, 0);
105 void *msg_start = &qm->qmux;
107 qm->qmux.len = cpu_to_le16(tlv_end - msg_start);
108 return tlv_end - msg_start + 1;
111 int qmi_check_message_status(void *tlv_buf, int len)
119 while ((tlv = tlv_get_next(&tlv_buf, &len)) != NULL) {
123 if (tlv_data_len(tlv) != sizeof(*status))
124 return QMI_ERROR_INVALID_DATA;
126 status = (void *) tlv->data;
130 return le16_to_cpu(status->code);
133 return QMI_ERROR_NO_DATA;
136 void *qmi_msg_get_tlv_buf(struct qmi_msg *qm, int *tlv_len)
141 if (qm->qmux.service == QMI_SERVICE_CTL) {
143 len = qm->ctl.tlv_len;
146 len = qm->svc.tlv_len;