Message-Id: <20170804145938.25427-5-bjorn.andersson@linaro.org>
Date:   Fri,  4 Aug 2017 07:59:36 -0700
From:   Bjorn Andersson <bjorn.andersson@...aro.org>
To:     "David S. Miller" <davem@...emloft.net>,
        Andy Gross <andy.gross@...aro.org>,
        David Brown <david.brown@...aro.org>
Cc:     linux-arm-msm@...r.kernel.org, linux-soc@...r.kernel.org,
        netdev@...r.kernel.org, linux-kernel@...r.kernel.org
Subject: [PATCH 4/6] soc: qcom: Introduce QMI encoder/decoder

Add the helper library for encoding and decoding QMI encoded messages.
The implementation is taken from lib/qmi_encdec.c of the Qualcomm kernel
(msm-3.18).

Modifications has been made to the public API, source buffers has been
made const and the debug-logging part was omitted, for now.

Signed-off-by: Bjorn Andersson <bjorn.andersson@...aro.org>
---
 drivers/soc/qcom/Kconfig      |   8 +
 drivers/soc/qcom/Makefile     |   2 +
 drivers/soc/qcom/qmi_encdec.c | 812 ++++++++++++++++++++++++++++++++++++++++++
 include/linux/soc/qcom/qmi.h  | 116 ++++++
 4 files changed, 938 insertions(+)
 create mode 100644 drivers/soc/qcom/qmi_encdec.c
 create mode 100644 include/linux/soc/qcom/qmi.h

diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index 9fca977ef18d..2541ae07ad2a 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -24,6 +24,14 @@ config QCOM_PM
 	  modes. It interface with various system drivers to put the cores in
 	  low power modes.
 
+config QCOM_QMI_HELPERS
+	bool
+	help
+	  Helper library for handling QMI encoded messages.  QMI encoded
+	  messages are used in communication between the majority of QRTR
+	  clients and this helpers provide the common functionality needed for
+	  doing this from a kernel driver.
+
 config QCOM_SMEM
 	tristate "Qualcomm Shared Memory Manager (SMEM)"
 	depends on ARCH_QCOM
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index 414f0de274fa..27b60da7a062 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -1,6 +1,8 @@
 obj-$(CONFIG_QCOM_GSBI)	+=	qcom_gsbi.o
 obj-$(CONFIG_QCOM_MDT_LOADER)	+= mdt_loader.o
 obj-$(CONFIG_QCOM_PM)	+=	spm.o
+obj-$(CONFIG_QCOM_QMI_HELPERS)	+= qmi_helpers.o
+qmi_helpers-y	+= qmi_encdec.o
 obj-$(CONFIG_QCOM_SMD_RPM)	+= smd-rpm.o
 obj-$(CONFIG_QCOM_SMEM) +=	smem.o
 obj-$(CONFIG_QCOM_SMEM_STATE) += smem_state.o
