1 #include <errno.h>
2 #include <string.h>
3 #include <stdint.h>
4 #include <stdlib.h>
5 #include <stdbool.h>
6
7 #include "qmi_rmtfs.h"
8
9 struct qmi_packet {
10 uint8_t flags;
11 uint16_t txn_id;
12 uint16_t msg_id;
13 uint16_t msg_len;
14 uint8_t data[];
15 } __attribute__((__packed__));
16
17 struct qmi_tlv {
18 void *allocated;
19 void *buf;
20 size_t size;
21 int error;
22 };
23
24 struct qmi_tlv_item {
25 uint8_t key;
26 uint16_t len;
27 uint8_t data[];
28 } __attribute__((__packed__));
29
qmi_tlv_init(unsigned txn,unsigned msg_id,unsigned msg_type)30 struct qmi_tlv *qmi_tlv_init(unsigned txn, unsigned msg_id, unsigned msg_type)
31 {
32 struct qmi_packet *pkt;
33 struct qmi_tlv *tlv;
34
35 tlv = malloc(sizeof(struct qmi_tlv));
36 memset(tlv, 0, sizeof(struct qmi_tlv));
37
38 tlv->size = sizeof(struct qmi_packet);
39 tlv->allocated = malloc(tlv->size);
40 tlv->buf = tlv->allocated;
41
42 pkt = tlv->buf;
43 pkt->flags = msg_type;
44 pkt->txn_id = txn;
45 pkt->msg_id = msg_id;
46 pkt->msg_len = 0;
47
48 return tlv;
49 }
50
qmi_tlv_decode(void * buf,size_t len,unsigned * txn,unsigned msg_type)51 struct qmi_tlv *qmi_tlv_decode(void *buf, size_t len, unsigned *txn, unsigned msg_type)
52 {
53 struct qmi_packet *pkt = buf;
54 struct qmi_tlv *tlv;
55
56 if (pkt->flags != msg_type)
57 return NULL;
58
59 tlv = malloc(sizeof(struct qmi_tlv));
60 memset(tlv, 0, sizeof(struct qmi_tlv));
61
62 tlv->buf = buf;
63 tlv->size = len;
64
65 if (txn)
66 *txn = pkt->txn_id;
67
68 return tlv;
69 }
70
qmi_tlv_encode(struct qmi_tlv * tlv,size_t * len)71 void *qmi_tlv_encode(struct qmi_tlv *tlv, size_t *len)
72 {
73
74 struct qmi_packet *pkt;
75
76 if (!tlv || tlv->error)
77 return NULL;
78
79 pkt = tlv->buf;
80 pkt->msg_len = tlv->size - sizeof(struct qmi_packet);
81
82 *len = tlv->size;
83 return tlv->buf;
84 }
85
qmi_tlv_free(struct qmi_tlv * tlv)86 void qmi_tlv_free(struct qmi_tlv *tlv)
87 {
88 free(tlv->allocated);
89 free(tlv);
90 }
91
qmi_tlv_get_item(struct qmi_tlv * tlv,unsigned id)92 static struct qmi_tlv_item *qmi_tlv_get_item(struct qmi_tlv *tlv, unsigned id)
93 {
94 struct qmi_tlv_item *item;
95 struct qmi_packet *pkt;
96 unsigned offset = 0;
97 void *pkt_data;
98
99 pkt = tlv->buf;
100 pkt_data = pkt->data;
101
102 while (offset < tlv->size) {
103 item = pkt_data + offset;
104 if (item->key == id)
105 return pkt_data + offset;
106
107 offset += sizeof(struct qmi_tlv_item) + item->len;
108 }
109 return NULL;
110 }
111
qmi_tlv_get(struct qmi_tlv * tlv,unsigned id,size_t * len)112 void *qmi_tlv_get(struct qmi_tlv *tlv, unsigned id, size_t *len)
113 {
114 struct qmi_tlv_item *item;
115
116 item = qmi_tlv_get_item(tlv, id);
117 if (!item)
118 return NULL;
119
120 *len = item->len;
121 return item->data;
122 }
123
qmi_tlv_get_array(struct qmi_tlv * tlv,unsigned id,unsigned len_size,size_t * len,size_t * size)124 void *qmi_tlv_get_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, size_t *len, size_t *size)
125 {
126 struct qmi_tlv_item *item;
127 unsigned count;
128 void *ptr;
129
130 item = qmi_tlv_get_item(tlv, id);
131 if (!item)
132 return NULL;
133
134 ptr = item->data;
135 switch (len_size) {
136 case 4:
137 count = *(uint32_t*)ptr++;
138 break;
139 case 2:
140 count = *(uint16_t*)ptr++;
141 break;
142 case 1:
143 count = *(uint8_t*)ptr++;
144 break;
145 }
146
147 *len = count;
148 *size = (item->len - len_size) / count;
149
150 return ptr;
151 }
152
qmi_tlv_alloc_item(struct qmi_tlv * tlv,unsigned id,size_t len)153 static struct qmi_tlv_item *qmi_tlv_alloc_item(struct qmi_tlv *tlv, unsigned id, size_t len)
154 {
155 struct qmi_tlv_item *item;
156 size_t new_size;
157 bool migrate;
158 void *newp;
159
160 /* If using user provided buffer, migrate data */
161 migrate = !tlv->allocated;
162
163 new_size = tlv->size + sizeof(struct qmi_tlv_item) + len;
164 newp = realloc(tlv->allocated, new_size);
165 if (!newp)
166 return NULL;
167
168 if (migrate)
169 memcpy(newp, tlv->buf, tlv->size);
170
171 item = newp + tlv->size;
172 item->key = id;
173 item->len = len;
174
175 tlv->buf = tlv->allocated = newp;
176 tlv->size = new_size;
177
178 return item;
179 }
180
qmi_tlv_set(struct qmi_tlv * tlv,unsigned id,void * buf,size_t len)181 int qmi_tlv_set(struct qmi_tlv *tlv, unsigned id, void *buf, size_t len)
182 {
183 struct qmi_tlv_item *item;
184
185 if (!tlv)
186 return -EINVAL;
187
188 item = qmi_tlv_alloc_item(tlv, id, len);
189 if (!item) {
190 tlv->error = ENOMEM;
191 return -ENOMEM;
192 }
193
194 memcpy(item->data, buf, len);
195
196 return 0;
197 }
198
qmi_tlv_set_array(struct qmi_tlv * tlv,unsigned id,unsigned len_size,void * buf,size_t len,size_t size)199 int qmi_tlv_set_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, void *buf, size_t len, size_t size)
200 {
201 struct qmi_tlv_item *item;
202 size_t array_size;
203 void *ptr;
204
205 if (!tlv)
206 return -EINVAL;
207
208 array_size = len * size;
209 item = qmi_tlv_alloc_item(tlv, id, len_size + array_size);
210 if (!item) {
211 tlv->error = ENOMEM;
212 return -ENOMEM;
213 }
214
215 ptr = item->data;
216
217 switch (len_size) {
218 case 4:
219 *(uint32_t*)ptr++ = len;
220 break;
221 case 2:
222 *(uint16_t*)ptr++ = len;
223 break;
224 case 1:
225 *(uint8_t*)ptr++ = len;
226 break;
227 }
228 memcpy(ptr, buf, array_size);
229
230 return 0;
231 }
232
233
234