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>
This commit is contained in:
Bjorn Andersson
Andy Gross
parent
29ff62f7db
commit
9b8a11e826
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue