forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
816 lines
24 KiB
816 lines
24 KiB
// 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");
|
|
|