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.

Acked-by: Chris Lew <clew@codeaurora.org>
Tested-by: Chris Lew <clew@codeaurora.org>
Tested-by: Srinivas Kandagatla <srinivas.kandagatla@linaro.org>
Signed-off-by: Bjorn Andersson <bjorn.andersson@linaro.org>
Signed-off-by: Andy Gross <andy.gross@linaro.org>

4 files changed, 933 insertions, 0 deletions

diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index 40c711583f0d..e050eb83341d 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -35,6 +35,15 @@ config QCOM_PM
           modes. It interface with various system drivers to put the cores in
           low power modes.
 
+config QCOM_QMI_HELPERS
+        tristate
+        depends on ARCH_QCOM
+        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_RMTFS_MEM
         tristate "Qualcomm Remote Filesystem memory driver"
         depends on ARCH_QCOM
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index 40c56f67e94a..37f85b45d0a1 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -3,6 +3,8 @@ obj-$(CONFIG_QCOM_GLINK_SSR) +=	glink_ssr.o
 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_RMTFS_MEM)    += rmtfs_mem.o
 obj-$(CONFIG_QCOM_SMD_RPM)      += smd-rpm.o
 obj-$(CONFIG_QCOM_SMEM) +=      smem.o
diff --git a/drivers/soc/qcom/qmi_encdec.c b/drivers/soc/qcom/qmi_encdec.c
new file mode 100644
index 000000000000..3aaab71d1b2c
--- /dev/null
+++ b/drivers/soc/qcom/qmi_encdec.c
@@ -0,0 +1,816 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ */
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.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++ = ((u8)((length) & 0xFF)); \
+        *p_dst++ = ((u8)(((length) >> 8) & 0xFF)); \
+} while (0)
+
+#define QMI_ENCDEC_DECODE_TLV(p_type, p_length, p_src) do { \
+        *p_type = (u8)*p_src++; \
+        *p_length = (u8)*p_src++; \
+        *p_length |= ((u8)*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 = (u8 *)p_dst + size; \
+        p_src = (u8 *)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 = (u8 *)p_dst + size; \
+        p_src = (u8 *)p_src + size; \
+} while (0)
+
+#define UPDATE_ENCODE_VARIABLES(temp_si, buf_dst, \
+                                encoded_bytes, tlv_len, encode_tlv, rc) \
+do { \
+        buf_dst = (u8 *)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 = (u8 *)buf_src + rc; \
+        decoded_bytes += rc; \
+} while (0)
+
+#define TLV_LEN_SIZE sizeof(u16)
+#define TLV_TYPE_SIZE sizeof(u8)
+#define OPTIONAL_TLV_TYPE_START 0x10
+
+static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf,
+                      const void *in_c_struct, u32 out_buf_len,
+                      int enc_level);
+
+static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct,
+                      const void *in_buf, u32 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.
+ *
+ * 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.
+ *
+ * Return: struct info of the next element that can be encoded.
+ */
+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;
+        u8 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.
+ *
+ * Return: 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(u8) ?
+                                        sizeof(u8) : sizeof(u16));
+                        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(u8) : sizeof(u16);
+                        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.
+ *
+ * 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 u8 - u64 or similar. This
+ * function returns the number of bytes of encoded information.
+ *
+ * Return: The number of bytes of encoded information.
+ */
+static int qmi_encode_basic_elem(void *buf_dst, const void *buf_src,
+                                 u32 elem_len, u32 elem_size)
+{
+        u32 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.
+ *
+ * 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.
+ *
+ * Return: The number of bytes of encoded information on success or negative
+ * errno on error.
+ */
+static int qmi_encode_struct_elem(struct qmi_elem_info *ei_array,
+                                  void *buf_dst, const void *buf_src,
+                                  u32 elem_len, u32 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.
+ *
+ * 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.
+ *
+ * Return: The number of bytes of encoded information on success or negative
+ * errno on error.
+ */
+static int qmi_encode_string_elem(struct qmi_elem_info *ei_array,
+                                  void *buf_dst, const void *buf_src,
+                                  u32 out_buf_len, int enc_level)
+{
+        int rc;
+        int encoded_bytes = 0;
+        struct qmi_elem_info *temp_ei = ei_array;
+        u32 string_len = 0;
+        u32 string_len_sz = 0;
+
+        string_len = strlen(buf_src);
+        string_len_sz = temp_ei->elem_len <= U8_MAX ?
+                        sizeof(u8) : sizeof(u16);
+        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.
+ *
+ * Return: 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, u32 out_buf_len,
+                      int enc_level)
+{
+        struct qmi_elem_info *temp_ei = ei_array;
+        u8 opt_flag_value = 0;
+        u32 data_len_value = 0, data_len_sz;
+        u8 *buf_dst = (u8 *)out_buf;
+        u8 *tlv_pointer;
+        u32 tlv_len;
+        u8 tlv_type;
+        u32 encoded_bytes = 0;
+        const void *buf_src;
+        int encode_tlv = 0;
+        int rc;
+
+        if (!ei_array)
+                return 0;
+
+        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->array_type == NO_ARRAY) {
+                        data_len_value = 1;
+                } else if (temp_ei->array_type == 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(u8));
+                        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(u8) ?
+                                        sizeof(u8) : sizeof(u16);
+                        /* 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.
+ *
+ * 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 u8 - u64 or similar. This
+ * function returns the number of bytes of decoded information.
+ *
+ * Return: The total size of the decoded data elements, in bytes.
+ */
+static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src,
+                                 u32 elem_len, u32 elem_size)
+{
+        u32 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.
+ *
+ * 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.
+ *
+ * Return: The total size of the decoded data elements on success, negative
+ * errno on error.
+ */
+static int qmi_decode_struct_elem(struct qmi_elem_info *ei_array,
+                                  void *buf_dst, const void *buf_src,
+                                  u32 elem_len, u32 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.
+ *
+ * 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.
+ *
+ * Return: The total size of the decoded data elements on success, negative
+ * errno on error.
+ */
+static int qmi_decode_string_elem(struct qmi_elem_info *ei_array,
+                                  void *buf_dst, const void *buf_src,
+                                  u32 tlv_len, int dec_level)
+{
+        int rc;
+        int decoded_bytes = 0;
+        u32 string_len = 0;
+        u32 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(u8) : sizeof(u16);
+                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.
+ *
+ * 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.
+ *
+ * Return: Pointer to struct info, if found
+ */
+static struct qmi_elem_info *find_ei(struct qmi_elem_info *ei_array,
+                                     u32 type)
+{
+        struct qmi_elem_info *temp_ei = ei_array;
+
+        while (temp_ei->data_type != QMI_EOTI) {
+                if (temp_ei->tlv_type == (u8)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
+ *
+ * Return: 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, u32 in_buf_len,
+                      int dec_level)
+{
+        struct qmi_elem_info *temp_ei = ei_array;
+        u8 opt_flag_value = 1;
+        u32 data_len_value = 0, data_len_sz = 0;
+        u8 *buf_dst = out_c_struct;
+        const u8 *tlv_pointer;
+        u32 tlv_len = 0;
+        u32 tlv_type;
+        u32 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(u8));
+                        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(u8) ?
+                                        sizeof(u8) : sizeof(u16);
+                        rc = qmi_decode_basic_elem(&data_len_value, buf_src,
+                                                   1, data_len_sz);
+                        memcpy(buf_dst, &data_len_value, sizeof(u32));
+                        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->array_type == NO_ARRAY) {
+                        data_len_value = 1;
+                } else if (temp_ei->array_type == 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
+ *
+ * Return: 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;
+        ssize_t msglen = 0;
+        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);
+                }
+        }
+
+        msg = kzalloc(sizeof(*hdr) + *len, GFP_KERNEL);
+        if (!msg)
+                return ERR_PTR(-ENOMEM);
+
+        /* Encode message, if we have a message */
+        if (c_struct) {
+                msglen = qmi_encode(ei, msg + sizeof(*hdr), c_struct, *len, 1);
+                if (msglen < 0) {
+                        kfree(msg);
+                        return ERR_PTR(msglen);
+                }
+        }
+
+        hdr = msg;
+        hdr->type = type;
+        hdr->txn_id = txn_id;
+        hdr->msg_id = msg_id;
+        hdr->msg_len = msglen;
+
+        *len = sizeof(*hdr) + msglen;
+
+        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
+ *
+ * Return: 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(u16),
+                .array_type     = 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(u16),
+                .array_type     = 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,
+                .array_type     = 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..3523295f3022
--- /dev/null
+++ b/include/linux/soc/qcom/qmi.h
@@ -0,0 +1,106 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2012-2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017, Linaro Ltd.
+ */
+#ifndef __QMI_HELPERS_H__
+#define __QMI_HELPERS_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 {
+        u8 type;
+        u16 txn_id;
+        u16 msg_id;
+        u16 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.
+ * @array_type: 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;
+        u32 elem_len;
+        u32 elem_size;
+        enum qmi_array_type array_type;
+        u8 tlv_type;
+        u32 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 {
+        u16 result;
+        u16 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