• source navigation  • diff markup  • identifier search  • freetext search  • 

Sources/uqmi/qmi-message.c

  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