qmi_encdec.c

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