5 #include "qmi-message.h"
7 static uint8_t buf
[QMI_BUFFER_LEN
];
8 static unsigned int buf_ofs
;
10 uint8_t *__qmi_get_buf(unsigned int *ofs
)
16 void __qmi_alloc_reset(void)
21 void *__qmi_alloc_static(unsigned 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
, unsigned int len
)
40 res
= (char *) &buf
[buf_ofs
];
42 memcpy(res
, data
, len
);
47 struct tlv
*tlv_get_next(void **buf
, unsigned 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
, unsigned 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
;