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