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