xref: /linux/drivers/soc/qcom/qmi_encdec.c (revision 0ea5c948cb64bab5bc7a5516774eb8536f05aa0d)
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_GPL(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_GPL(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_GPL(qmi_response_type_v01_ei);
814 
815 MODULE_DESCRIPTION("QMI encoder/decoder helper");
816 MODULE_LICENSE("GPL v2");
817