diff --git a/drivers/soc/qcom/qmi_encdec.c b/drivers/soc/qcom/qmi_encdec.c
new file mode 100644
index 000000000000..4eb3099c64e7
--- /dev/null
+++ b/drivers/soc/qcom/qmi_encdec.c
@@ -0,0 +1,812 @@
+/*
+ * Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/string.h>
+#include <linux/soc/qcom/qmi.h>
+
+#define QMI_ENCDEC_ENCODE_TLV(type, length, p_dst) do { \
+	*p_dst++ = type; \
+	*p_dst++ = ((uint8_t)((length) & 0xFF)); \
+	*p_dst++ = ((uint8_t)(((length) >> 8) & 0xFF)); \
+} while (0)
+
+#define QMI_ENCDEC_DECODE_TLV(p_type, p_length, p_src) do { \
+	*p_type = (uint8_t)*p_src++; \
+	*p_length = (uint8_t)*p_src++; \
+	*p_length |= ((uint8_t)*p_src) << 8; \
+} while (0)
+
+#define QMI_ENCDEC_ENCODE_N_BYTES(p_dst, p_src, size) \
+do { \
+	memcpy(p_dst, p_src, size); \
+	p_dst = (uint8_t *)p_dst + size; \
+	p_src = (uint8_t *)p_src + size; \
+} while (0)
+
+#define QMI_ENCDEC_DECODE_N_BYTES(p_dst, p_src, size) \
+do { \
+	memcpy(p_dst, p_src, size); \
+	p_dst = (uint8_t *)p_dst + size; \
+	p_src = (uint8_t *)p_src + size; \
+} while (0)
+
+#define UPDATE_ENCODE_VARIABLES(temp_si, buf_dst, \
+				encoded_bytes, tlv_len, encode_tlv, rc) \
+do { \
+	buf_dst = (uint8_t *)buf_dst + rc; \
+	encoded_bytes += rc; \
+	tlv_len += rc; \
+	temp_si = temp_si + 1; \
+	encode_tlv = 1; \
+} while (0)
+
+#define UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc) \
+do { \
+	buf_src = (uint8_t *)buf_src + rc; \
+	decoded_bytes += rc; \
+} while (0)
+
+#define TLV_LEN_SIZE sizeof(uint16_t)
+#define TLV_TYPE_SIZE sizeof(uint8_t)
+#define OPTIONAL_TLV_TYPE_START 0x10
+
+static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf,
+		      const void *in_c_struct, uint32_t out_buf_len,
+		      int enc_level);
+
+static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct,
+		      const void *in_buf, uint32_t in_buf_len, int dec_level);
+
+/**
+ * skip_to_next_elem() - Skip to next element in the structure to be encoded
+ * @ei_array: Struct info describing the element to be skipped.
+ * @level: Depth level of encoding/decoding to identify nested structures.
+ *
+ * Returns struct info of the next element that can be encoded.
+ *
+ * This function is used while encoding optional elements. If the flag
+ * corresponding to an optional element is not set, then encoding the
+ * optional element can be skipped. This function can be used to perform
+ * that operation.
+ */
+static struct qmi_elem_info *skip_to_next_elem(struct qmi_elem_info *ei_array,
+					   int level)
+{
+	struct qmi_elem_info *temp_ei = ei_array;
+	uint8_t tlv_type;
+
+	if (level > 1) {
+		temp_ei = temp_ei + 1;
+	} else {
+		do {
+			tlv_type = temp_ei->tlv_type;
+			temp_ei = temp_ei + 1;
+		} while (tlv_type == temp_ei->tlv_type);
+	}
+
+	return temp_ei;
+}
+
+/**
+ * qmi_calc_min_msg_len() - Calculate the minimum length of a QMI message
+ * @ei_array: Struct info array describing the structure.
+ * @level: Level to identify the depth of the nested structures.
+ *
+ * Returns expected minimum length of the QMI message or 0 on error.
+ */
+static int qmi_calc_min_msg_len(struct qmi_elem_info *ei_array,
+				int level)
+{
+	int min_msg_len = 0;
+	struct qmi_elem_info *temp_ei = ei_array;
+
+	if (!ei_array)
+		return min_msg_len;
+
+	while (temp_ei->data_type != QMI_EOTI) {
+		/* Optional elements do not count in minimum length */
+		if (temp_ei->data_type == QMI_OPT_FLAG) {
+			temp_ei = skip_to_next_elem(temp_ei, level);
+			continue;
+		}
+
+		if (temp_ei->data_type == QMI_DATA_LEN) {
+			min_msg_len += (temp_ei->elem_size == sizeof(uint8_t) ?
+					sizeof(uint8_t) : sizeof(uint16_t));
+			temp_ei++;
+			continue;
+		} else if (temp_ei->data_type == QMI_STRUCT) {
+			min_msg_len += qmi_calc_min_msg_len(temp_ei->ei_array,
+							    (level + 1));
+			temp_ei++;
+		} else if (temp_ei->data_type == QMI_STRING) {
+			if (level > 1)
+				min_msg_len += temp_ei->elem_len <= U8_MAX ?
+					sizeof(uint8_t) : sizeof(uint16_t);
+			min_msg_len += temp_ei->elem_len * temp_ei->elem_size;
+			temp_ei++;
+		} else {
+			min_msg_len += (temp_ei->elem_len * temp_ei->elem_size);
+			temp_ei++;
+		}
+
+		/*
+		 * Type & Length info. not prepended for elements in the
+		 * nested structure.
+		 */
+		if (level == 1)
+			min_msg_len += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+	}
+	return min_msg_len;
+}
+
+/**
+ * qmi_encode_basic_elem() - Encodes elements of basic/primary data type
+ * @buf_dst: Buffer to store the encoded information.
+ * @buf_src: Buffer containing the elements to be encoded.
+ * @elem_len: Number of elements, in the buf_src, to be encoded.
+ * @elem_size: Size of a single instance of the element to be encoded.
+ *
+ * Returns the number of bytes of encoded information.
+ *
+ * This function encodes the "elem_len" number of data elements, each of
+ * size "elem_size" bytes from the source buffer "buf_src" and stores the
+ * encoded information in the destination buffer "buf_dst". The elements are
+ * of primary data type which include uint8_t - uint64_t or similar. This
+ * function returns the number of bytes of encoded information.
+ */
+static int qmi_encode_basic_elem(void *buf_dst, const void *buf_src,
+				 uint32_t elem_len, uint32_t elem_size)
+{
+	uint32_t i, rc = 0;
+
+	for (i = 0; i < elem_len; i++) {
+		QMI_ENCDEC_ENCODE_N_BYTES(buf_dst, buf_src, elem_size);
+		rc += elem_size;
+	}
+
+	return rc;
+}
+
+/**
+ * qmi_encode_struct_elem() - Encodes elements of struct data type
+ * @ei_array: Struct info array descibing the struct element.
+ * @buf_dst: Buffer to store the encoded information.
+ * @buf_src: Buffer containing the elements to be encoded.
+ * @elem_len: Number of elements, in the buf_src, to be encoded.
+ * @out_buf_len: Available space in the encode buffer.
+ * @enc_level: Depth of the nested structure from the main structure.
+ *
+ * Returns the number of bytes of encoded information on success or negative
+ * errno on error.
+ *
+ * This function encodes the "elem_len" number of struct elements, each of
+ * size "ei_array->elem_size" bytes from the source buffer "buf_src" and
+ * stores the encoded information in the destination buffer "buf_dst". The
+ * elements are of struct data type which includes any C structure. This
+ * function returns the number of bytes of encoded information.
+ */
+static int qmi_encode_struct_elem(struct qmi_elem_info *ei_array,
+				  void *buf_dst, const void *buf_src,
+				  uint32_t elem_len, uint32_t out_buf_len,
+				  int enc_level)
+{
+	int i, rc, encoded_bytes = 0;
+	struct qmi_elem_info *temp_ei = ei_array;
+
+	for (i = 0; i < elem_len; i++) {
+		rc = qmi_encode(temp_ei->ei_array, buf_dst, buf_src,
+				out_buf_len - encoded_bytes, enc_level);
+		if (rc < 0) {
+			pr_err("%s: STRUCT Encode failure\n", __func__);
+			return rc;
+		}
+		buf_dst = buf_dst + rc;
+		buf_src = buf_src + temp_ei->elem_size;
+		encoded_bytes += rc;
+	}
+
+	return encoded_bytes;
+}
+
+/**
+ * qmi_encode_string_elem() - Encodes elements of string data type
+ * @ei_array: Struct info array descibing the string element.
+ * @buf_dst: Buffer to store the encoded information.
+ * @buf_src: Buffer containing the elements to be encoded.
+ * @out_buf_len: Available space in the encode buffer.
+ * @enc_level: Depth of the string element from the main structure.
+ *
+ * Returns the number of bytes of encoded information on success or negative
+ * errno on error.
+ *
+ * This function encodes a string element of maximum length "ei_array->elem_len"
+ * bytes from the source buffer "buf_src" and stores the encoded information in
+ * the destination buffer "buf_dst". This function returns the number of bytes
+ * of encoded information.
+ */
+static int qmi_encode_string_elem(struct qmi_elem_info *ei_array,
+				  void *buf_dst, const void *buf_src,
+				  uint32_t out_buf_len, int enc_level)
+{
+	int rc;
+	int encoded_bytes = 0;
+	struct qmi_elem_info *temp_ei = ei_array;
+	uint32_t string_len = 0;
+	uint32_t string_len_sz = 0;
+
+	string_len = strlen(buf_src);
+	string_len_sz = temp_ei->elem_len <= U8_MAX ?
+			sizeof(uint8_t) : sizeof(uint16_t);
+	if (string_len > temp_ei->elem_len) {
+		pr_err("%s: String to be encoded is longer - %d > %d\n",
+			__func__, string_len, temp_ei->elem_len);
+		return -EINVAL;
+	}
+
+	if (enc_level == 1) {
+		if (string_len + TLV_LEN_SIZE + TLV_TYPE_SIZE >
+		    out_buf_len) {
+			pr_err("%s: Output len %d > Out Buf len %d\n",
+				__func__, string_len, out_buf_len);
+			return -ETOOSMALL;
+		}
+	} else {
+		if (string_len + string_len_sz > out_buf_len) {
+			pr_err("%s: Output len %d > Out Buf len %d\n",
+				__func__, string_len, out_buf_len);
+			return -ETOOSMALL;
+		}
+		rc = qmi_encode_basic_elem(buf_dst, &string_len,
+					   1, string_len_sz);
+		encoded_bytes += rc;
+	}
+
+	rc = qmi_encode_basic_elem(buf_dst + encoded_bytes, buf_src,
+				   string_len, temp_ei->elem_size);
+	encoded_bytes += rc;
+	return encoded_bytes;
+}
+
+/**
+ * qmi_encode() - Core Encode Function
+ * @ei_array: Struct info array describing the structure to be encoded.
+ * @out_buf: Buffer to hold the encoded QMI message.
+ * @in_c_struct: Pointer to the C structure to be encoded.
+ * @out_buf_len: Available space in the encode buffer.
+ * @enc_level: Encode level to indicate the depth of the nested structure,
+ *             within the main structure, being encoded.
+ *
+ * Returns the number of bytes of encoded information on success or negative
+ * errno on error.
+ */
+static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf,
+		      const void *in_c_struct, uint32_t out_buf_len,
+		      int enc_level)
+{
+	struct qmi_elem_info *temp_ei = ei_array;
+	uint8_t opt_flag_value = 0;
+	uint32_t data_len_value = 0, data_len_sz;
+	uint8_t *buf_dst = (uint8_t *)out_buf;
+	uint8_t *tlv_pointer;
+	uint32_t tlv_len;
+	uint8_t tlv_type;
+	uint32_t encoded_bytes = 0;
+	const void *buf_src;
+	int encode_tlv = 0;
+	int rc;
+
+	tlv_pointer = buf_dst;
+	tlv_len = 0;
+	if (enc_level == 1)
+		buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE);
+
+	while (temp_ei->data_type != QMI_EOTI) {
+		buf_src = in_c_struct + temp_ei->offset;
+		tlv_type = temp_ei->tlv_type;
+
+		if (temp_ei->is_array == NO_ARRAY) {
+			data_len_value = 1;
+		} else if (temp_ei->is_array == STATIC_ARRAY) {
+			data_len_value = temp_ei->elem_len;
+		} else if (data_len_value <= 0 ||
+			    temp_ei->elem_len < data_len_value) {
+			pr_err("%s: Invalid data length\n", __func__);
+			return -EINVAL;
+		}
+
+		switch (temp_ei->data_type) {
+		case QMI_OPT_FLAG:
+			rc = qmi_encode_basic_elem(&opt_flag_value, buf_src,
+						   1, sizeof(uint8_t));
+			if (opt_flag_value)
+				temp_ei = temp_ei + 1;
+			else
+				temp_ei = skip_to_next_elem(temp_ei, enc_level);
+			break;
+
+		case QMI_DATA_LEN:
+			memcpy(&data_len_value, buf_src, temp_ei->elem_size);
+			data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ?
+					sizeof(uint8_t) : sizeof(uint16_t);
+			/* Check to avoid out of range buffer access */
+			if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE +
+			    TLV_TYPE_SIZE) > out_buf_len) {
+				pr_err("%s: Too Small Buffer @DATA_LEN\n",
+					__func__);
+				return -ETOOSMALL;
+			}
+			rc = qmi_encode_basic_elem(buf_dst, &data_len_value,
+						   1, data_len_sz);
+			UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+				encoded_bytes, tlv_len, encode_tlv, rc);
+			if (!data_len_value)
+				temp_ei = skip_to_next_elem(temp_ei, enc_level);
+			else
+				encode_tlv = 0;
+			break;
+
+		case QMI_UNSIGNED_1_BYTE:
+		case QMI_UNSIGNED_2_BYTE:
+		case QMI_UNSIGNED_4_BYTE:
+		case QMI_UNSIGNED_8_BYTE:
+		case QMI_SIGNED_2_BYTE_ENUM:
+		case QMI_SIGNED_4_BYTE_ENUM:
+			/* Check to avoid out of range buffer access */
+			if (((data_len_value * temp_ei->elem_size) +
+			    encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) >
+			    out_buf_len) {
+				pr_err("%s: Too Small Buffer @data_type:%d\n",
+					__func__, temp_ei->data_type);
+				return -ETOOSMALL;
+			}
+			rc = qmi_encode_basic_elem(buf_dst, buf_src,
+				data_len_value, temp_ei->elem_size);
+			UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+				encoded_bytes, tlv_len, encode_tlv, rc);
+			break;
+
+		case QMI_STRUCT:
+			rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src,
+				data_len_value, (out_buf_len - encoded_bytes),
+				(enc_level + 1));
+			if (rc < 0)
+				return rc;
+			UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+				encoded_bytes, tlv_len, encode_tlv, rc);
+			break;
+
+		case QMI_STRING:
+			rc = qmi_encode_string_elem(temp_ei, buf_dst, buf_src,
+				out_buf_len - encoded_bytes, enc_level);
+			if (rc < 0)
+				return rc;
+			UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+				encoded_bytes, tlv_len, encode_tlv, rc);
+			break;
+		default:
+			pr_err("%s: Unrecognized data type\n", __func__);
+			return -EINVAL;
+
+		}
+
+		if (encode_tlv && enc_level == 1) {
+			QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer);
+			encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+			tlv_pointer = buf_dst;
+			tlv_len = 0;
+			buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE;
+			encode_tlv = 0;
+		}
+	}
+	return encoded_bytes;
+}
+
+/**
+ * qmi_decode_basic_elem() - Decodes elements of basic/primary data type
+ * @buf_dst: Buffer to store the decoded element.
+ * @buf_src: Buffer containing the elements in QMI wire format.
+ * @elem_len: Number of elements to be decoded.
+ * @elem_size: Size of a single instance of the element to be decoded.
+ *
+ * Returns the total size of the decoded data elements, in bytes.
+ *
+ * This function decodes the "elem_len" number of elements in QMI wire format,
+ * each of size "elem_size" bytes from the source buffer "buf_src" and stores
+ * the decoded elements in the destination buffer "buf_dst". The elements are
+ * of primary data type which include uint8_t - uint64_t or similar. This
+ * function returns the number of bytes of decoded information.
+ */
+static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src,
+				 uint32_t elem_len, uint32_t elem_size)
+{
+	uint32_t i, rc = 0;
+
+	for (i = 0; i < elem_len; i++) {
+		QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size);
+		rc += elem_size;
+	}
+
+	return rc;
+}
+
+/**
+ * qmi_decode_struct_elem() - Decodes elements of struct data type
+ * @ei_array: Struct info array descibing the struct element.
+ * @buf_dst: Buffer to store the decoded element.
+ * @buf_src: Buffer containing the elements in QMI wire format.
+ * @elem_len: Number of elements to be decoded.
+ * @tlv_len: Total size of the encoded inforation corresponding to
+ *           this struct element.
+ * @dec_level: Depth of the nested structure from the main structure.
+ *
+ * Returns the total size of the decoded data elements on success, negative
+ * errno on error.
+ *
+ * This function decodes the "elem_len" number of elements in QMI wire format,
+ * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src"
+ * and stores the decoded elements in the destination buffer "buf_dst". The
+ * elements are of struct data type which includes any C structure. This
+ * function returns the number of bytes of decoded information.
+ */
+static int qmi_decode_struct_elem(struct qmi_elem_info *ei_array,
+				  void *buf_dst, const void *buf_src,
+				  uint32_t elem_len, uint32_t tlv_len,
+				  int dec_level)
+{
+	int i, rc, decoded_bytes = 0;
+	struct qmi_elem_info *temp_ei = ei_array;
+
+	for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) {
+		rc = qmi_decode(temp_ei->ei_array, buf_dst, buf_src,
+				tlv_len - decoded_bytes, dec_level);
+		if (rc < 0)
+			return rc;
+		buf_src = buf_src + rc;
+		buf_dst = buf_dst + temp_ei->elem_size;
+		decoded_bytes += rc;
+	}
+
+	if ((dec_level <= 2 && decoded_bytes != tlv_len) ||
+	    (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) {
+		pr_err("%s: Fault in decoding: dl(%d), db(%d), tl(%d), i(%d), el(%d)\n",
+			__func__, dec_level, decoded_bytes, tlv_len,
+			i, elem_len);
+		return -EFAULT;
+	}
+	return decoded_bytes;
+}
+
+/**
+ * qmi_decode_string_elem() - Decodes elements of string data type
+ * @ei_array: Struct info array descibing the string element.
+ * @buf_dst: Buffer to store the decoded element.
+ * @buf_src: Buffer containing the elements in QMI wire format.
+ * @tlv_len: Total size of the encoded inforation corresponding to
+ *           this string element.
+ * @dec_level: Depth of the string element from the main structure.
+ *
+
+ * Returns the total size of the decoded data elements on success, negative
+ * errno on error.
+
+ *
+ * This function decodes the string element of maximum length
+ * "ei_array->elem_len" from the source buffer "buf_src" and puts it into
+ * the destination buffer "buf_dst". This function returns number of bytes
+ * decoded from the input buffer.
+ */
+static int qmi_decode_string_elem(struct qmi_elem_info *ei_array,
+				  void *buf_dst, const void *buf_src,
+				  uint32_t tlv_len, int dec_level)
+{
+	int rc;
+	int decoded_bytes = 0;
+	uint32_t string_len = 0;
+	uint32_t string_len_sz = 0;
+	struct qmi_elem_info *temp_ei = ei_array;
+
+	if (dec_level == 1) {
+		string_len = tlv_len;
+	} else {
+		string_len_sz = temp_ei->elem_len <= U8_MAX ?
+				sizeof(uint8_t) : sizeof(uint16_t);
+		rc = qmi_decode_basic_elem(&string_len, buf_src,
+					   1, string_len_sz);
+		decoded_bytes += rc;
+	}
+
+	if (string_len > temp_ei->elem_len) {
+		pr_err("%s: String len %d > Max Len %d\n",
+			__func__, string_len, temp_ei->elem_len);
+		return -ETOOSMALL;
+	} else if (string_len > tlv_len) {
+		pr_err("%s: String len %d > Input Buffer Len %d\n",
+			__func__, string_len, tlv_len);
+		return -EFAULT;
+	}
+
+	rc = qmi_decode_basic_elem(buf_dst, buf_src + decoded_bytes,
+				   string_len, temp_ei->elem_size);
+	*((char *)buf_dst + string_len) = '\0';
+	decoded_bytes += rc;
+	return decoded_bytes;
+}
+
+/**
+ * find_ei() - Find element info corresponding to TLV Type
+ * @ei_array: Struct info array of the message being decoded.
+ * @type: TLV Type of the element being searched.
+ *
+ * Returns pointer to struct info, if found
+ *
+ * Every element that got encoded in the QMI message will have a type
+ * information associated with it. While decoding the QMI message,
+ * this function is used to find the struct info regarding the element
+ * that corresponds to the type being decoded.
+ */
+static struct qmi_elem_info *find_ei(struct qmi_elem_info *ei_array,
+				   uint32_t type)
+{
+	struct qmi_elem_info *temp_ei = ei_array;
+	while (temp_ei->data_type != QMI_EOTI) {
+		if (temp_ei->tlv_type == (uint8_t)type)
+			return temp_ei;
+		temp_ei = temp_ei + 1;
+	}
+	return NULL;
+}
+
+/**
+ * qmi_decode() - Core Decode Function
+ * @ei_array: Struct info array describing the structure to be decoded.
+ * @out_c_struct: Buffer to hold the decoded C struct
+ * @in_buf: Buffer containing the QMI message to be decoded
+ * @in_buf_len: Length of the QMI message to be decoded
+ * @dec_level: Decode level to indicate the depth of the nested structure,
+ *             within the main structure, being decoded
+ *
+ * Returns the number of bytes of decoded information on success, negative
+ * errno on error.
+ */
+static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct,
+		      const void *in_buf, uint32_t in_buf_len,
+		      int dec_level)
+{
+	struct qmi_elem_info *temp_ei = ei_array;
+	uint8_t opt_flag_value = 1;
+	uint32_t data_len_value = 0, data_len_sz = 0;
+	uint8_t *buf_dst = out_c_struct;
+	const uint8_t *tlv_pointer;
+	uint32_t tlv_len = 0;
+	uint32_t tlv_type;
+	uint32_t decoded_bytes = 0;
+	const void *buf_src = in_buf;
+	int rc;
+
+	while (decoded_bytes < in_buf_len) {
+		if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI)
+			return decoded_bytes;
+
+		if (dec_level == 1) {
+			tlv_pointer = buf_src;
+			QMI_ENCDEC_DECODE_TLV(&tlv_type,
+					      &tlv_len, tlv_pointer);
+			buf_src += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+			decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+			temp_ei = find_ei(ei_array, tlv_type);
+			if (!temp_ei && (tlv_type < OPTIONAL_TLV_TYPE_START)) {
+				pr_err("%s: Inval element info\n", __func__);
+				return -EINVAL;
+			} else if (!temp_ei) {
+				UPDATE_DECODE_VARIABLES(buf_src,
+						decoded_bytes, tlv_len);
+				continue;
+			}
+		} else {
+			/*
+			 * No length information for elements in nested
+			 * structures. So use remaining decodable buffer space.
+			 */
+			tlv_len = in_buf_len - decoded_bytes;
+		}
+
+		buf_dst = out_c_struct + temp_ei->offset;
+		if (temp_ei->data_type == QMI_OPT_FLAG) {
+			memcpy(buf_dst, &opt_flag_value, sizeof(uint8_t));
+			temp_ei = temp_ei + 1;
+			buf_dst = out_c_struct + temp_ei->offset;
+		}
+
+		if (temp_ei->data_type == QMI_DATA_LEN) {
+			data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ?
+					sizeof(uint8_t) : sizeof(uint16_t);
+			rc = qmi_decode_basic_elem(&data_len_value, buf_src,
+						   1, data_len_sz);
+			memcpy(buf_dst, &data_len_value, sizeof(uint32_t));
+			temp_ei = temp_ei + 1;
+			buf_dst = out_c_struct + temp_ei->offset;
+			tlv_len -= data_len_sz;
+			UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+		}
+
+		if (temp_ei->is_array == NO_ARRAY) {
+			data_len_value = 1;
+		} else if (temp_ei->is_array == STATIC_ARRAY) {
+			data_len_value = temp_ei->elem_len;
+		} else if (data_len_value > temp_ei->elem_len) {
+			pr_err("%s: Data len %d > max spec %d\n",
+				__func__, data_len_value, temp_ei->elem_len);
+			return -ETOOSMALL;
+		}
+
+		switch (temp_ei->data_type) {
+		case QMI_UNSIGNED_1_BYTE:
+		case QMI_UNSIGNED_2_BYTE:
+		case QMI_UNSIGNED_4_BYTE:
+		case QMI_UNSIGNED_8_BYTE:
+		case QMI_SIGNED_2_BYTE_ENUM:
+		case QMI_SIGNED_4_BYTE_ENUM:
+			rc = qmi_decode_basic_elem(buf_dst, buf_src,
+				data_len_value, temp_ei->elem_size);
+			UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+			break;
+
+		case QMI_STRUCT:
+			rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src,
+				data_len_value, tlv_len, (dec_level + 1));
+			if (rc < 0)
+				return rc;
+			UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+			break;
+
+		case QMI_STRING:
+			rc = qmi_decode_string_elem(temp_ei, buf_dst, buf_src,
+						     tlv_len, dec_level);
+			if (rc < 0)
+				return rc;
+			UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+			break;
+
+		default:
+			pr_err("%s: Unrecognized data type\n", __func__);
+			return -EINVAL;
+		}
+		temp_ei = temp_ei + 1;
+	}
+	return decoded_bytes;
+}
+
+/**
+ * qmi_encode_message() - Encode C structure as QMI encoded message
+ * @type:	Type of QMI message
+ * @msg_id:	Message ID of the message
+ * @len:	Passed as max length of the message, updated to actual size
+ * @txn_id:	Transaction ID
+ * @ei:		QMI message descriptor
+ * @c_struct:	Reference to structure to encode
+ *
+ * Returns buffer with encoded message, or negative ERR_PTR() on error
+ */
+void *qmi_encode_message(int type, unsigned int msg_id, size_t *len,
+			 unsigned int txn_id, struct qmi_elem_info *ei,
+			 const void *c_struct)
+{
+	struct qmi_header *hdr;
+	void *msg;
+	int ret;
+
+	/* Check the possibility of a zero length QMI message */
+	if (!c_struct) {
+		ret = qmi_calc_min_msg_len(ei, 1);
+		if (ret) {
+			pr_err("%s: Calc. len %d != 0, but NULL c_struct\n",
+				__func__, ret);
+			return ERR_PTR(-EINVAL);
+		} else {
+			return NULL;
+		}
+	}
+
+	if (!ei)
+		return ERR_PTR(-EINVAL);
+
+	hdr = msg = kzalloc(sizeof(*hdr) + *len, GFP_KERNEL);
+	if (!msg)
+		return ERR_PTR(-ENOMEM);
+
+	ret = qmi_encode(ei, msg + sizeof(*hdr), c_struct, *len, 1);
+	if (ret < 0) {
+		kfree(msg);
+		return ERR_PTR(ret);
+	}
+
+	hdr->type = type;
+	hdr->txn_id = txn_id;
+	hdr->msg_id = msg_id;
+	hdr->msg_len = ret;
+
+	*len = sizeof(*hdr) + ret;
+
+	return msg;
+}
+EXPORT_SYMBOL(qmi_encode_message);
+
+/**
+ * qmi_decode_message() - Decode QMI encoded message to C structure
+ * @buf:	Buffer with encoded message
+ * @len:	Amount of data in @buf
+ * @ei:		QMI message descriptor
+ * @c_struct:	Reference to structure to decode into
+ *
+ * Returns the number of bytes of decoded information on success, negative
+ * errno on error.
+ */
+int qmi_decode_message(const void *buf, size_t len,
+		       struct qmi_elem_info *ei, void *c_struct)
+{
+	if (!ei)
+		return -EINVAL;
+
+	if (!c_struct || !buf || !len)
+		return -EINVAL;
+
+	return qmi_decode(ei, c_struct, buf + sizeof(struct qmi_header),
+			  len - sizeof(struct qmi_header), 1);
+}
+EXPORT_SYMBOL(qmi_decode_message);
+
+/* Common header in all QMI responses */
+struct qmi_elem_info qmi_response_type_v01_ei[] = {
+	{
+		.data_type	= QMI_SIGNED_2_BYTE_ENUM,
+		.elem_len	= 1,
+		.elem_size	= sizeof(uint16_t),
+		.is_array	= NO_ARRAY,
+		.tlv_type	= QMI_COMMON_TLV_TYPE,
+		.offset		= offsetof(struct qmi_response_type_v01,
+					   result),
+		.ei_array	= NULL,
+	},
+	{
+		.data_type      = QMI_SIGNED_2_BYTE_ENUM,
+		.elem_len       = 1,
+		.elem_size      = sizeof(uint16_t),
+		.is_array       = NO_ARRAY,
+		.tlv_type       = QMI_COMMON_TLV_TYPE,
+		.offset         = offsetof(struct qmi_response_type_v01,
+					   error),
+		.ei_array       = NULL,
+	},
+	{
+		.data_type	= QMI_EOTI,
+		.elem_len	= 0,
+		.elem_size	= 0,
+		.is_array	= NO_ARRAY,
+		.tlv_type	= QMI_COMMON_TLV_TYPE,
+		.offset		= 0,
+		.ei_array	= NULL,
+	},
+};
+EXPORT_SYMBOL(qmi_response_type_v01_ei);
+
+MODULE_DESCRIPTION("QMI encoder/decoder helper");
+MODULE_LICENSE("GPL v2");
diff --git a/include/linux/soc/qcom/qmi.h b/include/linux/soc/qcom/qmi.h
new file mode 100644
index 000000000000..c012a1e9e24b
--- /dev/null
+++ b/include/linux/soc/qcom/qmi.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2012-2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017, Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef __QMI_HELPERS_H__
+#define __QMI_HELPERS_H__
+
+#include <linux/qrtr.h>
+#include <linux/types.h>
+
+/**
+ * qmi_header - wireformat header of QMI messages
+ * @type:	type of message
+ * @txn_id:	transaction id
+ * @msg_id:	message id
+ * @msg_len:	length of message payload following header
+ */
+struct qmi_header {
+	uint8_t type;
+	uint16_t txn_id;
+	uint16_t msg_id;
+	uint16_t msg_len;
+} __packed;
+
+#define QMI_REQUEST	0
+#define QMI_RESPONSE	2
+#define QMI_INDICATION	4
+
+#define QMI_COMMON_TLV_TYPE 0
+
+enum qmi_elem_type {
+	QMI_EOTI,
+	QMI_OPT_FLAG,
+	QMI_DATA_LEN,
+	QMI_UNSIGNED_1_BYTE,
+	QMI_UNSIGNED_2_BYTE,
+	QMI_UNSIGNED_4_BYTE,
+	QMI_UNSIGNED_8_BYTE,
+	QMI_SIGNED_2_BYTE_ENUM,
+	QMI_SIGNED_4_BYTE_ENUM,
+	QMI_STRUCT,
+	QMI_STRING,
+};
+
+enum qmi_array_type {
+	NO_ARRAY,
+	STATIC_ARRAY,
+	VAR_LEN_ARRAY,
+};
+
+/**
+ * struct qmi_elem_info - describes how to encode a single QMI element
+ * @data_type:	Data type of this element.
+ * @elem_len:	Array length of this element, if an array.
+ * @elem_size:	Size of a single instance of this data type.
+ * @is_array:	Array type of this element.
+ * @tlv_type:	QMI message specific type to identify which element
+ *		is present in an incoming message.
+ * @offset:	Specifies the offset of the first instance of this
+ *		element in the data structure.
+ * @ei_array:	Null-terminated array of @qmi_elem_info to describe nested
+ *		structures.
+ */
+struct qmi_elem_info {
+	enum qmi_elem_type data_type;
+	uint32_t elem_len;
+	uint32_t elem_size;
+	enum qmi_array_type is_array;
+	uint8_t tlv_type;
+	uint32_t offset;
+	struct qmi_elem_info *ei_array;
+};
+
+#define QMI_RESULT_SUCCESS_V01			0
+#define QMI_RESULT_FAILURE_V01			1
+
+#define QMI_ERR_NONE_V01			0
+#define QMI_ERR_MALFORMED_MSG_V01		1
+#define QMI_ERR_NO_MEMORY_V01			2
+#define QMI_ERR_INTERNAL_V01			3
+#define QMI_ERR_CLIENT_IDS_EXHAUSTED_V01	5
+#define QMI_ERR_INVALID_ID_V01			41
+#define QMI_ERR_ENCODING_V01			58
+#define QMI_ERR_INCOMPATIBLE_STATE_V01		90
+#define QMI_ERR_NOT_SUPPORTED_V01		94
+
+/**
+ * qmi_response_type_v01 - common response header (decoded)
+ * @result:	result of the transaction
+ * @error:	error value, when @result is QMI_RESULT_FAILURE_V01
+ */
+struct qmi_response_type_v01 {
+	u32 result;
+	u32 error;
+};
+
+extern struct qmi_elem_info qmi_response_type_v01_ei[];
+
+void *qmi_encode_message(int type, unsigned int msg_id, size_t *len,
+			 unsigned int txn_id, struct qmi_elem_info *ei,
+			 const void *c_struct);
+
+int qmi_decode_message(const void *buf, size_t len,
+		       struct qmi_elem_info *ei, void *c_struct);
+
+
+#endif
-- 
2.12.0

Powered by blists - more mailing lists