qmi-message.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <stdlib.h>
   4
   5 #include "qmi-message.h"
   6
   7 static uint8_t buf[QMI_BUFFER_LEN];
   8 static int buf_ofs;
   9
  10 uint8_t *__qmi_get_buf(int *ofs)
  11 {
  12         *ofs = buf_ofs;
  13         return buf;
  14 }
  15
  16 void __qmi_alloc_reset(void)
  17 {
  18         buf_ofs = 0;
  19 }
  20
  21 void *__qmi_alloc_static(int len)
  22 {
  23         void *ret;
  24
  25         if (buf_ofs + len > sizeof(buf)) {
  26                 fprintf(stderr, "ERROR: static buffer for message data too small\n");
  27                 abort();
  28         }
  29
  30         ret = &buf[buf_ofs];
  31         buf_ofs += len;
  32         memset(ret, 0, len);
  33         return ret;
  34 }
  35
  36 char *__qmi_copy_string(void *data, int len)
  37 {
  38         char *res;
  39
  40         res = (char *) &buf[buf_ofs];
  41         buf_ofs += len + 1;
  42         memcpy(res, data, len);
  43         res[len] = 0;
  44         return res;
  45 }
  46
  47 struct tlv *tlv_get_next(void **buf, int *buflen)
  48 {
  49         struct tlv *tlv = NULL;
  50         unsigned int tlv_len;
  51
  52         if (*buflen < sizeof(*tlv))
  53                 return NULL;
  54
  55         tlv = *buf;
  56         tlv_len = le16_to_cpu(tlv->len) + sizeof(*tlv);
  57         if (tlv_len > *buflen)
  58                 return NULL;
  59
  60         *buflen -= tlv_len;
  61         *buf += tlv_len;
  62         return tlv;
  63 }
  64
  65 static struct tlv *qmi_msg_next_tlv(struct qmi_msg *qm, int add)
  66 {
  67         int tlv_len;
  68         void *tlv;
  69
  70         if (qm->qmux.service == QMI_SERVICE_CTL) {
  71                 tlv = qm->ctl.tlv;
  72                 tlv_len = le16_to_cpu(qm->ctl.tlv_len);
  73                 qm->ctl.tlv_len = cpu_to_le16(tlv_len + add);
  74         } else {
  75                 tlv = qm->svc.tlv;
  76                 tlv_len = le16_to_cpu(qm->svc.tlv_len);
  77                 qm->svc.tlv_len = cpu_to_le16(tlv_len + add);
  78         }
  79
  80         tlv += tlv_len;
  81
  82         return tlv;
  83 }
  84
  85 void tlv_new(struct qmi_msg *qm, uint8_t type, uint16_t len, void *data)
  86 {
  87         struct tlv *tlv;
  88
  89         tlv = qmi_msg_next_tlv(qm, sizeof(*tlv) + len);
  90         tlv->type = type;
  91         tlv->len = cpu_to_le16(len);
  92         memcpy(tlv->data, data, len);
  93 }
  94
  95 void qmi_init_request_message(struct qmi_msg *qm, QmiService service)
  96 {
  97         memset(qm, 0, sizeof(*qm));
  98         qm->marker = 1;
  99         qm->qmux.service = service;
 100 }
 101
 102 int qmi_complete_request_message(struct qmi_msg *qm)
 103 {
 104         void *tlv_end = qmi_msg_next_tlv(qm, 0);
 105         void *msg_start = &qm->qmux;
 106
 107         qm->qmux.len = cpu_to_le16(tlv_end - msg_start);
 108         return tlv_end - msg_start + 1;
 109 }
 110
 111 int qmi_check_message_status(void *tlv_buf, int len)
 112 {
 113         struct tlv *tlv;
 114         struct {
 115                 uint16_t status;
 116                 uint16_t code;
 117         } __packed *status;
 118
 119         while ((tlv = tlv_get_next(&tlv_buf, &len)) != NULL) {
 120                 if (tlv->type != 2)
 121                         continue;
 122
 123                 if (tlv_data_len(tlv) != sizeof(*status))
 124                         return QMI_ERROR_INVALID_DATA;
 125
 126                 status = (void *) tlv->data;
 127                 if (!status->status)
 128                         return 0;
 129
 130                 return le16_to_cpu(status->code);
 131         }
 132
 133         return QMI_ERROR_NO_DATA;
 134 }
 135
 136 void *qmi_msg_get_tlv_buf(struct qmi_msg *qm, int *tlv_len)
 137 {
 138         void *ptr;
 139         int len;
 140
 141         if (qm->qmux.service == QMI_SERVICE_CTL) {
 142                 ptr = qm->ctl.tlv;
 143                 len = qm->ctl.tlv_len;
 144         } else {
 145                 ptr = qm->svc.tlv;
 146                 len = qm->svc.tlv_len;
 147         }
 148
 149         if (tlv_len)
 150                 *tlv_len = len;
 151
 152         return ptr;
 153 }
 154
 155