Return-Path: Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1753071AbdHDPAy (ORCPT ); Fri, 4 Aug 2017 11:00:54 -0400 Received: from mail-pf0-f176.google.com ([209.85.192.176]:35305 "EHLO mail-pf0-f176.google.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1752350AbdHDO7t (ORCPT ); Fri, 4 Aug 2017 10:59:49 -0400 From: Bjorn Andersson To: "David S. Miller" , Andy Gross , David Brown Cc: linux-arm-msm@vger.kernel.org, linux-soc@vger.kernel.org, netdev@vger.kernel.org, linux-kernel@vger.kernel.org Subject: [PATCH 4/6] soc: qcom: Introduce QMI encoder/decoder Date: Fri, 4 Aug 2017 07:59:36 -0700 Message-Id: <20170804145938.25427-5-bjorn.andersson@linaro.org> X-Mailer: git-send-email 2.12.0 In-Reply-To: <20170804145938.25427-1-bjorn.andersson@linaro.org> References: <20170804145938.25427-1-bjorn.andersson@linaro.org> Sender: linux-kernel-owner@vger.kernel.org List-ID: X-Mailing-List: linux-kernel@vger.kernel.org Content-Length: 31323 Lines: 991 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 --- 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 +#include +#include +#include +#include +#include +#include +#include + +#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 +#include + +/** + * 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