1 /* 2 * uqmi -- tiny QMI support implementation 3 * 4 * Copyright (C) 2014-2015 Felix Fietkau <nbd@openwrt.org> 5 * 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. 10 * 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. 15 * 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. 20 */ 21 22 #include <stdio.h> 23 #include <string.h> 24 #include <stdlib.h> 25 26 #include "qmi-message.h" 27 28 static uint8_t buf[QMI_BUFFER_LEN]; 29 static unsigned int buf_ofs; 30 31 uint8_t *__qmi_get_buf(unsigned int *ofs) 32 { 33 *ofs = buf_ofs; 34 return buf; 35 } 36 37 void __qmi_alloc_reset(void) 38 { 39 buf_ofs = 0; 40 } 41 42 void *__qmi_alloc_static(unsigned int len) 43 { 44 void *ret; 45 46 if (buf_ofs + len > sizeof(buf)) { 47 fprintf(stderr, "ERROR: static buffer for message data too small\n"); 48 abort(); 49 } 50 51 ret = &buf[buf_ofs]; 52 buf_ofs += len; 53 memset(ret, 0, len); 54 return ret; 55 } 56 57 char *__qmi_copy_string(void *data, unsigned int len) 58 { 59 char *res; 60 61 res = (char *) &buf[buf_ofs]; 62 buf_ofs += len + 1; 63 memcpy(res, data, len); 64 res[len] = 0; 65 return res; 66 } 67 68 struct tlv *tlv_get_next(void **buf, unsigned int *buflen) 69 { 70 struct tlv *tlv = NULL; 71 unsigned int tlv_len; 72 73 if (*buflen < sizeof(*tlv)) 74 return NULL; 75 76 tlv = *buf; 77 tlv_len = le16_to_cpu(tlv->len) + sizeof(*tlv); 78 if (tlv_len > *buflen) 79 return NULL; 80 81 *buflen -= tlv_len; 82 *buf += tlv_len; 83 return tlv; 84 } 85 86 static struct tlv *qmi_msg_next_tlv(struct qmi_msg *qm, int add) 87 { 88 int tlv_len; 89 void *tlv; 90 91 if (qm->qmux.service == QMI_SERVICE_CTL) { 92 tlv = qm->ctl.tlv; 93 tlv_len = le16_to_cpu(qm->ctl.tlv_len); 94 qm->ctl.tlv_len = cpu_to_le16(tlv_len + add); 95 } else { 96 tlv = qm->svc.tlv; 97 tlv_len = le16_to_cpu(qm->svc.tlv_len); 98 qm->svc.tlv_len = cpu_to_le16(tlv_len + add); 99 } 100 101 tlv += tlv_len; 102 103 return tlv; 104 } 105 106 void tlv_new(struct qmi_msg *qm, uint8_t type, uint16_t len, void *data) 107 { 108 struct tlv *tlv; 109 110 tlv = qmi_msg_next_tlv(qm, sizeof(*tlv) + len); 111 tlv->type = type; 112 tlv->len = cpu_to_le16(len); 113 memcpy(tlv->data, data, len); 114 } 115 116 void qmi_init_request_message(struct qmi_msg *qm, QmiService service) 117 { 118 memset(qm, 0, sizeof(*qm)); 119 qm->marker = 1; 120 qm->qmux.service = service; 121 } 122 123 int qmi_complete_request_message(struct qmi_msg *qm) 124 { 125 void *tlv_end = qmi_msg_next_tlv(qm, 0); 126 void *msg_start = &qm->qmux; 127 128 qm->qmux.len = cpu_to_le16(tlv_end - msg_start); 129 return tlv_end - msg_start + 1; 130 } 131 132 int qmi_check_message_status(void *tlv_buf, unsigned int len) 133 { 134 struct tlv *tlv; 135 struct { 136 uint16_t status; 137 uint16_t code; 138 } __packed *status; 139 140 while ((tlv = tlv_get_next(&tlv_buf, &len)) != NULL) { 141 if (tlv->type != 2) 142 continue; 143 144 if (tlv_data_len(tlv) != sizeof(*status)) 145 return QMI_ERROR_INVALID_DATA; 146 147 status = (void *) tlv->data; 148 if (!status->status) 149 return 0; 150 151 return le16_to_cpu(status->code); 152 } 153 154 return QMI_ERROR_NO_DATA; 155 } 156 157 void *qmi_msg_get_tlv_buf(struct qmi_msg *qm, int *tlv_len) 158 { 159 void *ptr; 160 int len; 161 162 if (qm->qmux.service == QMI_SERVICE_CTL) { 163 ptr = qm->ctl.tlv; 164 len = qm->ctl.tlv_len; 165 } else { 166 ptr = qm->svc.tlv; 167 len = qm->svc.tlv_len; 168 } 169 170 if (tlv_len) 171 *tlv_len = len; 172 173 return ptr; 174 } 175 176 177
This page was automatically generated by LXR 0.3.1. • OpenWrt