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