1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.
4 * Copyright (C) 2017 Linaro Ltd.
5 */
6 #include <linux/slab.h>
7 #include <linux/uaccess.h>
8 #include <linux/module.h>
9 #include <linux/kernel.h>
10 #include <linux/errno.h>
11 #include <linux/string.h>
12 #include <linux/soc/qcom/qmi.h>
13
14 #define QMI_ENCDEC_ENCODE_TLV(type, length, p_dst) do { \
15 *p_dst++ = type; \
16 *p_dst++ = ((u8)((length) & 0xFF)); \
17 *p_dst++ = ((u8)(((length) >> 8) & 0xFF)); \
18 } while (0)
19
20 #define QMI_ENCDEC_DECODE_TLV(p_type, p_length, p_src) do { \
21 *p_type = (u8)*p_src++; \
22 *p_length = (u8)*p_src++; \
23 *p_length |= ((u8)*p_src) << 8; \
24 } while (0)
25
26 #define QMI_ENCDEC_ENCODE_N_BYTES(p_dst, p_src, size) \
27 do { \
28 memcpy(p_dst, p_src, size); \
29 p_dst = (u8 *)p_dst + size; \
30 p_src = (u8 *)p_src + size; \
31 } while (0)
32
33 #define QMI_ENCDEC_DECODE_N_BYTES(p_dst, p_src, size) \
34 do { \
35 memcpy(p_dst, p_src, size); \
36 p_dst = (u8 *)p_dst + size; \
37 p_src = (u8 *)p_src + size; \
38 } while (0)
39
40 #define UPDATE_ENCODE_VARIABLES(temp_si, buf_dst, \
41 encoded_bytes, tlv_len, encode_tlv, rc) \
42 do { \
43 buf_dst = (u8 *)buf_dst + rc; \
44 encoded_bytes += rc; \
45 tlv_len += rc; \
46 temp_si = temp_si + 1; \
47 encode_tlv = 1; \
48 } while (0)
49
50 #define UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc) \
51 do { \
52 buf_src = (u8 *)buf_src + rc; \
53 decoded_bytes += rc; \
54 } while (0)
55
56 #define TLV_LEN_SIZE sizeof(u16)
57 #define TLV_TYPE_SIZE sizeof(u8)
58 #define OPTIONAL_TLV_TYPE_START 0x10
59
60 static int qmi_encode(const struct qmi_elem_info *ei_array, void *out_buf,
61 const void *in_c_struct, u32 out_buf_len,
62 int enc_level);
63
64 static int qmi_decode(const struct qmi_elem_info *ei_array, void *out_c_struct,
65 const void *in_buf, u32 in_buf_len, int dec_level);
66
67 /**
68 * skip_to_next_elem() - Skip to next element in the structure to be encoded
69 * @ei_array: Struct info describing the element to be skipped.
70 * @level: Depth level of encoding/decoding to identify nested structures.
71 *
72 * This function is used while encoding optional elements. If the flag
73 * corresponding to an optional element is not set, then encoding the
74 * optional element can be skipped. This function can be used to perform
75 * that operation.
76 *
77 * Return: struct info of the next element that can be encoded.
78 */
79 static const struct qmi_elem_info *
skip_to_next_elem(const struct qmi_elem_info * ei_array,int level)80 skip_to_next_elem(const struct qmi_elem_info *ei_array, int level)
81 {
82 const struct qmi_elem_info *temp_ei = ei_array;
83 u8 tlv_type;
84
85 if (level > 1) {
86 temp_ei = temp_ei + 1;
87 } else {
88 do {
89 tlv_type = temp_ei->tlv_type;
90 temp_ei = temp_ei + 1;
91 } while (tlv_type == temp_ei->tlv_type);
92 }
93
94 return temp_ei;
95 }
96
97 /**
98 * qmi_calc_min_msg_len() - Calculate the minimum length of a QMI message
99 * @ei_array: Struct info array describing the structure.
100 * @level: Level to identify the depth of the nested structures.
101 *
102 * Return: Expected minimum length of the QMI message or 0 on error.
103 */
qmi_calc_min_msg_len(const struct qmi_elem_info * ei_array,int level)104 static int qmi_calc_min_msg_len(const struct qmi_elem_info *ei_array,
105 int level)
106 {
107 int min_msg_len = 0;
108 const struct qmi_elem_info *temp_ei = ei_array;
109
110 if (!ei_array)
111 return min_msg_len;
112
113 while (temp_ei->data_type != QMI_EOTI) {
114 /* Optional elements do not count in minimum length */
115 if (temp_ei->data_type == QMI_OPT_FLAG) {
116 temp_ei = skip_to_next_elem(temp_ei, level);
117 continue;
118 }
119
120 if (temp_ei->data_type == QMI_DATA_LEN) {
121 min_msg_len += (temp_ei->elem_size == sizeof(u8) ?
122 sizeof(u8) : sizeof(u16));
123 temp_ei++;
124 continue;
125 } else if (temp_ei->data_type == QMI_STRUCT) {
126 min_msg_len += qmi_calc_min_msg_len(temp_ei->ei_array,
127 (level + 1));
128 temp_ei++;
129 } else if (temp_ei->data_type == QMI_STRING) {
130 if (level > 1)
131 min_msg_len += temp_ei->elem_len <= U8_MAX ?
132 sizeof(u8) : sizeof(u16);
133 min_msg_len += temp_ei->elem_len * temp_ei->elem_size;
134 temp_ei++;
135 } else {
136 min_msg_len += (temp_ei->elem_len * temp_ei->elem_size);
137 temp_ei++;
138 }
139
140 /*
141 * Type & Length info. not prepended for elements in the
142 * nested structure.
143 */
144 if (level == 1)
145 min_msg_len += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
146 }
147
148 return min_msg_len;
149 }
150
151 /**
152 * qmi_encode_basic_elem() - Encodes elements of basic/primary data type
153 * @buf_dst: Buffer to store the encoded information.
154 * @buf_src: Buffer containing the elements to be encoded.
155 * @elem_len: Number of elements, in the buf_src, to be encoded.
156 * @elem_size: Size of a single instance of the element to be encoded.
157 *
158 * This function encodes the "elem_len" number of data elements, each of
159 * size "elem_size" bytes from the source buffer "buf_src" and stores the
160 * encoded information in the destination buffer "buf_dst". The elements are
161 * of primary data type which include u8 - u64 or similar. This
162 * function returns the number of bytes of encoded information.
163 *
164 * Return: The number of bytes of encoded information.
165 */
qmi_encode_basic_elem(void * buf_dst,const void * buf_src,u32 elem_len,u32 elem_size)166 static int qmi_encode_basic_elem(void *buf_dst, const void *buf_src,
167 u32 elem_len, u32 elem_size)
168 {
169 u32 i, rc = 0;
170
171 for (i = 0; i < elem_len; i++) {
172 QMI_ENCDEC_ENCODE_N_BYTES(buf_dst, buf_src, elem_size);
173 rc += elem_size;
174 }
175
176 return rc;
177 }
178
179 /**
180 * qmi_encode_struct_elem() - Encodes elements of struct data type
181 * @ei_array: Struct info array descibing the struct element.
182 * @buf_dst: Buffer to store the encoded information.
183 * @buf_src: Buffer containing the elements to be encoded.
184 * @elem_len: Number of elements, in the buf_src, to be encoded.
185 * @out_buf_len: Available space in the encode buffer.
186 * @enc_level: Depth of the nested structure from the main structure.
187 *
188 * This function encodes the "elem_len" number of struct elements, each of
189 * size "ei_array->elem_size" bytes from the source buffer "buf_src" and
190 * stores the encoded information in the destination buffer "buf_dst". The
191 * elements are of struct data type which includes any C structure. This
192 * function returns the number of bytes of encoded information.
193 *
194 * Return: The number of bytes of encoded information on success or negative
195 * errno on error.
196 */
qmi_encode_struct_elem(const struct qmi_elem_info * ei_array,void * buf_dst,const void * buf_src,u32 elem_len,u32 out_buf_len,int enc_level)197 static int qmi_encode_struct_elem(const struct qmi_elem_info *ei_array,
198 void *buf_dst, const void *buf_src,
199 u32 elem_len, u32 out_buf_len,
200 int enc_level)
201 {
202 int i, rc, encoded_bytes = 0;
203 const struct qmi_elem_info *temp_ei = ei_array;
204
205 for (i = 0; i < elem_len; i++) {
206 rc = qmi_encode(temp_ei->ei_array, buf_dst, buf_src,
207 out_buf_len - encoded_bytes, enc_level);
208 if (rc < 0) {
209 pr_err("%s: STRUCT Encode failure\n", __func__);
210 return rc;
211 }
212 buf_dst = buf_dst + rc;
213 buf_src = buf_src + temp_ei->elem_size;
214 encoded_bytes += rc;
215 }
216
217 return encoded_bytes;
218 }
219
220 /**
221 * qmi_encode_string_elem() - Encodes elements of string data type
222 * @ei_array: Struct info array descibing the string element.
223 * @buf_dst: Buffer to store the encoded information.
224 * @buf_src: Buffer containing the elements to be encoded.
225 * @out_buf_len: Available space in the encode buffer.
226 * @enc_level: Depth of the string element from the main structure.
227 *
228 * This function encodes a string element of maximum length "ei_array->elem_len"
229 * bytes from the source buffer "buf_src" and stores the encoded information in
230 * the destination buffer "buf_dst". This function returns the number of bytes
231 * of encoded information.
232 *
233 * Return: The number of bytes of encoded information on success or negative
234 * errno on error.
235 */
qmi_encode_string_elem(const struct qmi_elem_info * ei_array,void * buf_dst,const void * buf_src,u32 out_buf_len,int enc_level)236 static int qmi_encode_string_elem(const struct qmi_elem_info *ei_array,
237 void *buf_dst, const void *buf_src,
238 u32 out_buf_len, int enc_level)
239 {
240 int rc;
241 int encoded_bytes = 0;
242 const struct qmi_elem_info *temp_ei = ei_array;
243 u32 string_len = 0;
244 u32 string_len_sz = 0;
245
246 string_len = strlen(buf_src);
247 string_len_sz = temp_ei->elem_len <= U8_MAX ?
248 sizeof(u8) : sizeof(u16);
249 if (string_len > temp_ei->elem_len) {
250 pr_err("%s: String to be encoded is longer - %d > %d\n",
251 __func__, string_len, temp_ei->elem_len);
252 return -EINVAL;
253 }
254
255 if (enc_level == 1) {
256 if (string_len + TLV_LEN_SIZE + TLV_TYPE_SIZE >
257 out_buf_len) {
258 pr_err("%s: Output len %d > Out Buf len %d\n",
259 __func__, string_len, out_buf_len);
260 return -ETOOSMALL;
261 }
262 } else {
263 if (string_len + string_len_sz > out_buf_len) {
264 pr_err("%s: Output len %d > Out Buf len %d\n",
265 __func__, string_len, out_buf_len);
266 return -ETOOSMALL;
267 }
268 rc = qmi_encode_basic_elem(buf_dst, &string_len,
269 1, string_len_sz);
270 encoded_bytes += rc;
271 }
272
273 rc = qmi_encode_basic_elem(buf_dst + encoded_bytes, buf_src,
274 string_len, temp_ei->elem_size);
275 encoded_bytes += rc;
276
277 return encoded_bytes;
278 }
279
280 /**
281 * qmi_encode() - Core Encode Function
282 * @ei_array: Struct info array describing the structure to be encoded.
283 * @out_buf: Buffer to hold the encoded QMI message.
284 * @in_c_struct: Pointer to the C structure to be encoded.
285 * @out_buf_len: Available space in the encode buffer.
286 * @enc_level: Encode level to indicate the depth of the nested structure,
287 * within the main structure, being encoded.
288 *
289 * Return: The number of bytes of encoded information on success or negative
290 * errno on error.
291 */
qmi_encode(const struct qmi_elem_info * ei_array,void * out_buf,const void * in_c_struct,u32 out_buf_len,int enc_level)292 static int qmi_encode(const struct qmi_elem_info *ei_array, void *out_buf,
293 const void *in_c_struct, u32 out_buf_len,
294 int enc_level)
295 {
296 const struct qmi_elem_info *temp_ei = ei_array;
297 u8 opt_flag_value = 0;
298 u32 data_len_value = 0, data_len_sz;
299 u8 *buf_dst = (u8 *)out_buf;
300 u8 *tlv_pointer;
301 u32 tlv_len;
302 u8 tlv_type;
303 u32 encoded_bytes = 0;
304 const void *buf_src;
305 int encode_tlv = 0;
306 int rc;
307 u8 val8;
308 u16 val16;
309
310 if (!ei_array)
311 return 0;
312
313 tlv_pointer = buf_dst;
314 tlv_len = 0;
315 if (enc_level == 1)
316 buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE);
317
318 while (temp_ei->data_type != QMI_EOTI) {
319 buf_src = in_c_struct + temp_ei->offset;
320 tlv_type = temp_ei->tlv_type;
321
322 if (temp_ei->array_type == NO_ARRAY) {
323 data_len_value = 1;
324 } else if (temp_ei->array_type == STATIC_ARRAY) {
325 data_len_value = temp_ei->elem_len;
326 } else if (data_len_value <= 0 ||
327 temp_ei->elem_len < data_len_value) {
328 pr_err("%s: Invalid data length\n", __func__);
329 return -EINVAL;
330 }
331
332 switch (temp_ei->data_type) {
333 case QMI_OPT_FLAG:
334 rc = qmi_encode_basic_elem(&opt_flag_value, buf_src,
335 1, sizeof(u8));
336 if (opt_flag_value)
337 temp_ei = temp_ei + 1;
338 else
339 temp_ei = skip_to_next_elem(temp_ei, enc_level);
340 break;
341
342 case QMI_DATA_LEN:
343 data_len_sz = temp_ei->elem_size == sizeof(u8) ?
344 sizeof(u8) : sizeof(u16);
345 /* Check to avoid out of range buffer access */
346 if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE +
347 TLV_TYPE_SIZE) > out_buf_len) {
348 pr_err("%s: Too Small Buffer @DATA_LEN\n",
349 __func__);
350 return -ETOOSMALL;
351 }
352 if (data_len_sz == sizeof(u8)) {
353 val8 = *(u8 *)buf_src;
354 data_len_value = (u32)val8;
355 rc = qmi_encode_basic_elem(buf_dst, &val8,
356 1, data_len_sz);
357 } else {
358 val16 = *(u16 *)buf_src;
359 data_len_value = (u32)le16_to_cpu(val16);
360 rc = qmi_encode_basic_elem(buf_dst, &val16,
361 1, data_len_sz);
362 }
363 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
364 encoded_bytes, tlv_len,
365 encode_tlv, rc);
366 if (!data_len_value)
367 temp_ei = skip_to_next_elem(temp_ei, enc_level);
368 else
369 encode_tlv = 0;
370 break;
371
372 case QMI_UNSIGNED_1_BYTE:
373 case QMI_UNSIGNED_2_BYTE:
374 case QMI_UNSIGNED_4_BYTE:
375 case QMI_UNSIGNED_8_BYTE:
376 case QMI_SIGNED_2_BYTE_ENUM:
377 case QMI_SIGNED_4_BYTE_ENUM:
378 /* Check to avoid out of range buffer access */
379 if (((data_len_value * temp_ei->elem_size) +
380 encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) >
381 out_buf_len) {
382 pr_err("%s: Too Small Buffer @data_type:%d\n",
383 __func__, temp_ei->data_type);
384 return -ETOOSMALL;
385 }
386 rc = qmi_encode_basic_elem(buf_dst, buf_src,
387 data_len_value,
388 temp_ei->elem_size);
389 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
390 encoded_bytes, tlv_len,
391 encode_tlv, rc);
392 break;
393
394 case QMI_STRUCT:
395 rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src,
396 data_len_value,
397 out_buf_len - encoded_bytes,
398 enc_level + 1);
399 if (rc < 0)
400 return rc;
401 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
402 encoded_bytes, tlv_len,
403 encode_tlv, rc);
404 break;
405
406 case QMI_STRING:
407 rc = qmi_encode_string_elem(temp_ei, buf_dst, buf_src,
408 out_buf_len - encoded_bytes,
409 enc_level);
410 if (rc < 0)
411 return rc;
412 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
413 encoded_bytes, tlv_len,
414 encode_tlv, rc);
415 break;
416 default:
417 pr_err("%s: Unrecognized data type\n", __func__);
418 return -EINVAL;
419 }
420
421 if (encode_tlv && enc_level == 1) {
422 QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer);
423 encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
424 tlv_pointer = buf_dst;
425 tlv_len = 0;
426 buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE;
427 encode_tlv = 0;
428 }
429 }
430
431 return encoded_bytes;
432 }
433
434 /**
435 * qmi_decode_basic_elem() - Decodes elements of basic/primary data type
436 * @buf_dst: Buffer to store the decoded element.
437 * @buf_src: Buffer containing the elements in QMI wire format.
438 * @elem_len: Number of elements to be decoded.
439 * @elem_size: Size of a single instance of the element to be decoded.
440 *
441 * This function decodes the "elem_len" number of elements in QMI wire format,
442 * each of size "elem_size" bytes from the source buffer "buf_src" and stores
443 * the decoded elements in the destination buffer "buf_dst". The elements are
444 * of primary data type which include u8 - u64 or similar. This
445 * function returns the number of bytes of decoded information.
446 *
447 * Return: The total size of the decoded data elements, in bytes.
448 */
qmi_decode_basic_elem(void * buf_dst,const void * buf_src,u32 elem_len,u32 elem_size)449 static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src,
450 u32 elem_len, u32 elem_size)
451 {
452 u32 i, rc = 0;
453
454 for (i = 0; i < elem_len; i++) {
455 QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size);
456 rc += elem_size;
457 }
458
459 return rc;
460 }
461
462 /**
463 * qmi_decode_struct_elem() - Decodes elements of struct data type
464 * @ei_array: Struct info array describing the struct element.
465 * @buf_dst: Buffer to store the decoded element.
466 * @buf_src: Buffer containing the elements in QMI wire format.
467 * @elem_len: Number of elements to be decoded.
468 * @tlv_len: Total size of the encoded information corresponding to
469 * this struct element.
470 * @dec_level: Depth of the nested structure from the main structure.
471 *
472 * This function decodes the "elem_len" number of elements in QMI wire format,
473 * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src"
474 * and stores the decoded elements in the destination buffer "buf_dst". The
475 * elements are of struct data type which includes any C structure. This
476 * function returns the number of bytes of decoded information.
477 *
478 * Return: The total size of the decoded data elements on success, negative
479 * errno on error.
480 */
qmi_decode_struct_elem(const struct qmi_elem_info * ei_array,void * buf_dst,const void * buf_src,u32 elem_len,u32 tlv_len,int dec_level)481 static int qmi_decode_struct_elem(const struct qmi_elem_info *ei_array,
482 void *buf_dst, const void *buf_src,
483 u32 elem_len, u32 tlv_len,
484 int dec_level)
485 {
486 int i, rc, decoded_bytes = 0;
487 const struct qmi_elem_info *temp_ei = ei_array;
488
489 for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) {
490 rc = qmi_decode(temp_ei->ei_array, buf_dst, buf_src,
491 tlv_len - decoded_bytes, dec_level);
492 if (rc < 0)
493 return rc;
494 buf_src = buf_src + rc;
495 buf_dst = buf_dst + temp_ei->elem_size;
496 decoded_bytes += rc;
497 }
498
499 if ((dec_level <= 2 && decoded_bytes != tlv_len) ||
500 (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) {
501 pr_err("%s: Fault in decoding: dl(%d), db(%d), tl(%d), i(%d), el(%d)\n",
502 __func__, dec_level, decoded_bytes, tlv_len,
503 i, elem_len);
504 return -EFAULT;
505 }
506
507 return decoded_bytes;
508 }
509
510 /**
511 * qmi_decode_string_elem() - Decodes elements of string data type
512 * @ei_array: Struct info array describing the string element.
513 * @buf_dst: Buffer to store the decoded element.
514 * @buf_src: Buffer containing the elements in QMI wire format.
515 * @tlv_len: Total size of the encoded information corresponding to
516 * this string element.
517 * @dec_level: Depth of the string element from the main structure.
518 *
519 * This function decodes the string element of maximum length
520 * "ei_array->elem_len" from the source buffer "buf_src" and puts it into
521 * the destination buffer "buf_dst". This function returns number of bytes
522 * decoded from the input buffer.
523 *
524 * Return: The total size of the decoded data elements on success, negative
525 * errno on error.
526 */
qmi_decode_string_elem(const struct qmi_elem_info * ei_array,void * buf_dst,const void * buf_src,u32 tlv_len,int dec_level)527 static int qmi_decode_string_elem(const struct qmi_elem_info *ei_array,
528 void *buf_dst, const void *buf_src,
529 u32 tlv_len, int dec_level)
530 {
531 int rc;
532 int decoded_bytes = 0;
533 u32 string_len = 0;
534 u32 string_len_sz = 0;
535 const struct qmi_elem_info *temp_ei = ei_array;
536 u8 val8;
537 u16 val16;
538
539 if (dec_level == 1) {
540 string_len = tlv_len;
541 } else {
542 string_len_sz = temp_ei->elem_len <= U8_MAX ?
543 sizeof(u8) : sizeof(u16);
544 if (string_len_sz == sizeof(u8)) {
545 rc = qmi_decode_basic_elem(&val8, buf_src,
546 1, string_len_sz);
547 string_len = (u32)val8;
548 } else {
549 rc = qmi_decode_basic_elem(&val16, buf_src,
550 1, string_len_sz);
551 string_len = (u32)val16;
552 }
553 decoded_bytes += rc;
554 }
555
556 if (string_len >= temp_ei->elem_len) {
557 pr_err("%s: String len %d >= Max Len %d\n",
558 __func__, string_len, temp_ei->elem_len);
559 return -ETOOSMALL;
560 } else if (string_len > tlv_len) {
561 pr_err("%s: String len %d > Input Buffer Len %d\n",
562 __func__, string_len, tlv_len);
563 return -EFAULT;
564 }
565
566 rc = qmi_decode_basic_elem(buf_dst, buf_src + decoded_bytes,
567 string_len, temp_ei->elem_size);
568 *((char *)buf_dst + string_len) = '\0';
569 decoded_bytes += rc;
570
571 return decoded_bytes;
572 }
573
574 /**
575 * find_ei() - Find element info corresponding to TLV Type
576 * @ei_array: Struct info array of the message being decoded.
577 * @type: TLV Type of the element being searched.
578 *
579 * Every element that got encoded in the QMI message will have a type
580 * information associated with it. While decoding the QMI message,
581 * this function is used to find the struct info regarding the element
582 * that corresponds to the type being decoded.
583 *
584 * Return: Pointer to struct info, if found
585 */
find_ei(const struct qmi_elem_info * ei_array,u32 type)586 static const struct qmi_elem_info *find_ei(const struct qmi_elem_info *ei_array,
587 u32 type)
588 {
589 const struct qmi_elem_info *temp_ei = ei_array;
590
591 while (temp_ei->data_type != QMI_EOTI) {
592 if (temp_ei->tlv_type == (u8)type)
593 return temp_ei;
594 temp_ei = temp_ei + 1;
595 }
596
597 return NULL;
598 }
599
600 /**
601 * qmi_decode() - Core Decode Function
602 * @ei_array: Struct info array describing the structure to be decoded.
603 * @out_c_struct: Buffer to hold the decoded C struct
604 * @in_buf: Buffer containing the QMI message to be decoded
605 * @in_buf_len: Length of the QMI message to be decoded
606 * @dec_level: Decode level to indicate the depth of the nested structure,
607 * within the main structure, being decoded
608 *
609 * Return: The number of bytes of decoded information on success, negative
610 * errno on error.
611 */
qmi_decode(const struct qmi_elem_info * ei_array,void * out_c_struct,const void * in_buf,u32 in_buf_len,int dec_level)612 static int qmi_decode(const struct qmi_elem_info *ei_array, void *out_c_struct,
613 const void *in_buf, u32 in_buf_len,
614 int dec_level)
615 {
616 const struct qmi_elem_info *temp_ei = ei_array;
617 u8 opt_flag_value = 1;
618 u32 data_len_value = 0, data_len_sz = 0;
619 u8 *buf_dst = out_c_struct;
620 const u8 *tlv_pointer;
621 u32 tlv_len = 0;
622 u32 tlv_type;
623 u32 decoded_bytes = 0;
624 const void *buf_src = in_buf;
625 int rc;
626 u8 val8;
627 u16 val16;
628 u32 val32;
629
630 while (decoded_bytes < in_buf_len) {
631 if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI)
632 return decoded_bytes;
633
634 if (dec_level == 1) {
635 tlv_pointer = buf_src;
636 QMI_ENCDEC_DECODE_TLV(&tlv_type,
637 &tlv_len, tlv_pointer);
638 buf_src += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
639 decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
640 temp_ei = find_ei(ei_array, tlv_type);
641 if (!temp_ei && tlv_type < OPTIONAL_TLV_TYPE_START) {
642 pr_err("%s: Inval element info\n", __func__);
643 return -EINVAL;
644 } else if (!temp_ei) {
645 UPDATE_DECODE_VARIABLES(buf_src,
646 decoded_bytes, tlv_len);
647 continue;
648 }
649 } else {
650 /*
651 * No length information for elements in nested
652 * structures. So use remaining decodable buffer space.
653 */
654 tlv_len = in_buf_len - decoded_bytes;
655 }
656
657 buf_dst = out_c_struct + temp_ei->offset;
658 if (temp_ei->data_type == QMI_OPT_FLAG) {
659 memcpy(buf_dst, &opt_flag_value, sizeof(u8));
660 temp_ei = temp_ei + 1;
661 buf_dst = out_c_struct + temp_ei->offset;
662 }
663
664 if (temp_ei->data_type == QMI_DATA_LEN) {
665 data_len_sz = temp_ei->elem_size == sizeof(u8) ?
666 sizeof(u8) : sizeof(u16);
667 if (data_len_sz == sizeof(u8)) {
668 rc = qmi_decode_basic_elem(&val8, buf_src,
669 1, data_len_sz);
670 data_len_value = (u32)val8;
671 } else {
672 rc = qmi_decode_basic_elem(&val16, buf_src,
673 1, data_len_sz);
674 data_len_value = (u32)val16;
675 }
676 val32 = cpu_to_le32(data_len_value);
677 memcpy(buf_dst, &val32, sizeof(u32));
678 temp_ei = temp_ei + 1;
679 buf_dst = out_c_struct + temp_ei->offset;
680 tlv_len -= data_len_sz;
681 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
682 }
683
684 if (temp_ei->array_type == NO_ARRAY) {
685 data_len_value = 1;
686 } else if (temp_ei->array_type == STATIC_ARRAY) {
687 data_len_value = temp_ei->elem_len;
688 } else if (data_len_value > temp_ei->elem_len) {
689 pr_err("%s: Data len %d > max spec %d\n",
690 __func__, data_len_value, temp_ei->elem_len);
691 return -ETOOSMALL;
692 }
693
694 switch (temp_ei->data_type) {
695 case QMI_UNSIGNED_1_BYTE:
696 case QMI_UNSIGNED_2_BYTE:
697 case QMI_UNSIGNED_4_BYTE:
698 case QMI_UNSIGNED_8_BYTE:
699 case QMI_SIGNED_2_BYTE_ENUM:
700 case QMI_SIGNED_4_BYTE_ENUM:
701 rc = qmi_decode_basic_elem(buf_dst, buf_src,
702 data_len_value,
703 temp_ei->elem_size);
704 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
705 break;
706
707 case QMI_STRUCT:
708 rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src,
709 data_len_value, tlv_len,
710 dec_level + 1);
711 if (rc < 0)
712 return rc;
713 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
714 break;
715
716 case QMI_STRING:
717 rc = qmi_decode_string_elem(temp_ei, buf_dst, buf_src,
718 tlv_len, dec_level);
719 if (rc < 0)
720 return rc;
721 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
722 break;
723
724 default:
725 pr_err("%s: Unrecognized data type\n", __func__);
726 return -EINVAL;
727 }
728 temp_ei = temp_ei + 1;
729 }
730
731 return decoded_bytes;
732 }
733
734 /**
735 * qmi_encode_message() - Encode C structure as QMI encoded message
736 * @type: Type of QMI message
737 * @msg_id: Message ID of the message
738 * @len: Passed as max length of the message, updated to actual size
739 * @txn_id: Transaction ID
740 * @ei: QMI message descriptor
741 * @c_struct: Reference to structure to encode
742 *
743 * Return: Buffer with encoded message, or negative ERR_PTR() on error
744 */
qmi_encode_message(int type,unsigned int msg_id,size_t * len,unsigned int txn_id,const struct qmi_elem_info * ei,const void * c_struct)745 void *qmi_encode_message(int type, unsigned int msg_id, size_t *len,
746 unsigned int txn_id, const struct qmi_elem_info *ei,
747 const void *c_struct)
748 {
749 struct qmi_header *hdr;
750 ssize_t msglen = 0;
751 void *msg;
752 int ret;
753
754 /* Check the possibility of a zero length QMI message */
755 if (!c_struct) {
756 ret = qmi_calc_min_msg_len(ei, 1);
757 if (ret) {
758 pr_err("%s: Calc. len %d != 0, but NULL c_struct\n",
759 __func__, ret);
760 return ERR_PTR(-EINVAL);
761 }
762 }
763
764 msg = kzalloc(sizeof(*hdr) + *len, GFP_KERNEL);
765 if (!msg)
766 return ERR_PTR(-ENOMEM);
767
768 /* Encode message, if we have a message */
769 if (c_struct) {
770 msglen = qmi_encode(ei, msg + sizeof(*hdr), c_struct, *len, 1);
771 if (msglen < 0) {
772 kfree(msg);
773 return ERR_PTR(msglen);
774 }
775 }
776
777 hdr = msg;
778 hdr->type = type;
779 hdr->txn_id = cpu_to_le16(txn_id);
780 hdr->msg_id = cpu_to_le16(msg_id);
781 hdr->msg_len = cpu_to_le16(msglen);
782
783 *len = sizeof(*hdr) + msglen;
784
785 return msg;
786 }
787 EXPORT_SYMBOL_GPL(qmi_encode_message);
788
789 /**
790 * qmi_decode_message() - Decode QMI encoded message to C structure
791 * @buf: Buffer with encoded message
792 * @len: Amount of data in @buf
793 * @ei: QMI message descriptor
794 * @c_struct: Reference to structure to decode into
795 *
796 * Return: The number of bytes of decoded information on success, negative
797 * errno on error.
798 */
qmi_decode_message(const void * buf,size_t len,const struct qmi_elem_info * ei,void * c_struct)799 int qmi_decode_message(const void *buf, size_t len,
800 const struct qmi_elem_info *ei, void *c_struct)
801 {
802 if (!ei)
803 return -EINVAL;
804
805 if (!c_struct || !buf || !len)
806 return -EINVAL;
807
808 return qmi_decode(ei, c_struct, buf + sizeof(struct qmi_header),
809 len - sizeof(struct qmi_header), 1);
810 }
811 EXPORT_SYMBOL_GPL(qmi_decode_message);
812
813 /* Common header in all QMI responses */
814 const struct qmi_elem_info qmi_response_type_v01_ei[] = {
815 {
816 .data_type = QMI_SIGNED_2_BYTE_ENUM,
817 .elem_len = 1,
818 .elem_size = sizeof(u16),
819 .array_type = NO_ARRAY,
820 .tlv_type = QMI_COMMON_TLV_TYPE,
821 .offset = offsetof(struct qmi_response_type_v01, result),
822 .ei_array = NULL,
823 },
824 {
825 .data_type = QMI_SIGNED_2_BYTE_ENUM,
826 .elem_len = 1,
827 .elem_size = sizeof(u16),
828 .array_type = NO_ARRAY,
829 .tlv_type = QMI_COMMON_TLV_TYPE,
830 .offset = offsetof(struct qmi_response_type_v01, error),
831 .ei_array = NULL,
832 },
833 {
834 .data_type = QMI_EOTI,
835 .elem_len = 0,
836 .elem_size = 0,
837 .array_type = NO_ARRAY,
838 .tlv_type = QMI_COMMON_TLV_TYPE,
839 .offset = 0,
840 .ei_array = NULL,
841 },
842 };
843 EXPORT_SYMBOL_GPL(qmi_response_type_v01_ei);
844
845 MODULE_DESCRIPTION("QMI encoder/decoder helper");
846 MODULE_LICENSE("GPL v2");
847