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
308 if (!ei_array)
309 return 0;
310
311 tlv_pointer = buf_dst;
312 tlv_len = 0;
313 if (enc_level == 1)
314 buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE);
315
316 while (temp_ei->data_type != QMI_EOTI) {
317 buf_src = in_c_struct + temp_ei->offset;
318 tlv_type = temp_ei->tlv_type;
319
320 if (temp_ei->array_type == NO_ARRAY) {
321 data_len_value = 1;
322 } else if (temp_ei->array_type == STATIC_ARRAY) {
323 data_len_value = temp_ei->elem_len;
324 } else if (data_len_value <= 0 ||
325 temp_ei->elem_len < data_len_value) {
326 pr_err("%s: Invalid data length\n", __func__);
327 return -EINVAL;
328 }
329
330 switch (temp_ei->data_type) {
331 case QMI_OPT_FLAG:
332 rc = qmi_encode_basic_elem(&opt_flag_value, buf_src,
333 1, sizeof(u8));
334 if (opt_flag_value)
335 temp_ei = temp_ei + 1;
336 else
337 temp_ei = skip_to_next_elem(temp_ei, enc_level);
338 break;
339
340 case QMI_DATA_LEN:
341 memcpy(&data_len_value, buf_src, temp_ei->elem_size);
342 data_len_sz = temp_ei->elem_size == sizeof(u8) ?
343 sizeof(u8) : sizeof(u16);
344 /* Check to avoid out of range buffer access */
345 if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE +
346 TLV_TYPE_SIZE) > out_buf_len) {
347 pr_err("%s: Too Small Buffer @DATA_LEN\n",
348 __func__);
349 return -ETOOSMALL;
350 }
351 rc = qmi_encode_basic_elem(buf_dst, &data_len_value,
352 1, data_len_sz);
353 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
354 encoded_bytes, tlv_len,
355 encode_tlv, rc);
356 if (!data_len_value)
357 temp_ei = skip_to_next_elem(temp_ei, enc_level);
358 else
359 encode_tlv = 0;
360 break;
361
362 case QMI_UNSIGNED_1_BYTE:
363 case QMI_UNSIGNED_2_BYTE:
364 case QMI_UNSIGNED_4_BYTE:
365 case QMI_UNSIGNED_8_BYTE:
366 case QMI_SIGNED_2_BYTE_ENUM:
367 case QMI_SIGNED_4_BYTE_ENUM:
368 /* Check to avoid out of range buffer access */
369 if (((data_len_value * temp_ei->elem_size) +
370 encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) >
371 out_buf_len) {
372 pr_err("%s: Too Small Buffer @data_type:%d\n",
373 __func__, temp_ei->data_type);
374 return -ETOOSMALL;
375 }
376 rc = qmi_encode_basic_elem(buf_dst, buf_src,
377 data_len_value,
378 temp_ei->elem_size);
379 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
380 encoded_bytes, tlv_len,
381 encode_tlv, rc);
382 break;
383
384 case QMI_STRUCT:
385 rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src,
386 data_len_value,
387 out_buf_len - encoded_bytes,
388 enc_level + 1);
389 if (rc < 0)
390 return rc;
391 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
392 encoded_bytes, tlv_len,
393 encode_tlv, rc);
394 break;
395
396 case QMI_STRING:
397 rc = qmi_encode_string_elem(temp_ei, buf_dst, buf_src,
398 out_buf_len - encoded_bytes,
399 enc_level);
400 if (rc < 0)
401 return rc;
402 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
403 encoded_bytes, tlv_len,
404 encode_tlv, rc);
405 break;
406 default:
407 pr_err("%s: Unrecognized data type\n", __func__);
408 return -EINVAL;
409 }
410
411 if (encode_tlv && enc_level == 1) {
412 QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer);
413 encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
414 tlv_pointer = buf_dst;
415 tlv_len = 0;
416 buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE;
417 encode_tlv = 0;
418 }
419 }
420
421 return encoded_bytes;
422 }
423
424 /**
425 * qmi_decode_basic_elem() - Decodes elements of basic/primary data type
426 * @buf_dst: Buffer to store the decoded element.
427 * @buf_src: Buffer containing the elements in QMI wire format.
428 * @elem_len: Number of elements to be decoded.
429 * @elem_size: Size of a single instance of the element to be decoded.
430 *
431 * This function decodes the "elem_len" number of elements in QMI wire format,
432 * each of size "elem_size" bytes from the source buffer "buf_src" and stores
433 * the decoded elements in the destination buffer "buf_dst". The elements are
434 * of primary data type which include u8 - u64 or similar. This
435 * function returns the number of bytes of decoded information.
436 *
437 * Return: The total size of the decoded data elements, in bytes.
438 */
qmi_decode_basic_elem(void * buf_dst,const void * buf_src,u32 elem_len,u32 elem_size)439 static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src,
440 u32 elem_len, u32 elem_size)
441 {
442 u32 i, rc = 0;
443
444 for (i = 0; i < elem_len; i++) {
445 QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size);
446 rc += elem_size;
447 }
448
449 return rc;
450 }
451
452 /**
453 * qmi_decode_struct_elem() - Decodes elements of struct data type
454 * @ei_array: Struct info array describing the struct element.
455 * @buf_dst: Buffer to store the decoded element.
456 * @buf_src: Buffer containing the elements in QMI wire format.
457 * @elem_len: Number of elements to be decoded.
458 * @tlv_len: Total size of the encoded information corresponding to
459 * this struct element.
460 * @dec_level: Depth of the nested structure from the main structure.
461 *
462 * This function decodes the "elem_len" number of elements in QMI wire format,
463 * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src"
464 * and stores the decoded elements in the destination buffer "buf_dst". The
465 * elements are of struct data type which includes any C structure. This
466 * function returns the number of bytes of decoded information.
467 *
468 * Return: The total size of the decoded data elements on success, negative
469 * errno on error.
470 */
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)471 static int qmi_decode_struct_elem(const struct qmi_elem_info *ei_array,
472 void *buf_dst, const void *buf_src,
473 u32 elem_len, u32 tlv_len,
474 int dec_level)
475 {
476 int i, rc, decoded_bytes = 0;
477 const struct qmi_elem_info *temp_ei = ei_array;
478
479 for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) {
480 rc = qmi_decode(temp_ei->ei_array, buf_dst, buf_src,
481 tlv_len - decoded_bytes, dec_level);
482 if (rc < 0)
483 return rc;
484 buf_src = buf_src + rc;
485 buf_dst = buf_dst + temp_ei->elem_size;
486 decoded_bytes += rc;
487 }
488
489 if ((dec_level <= 2 && decoded_bytes != tlv_len) ||
490 (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) {
491 pr_err("%s: Fault in decoding: dl(%d), db(%d), tl(%d), i(%d), el(%d)\n",
492 __func__, dec_level, decoded_bytes, tlv_len,
493 i, elem_len);
494 return -EFAULT;
495 }
496
497 return decoded_bytes;
498 }
499
500 /**
501 * qmi_decode_string_elem() - Decodes elements of string data type
502 * @ei_array: Struct info array describing the string element.
503 * @buf_dst: Buffer to store the decoded element.
504 * @buf_src: Buffer containing the elements in QMI wire format.
505 * @tlv_len: Total size of the encoded information corresponding to
506 * this string element.
507 * @dec_level: Depth of the string element from the main structure.
508 *
509 * This function decodes the string element of maximum length
510 * "ei_array->elem_len" from the source buffer "buf_src" and puts it into
511 * the destination buffer "buf_dst". This function returns number of bytes
512 * decoded from the input buffer.
513 *
514 * Return: The total size of the decoded data elements on success, negative
515 * errno on error.
516 */
qmi_decode_string_elem(const struct qmi_elem_info * ei_array,void * buf_dst,const void * buf_src,u32 tlv_len,int dec_level)517 static int qmi_decode_string_elem(const struct qmi_elem_info *ei_array,
518 void *buf_dst, const void *buf_src,
519 u32 tlv_len, int dec_level)
520 {
521 int rc;
522 int decoded_bytes = 0;
523 u32 string_len = 0;
524 u32 string_len_sz = 0;
525 const struct qmi_elem_info *temp_ei = ei_array;
526
527 if (dec_level == 1) {
528 string_len = tlv_len;
529 } else {
530 string_len_sz = temp_ei->elem_len <= U8_MAX ?
531 sizeof(u8) : sizeof(u16);
532 rc = qmi_decode_basic_elem(&string_len, buf_src,
533 1, string_len_sz);
534 decoded_bytes += rc;
535 }
536
537 if (string_len >= temp_ei->elem_len) {
538 pr_err("%s: String len %d >= Max Len %d\n",
539 __func__, string_len, temp_ei->elem_len);
540 return -ETOOSMALL;
541 } else if (string_len > tlv_len) {
542 pr_err("%s: String len %d > Input Buffer Len %d\n",
543 __func__, string_len, tlv_len);
544 return -EFAULT;
545 }
546
547 rc = qmi_decode_basic_elem(buf_dst, buf_src + decoded_bytes,
548 string_len, temp_ei->elem_size);
549 *((char *)buf_dst + string_len) = '\0';
550 decoded_bytes += rc;
551
552 return decoded_bytes;
553 }
554
555 /**
556 * find_ei() - Find element info corresponding to TLV Type
557 * @ei_array: Struct info array of the message being decoded.
558 * @type: TLV Type of the element being searched.
559 *
560 * Every element that got encoded in the QMI message will have a type
561 * information associated with it. While decoding the QMI message,
562 * this function is used to find the struct info regarding the element
563 * that corresponds to the type being decoded.
564 *
565 * Return: Pointer to struct info, if found
566 */
find_ei(const struct qmi_elem_info * ei_array,u32 type)567 static const struct qmi_elem_info *find_ei(const struct qmi_elem_info *ei_array,
568 u32 type)
569 {
570 const struct qmi_elem_info *temp_ei = ei_array;
571
572 while (temp_ei->data_type != QMI_EOTI) {
573 if (temp_ei->tlv_type == (u8)type)
574 return temp_ei;
575 temp_ei = temp_ei + 1;
576 }
577
578 return NULL;
579 }
580
581 /**
582 * qmi_decode() - Core Decode Function
583 * @ei_array: Struct info array describing the structure to be decoded.
584 * @out_c_struct: Buffer to hold the decoded C struct
585 * @in_buf: Buffer containing the QMI message to be decoded
586 * @in_buf_len: Length of the QMI message to be decoded
587 * @dec_level: Decode level to indicate the depth of the nested structure,
588 * within the main structure, being decoded
589 *
590 * Return: The number of bytes of decoded information on success, negative
591 * errno on error.
592 */
qmi_decode(const struct qmi_elem_info * ei_array,void * out_c_struct,const void * in_buf,u32 in_buf_len,int dec_level)593 static int qmi_decode(const struct qmi_elem_info *ei_array, void *out_c_struct,
594 const void *in_buf, u32 in_buf_len,
595 int dec_level)
596 {
597 const struct qmi_elem_info *temp_ei = ei_array;
598 u8 opt_flag_value = 1;
599 u32 data_len_value = 0, data_len_sz = 0;
600 u8 *buf_dst = out_c_struct;
601 const u8 *tlv_pointer;
602 u32 tlv_len = 0;
603 u32 tlv_type;
604 u32 decoded_bytes = 0;
605 const void *buf_src = in_buf;
606 int rc;
607
608 while (decoded_bytes < in_buf_len) {
609 if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI)
610 return decoded_bytes;
611
612 if (dec_level == 1) {
613 tlv_pointer = buf_src;
614 QMI_ENCDEC_DECODE_TLV(&tlv_type,
615 &tlv_len, tlv_pointer);
616 buf_src += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
617 decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
618 temp_ei = find_ei(ei_array, tlv_type);
619 if (!temp_ei && tlv_type < OPTIONAL_TLV_TYPE_START) {
620 pr_err("%s: Inval element info\n", __func__);
621 return -EINVAL;
622 } else if (!temp_ei) {
623 UPDATE_DECODE_VARIABLES(buf_src,
624 decoded_bytes, tlv_len);
625 continue;
626 }
627 } else {
628 /*
629 * No length information for elements in nested
630 * structures. So use remaining decodable buffer space.
631 */
632 tlv_len = in_buf_len - decoded_bytes;
633 }
634
635 buf_dst = out_c_struct + temp_ei->offset;
636 if (temp_ei->data_type == QMI_OPT_FLAG) {
637 memcpy(buf_dst, &opt_flag_value, sizeof(u8));
638 temp_ei = temp_ei + 1;
639 buf_dst = out_c_struct + temp_ei->offset;
640 }
641
642 if (temp_ei->data_type == QMI_DATA_LEN) {
643 data_len_sz = temp_ei->elem_size == sizeof(u8) ?
644 sizeof(u8) : sizeof(u16);
645 rc = qmi_decode_basic_elem(&data_len_value, buf_src,
646 1, data_len_sz);
647 memcpy(buf_dst, &data_len_value, sizeof(u32));
648 temp_ei = temp_ei + 1;
649 buf_dst = out_c_struct + temp_ei->offset;
650 tlv_len -= data_len_sz;
651 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
652 }
653
654 if (temp_ei->array_type == NO_ARRAY) {
655 data_len_value = 1;
656 } else if (temp_ei->array_type == STATIC_ARRAY) {
657 data_len_value = temp_ei->elem_len;
658 } else if (data_len_value > temp_ei->elem_len) {
659 pr_err("%s: Data len %d > max spec %d\n",
660 __func__, data_len_value, temp_ei->elem_len);
661 return -ETOOSMALL;
662 }
663
664 switch (temp_ei->data_type) {
665 case QMI_UNSIGNED_1_BYTE:
666 case QMI_UNSIGNED_2_BYTE:
667 case QMI_UNSIGNED_4_BYTE:
668 case QMI_UNSIGNED_8_BYTE:
669 case QMI_SIGNED_2_BYTE_ENUM:
670 case QMI_SIGNED_4_BYTE_ENUM:
671 rc = qmi_decode_basic_elem(buf_dst, buf_src,
672 data_len_value,
673 temp_ei->elem_size);
674 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
675 break;
676
677 case QMI_STRUCT:
678 rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src,
679 data_len_value, tlv_len,
680 dec_level + 1);
681 if (rc < 0)
682 return rc;
683 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
684 break;
685
686 case QMI_STRING:
687 rc = qmi_decode_string_elem(temp_ei, buf_dst, buf_src,
688 tlv_len, dec_level);
689 if (rc < 0)
690 return rc;
691 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
692 break;
693
694 default:
695 pr_err("%s: Unrecognized data type\n", __func__);
696 return -EINVAL;
697 }
698 temp_ei = temp_ei + 1;
699 }
700
701 return decoded_bytes;
702 }
703
704 /**
705 * qmi_encode_message() - Encode C structure as QMI encoded message
706 * @type: Type of QMI message
707 * @msg_id: Message ID of the message
708 * @len: Passed as max length of the message, updated to actual size
709 * @txn_id: Transaction ID
710 * @ei: QMI message descriptor
711 * @c_struct: Reference to structure to encode
712 *
713 * Return: Buffer with encoded message, or negative ERR_PTR() on error
714 */
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)715 void *qmi_encode_message(int type, unsigned int msg_id, size_t *len,
716 unsigned int txn_id, const struct qmi_elem_info *ei,
717 const void *c_struct)
718 {
719 struct qmi_header *hdr;
720 ssize_t msglen = 0;
721 void *msg;
722 int ret;
723
724 /* Check the possibility of a zero length QMI message */
725 if (!c_struct) {
726 ret = qmi_calc_min_msg_len(ei, 1);
727 if (ret) {
728 pr_err("%s: Calc. len %d != 0, but NULL c_struct\n",
729 __func__, ret);
730 return ERR_PTR(-EINVAL);
731 }
732 }
733
734 msg = kzalloc(sizeof(*hdr) + *len, GFP_KERNEL);
735 if (!msg)
736 return ERR_PTR(-ENOMEM);
737
738 /* Encode message, if we have a message */
739 if (c_struct) {
740 msglen = qmi_encode(ei, msg + sizeof(*hdr), c_struct, *len, 1);
741 if (msglen < 0) {
742 kfree(msg);
743 return ERR_PTR(msglen);
744 }
745 }
746
747 hdr = msg;
748 hdr->type = type;
749 hdr->txn_id = txn_id;
750 hdr->msg_id = msg_id;
751 hdr->msg_len = msglen;
752
753 *len = sizeof(*hdr) + msglen;
754
755 return msg;
756 }
757 EXPORT_SYMBOL(qmi_encode_message);
758
759 /**
760 * qmi_decode_message() - Decode QMI encoded message to C structure
761 * @buf: Buffer with encoded message
762 * @len: Amount of data in @buf
763 * @ei: QMI message descriptor
764 * @c_struct: Reference to structure to decode into
765 *
766 * Return: The number of bytes of decoded information on success, negative
767 * errno on error.
768 */
qmi_decode_message(const void * buf,size_t len,const struct qmi_elem_info * ei,void * c_struct)769 int qmi_decode_message(const void *buf, size_t len,
770 const struct qmi_elem_info *ei, void *c_struct)
771 {
772 if (!ei)
773 return -EINVAL;
774
775 if (!c_struct || !buf || !len)
776 return -EINVAL;
777
778 return qmi_decode(ei, c_struct, buf + sizeof(struct qmi_header),
779 len - sizeof(struct qmi_header), 1);
780 }
781 EXPORT_SYMBOL(qmi_decode_message);
782
783 /* Common header in all QMI responses */
784 const struct qmi_elem_info qmi_response_type_v01_ei[] = {
785 {
786 .data_type = QMI_SIGNED_2_BYTE_ENUM,
787 .elem_len = 1,
788 .elem_size = sizeof(u16),
789 .array_type = NO_ARRAY,
790 .tlv_type = QMI_COMMON_TLV_TYPE,
791 .offset = offsetof(struct qmi_response_type_v01, result),
792 .ei_array = NULL,
793 },
794 {
795 .data_type = QMI_SIGNED_2_BYTE_ENUM,
796 .elem_len = 1,
797 .elem_size = sizeof(u16),
798 .array_type = NO_ARRAY,
799 .tlv_type = QMI_COMMON_TLV_TYPE,
800 .offset = offsetof(struct qmi_response_type_v01, error),
801 .ei_array = NULL,
802 },
803 {
804 .data_type = QMI_EOTI,
805 .elem_len = 0,
806 .elem_size = 0,
807 .array_type = NO_ARRAY,
808 .tlv_type = QMI_COMMON_TLV_TYPE,
809 .offset = 0,
810 .ei_array = NULL,
811 },
812 };
813 EXPORT_SYMBOL(qmi_response_type_v01_ei);
814
815 MODULE_DESCRIPTION("QMI encoder/decoder helper");
816 MODULE_LICENSE("GPL v2");
817