// SPDX-License-Identifier: GPL-2.0 /* * Copyright (c) 2012-2015, 2017, 2019 The Linux Foundation. All rights reserved. * Copyright (C) 2017 Linaro Ltd. */ #include #include #include #include #include #include #include #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 { \ u16 *var16; \ u32 *var32; \ u64 *var64; \ var16 = (u16 *) p_dst; \ var32 = (u32 *) p_dst; \ var64 = (u64 *) p_dst; \ memcpy(p_dst, p_src, size); \ if (size == 2) \ *var16 = cpu_to_le16(*var16); \ if (size == 4) \ *var32 = cpu_to_le32(*var32); \ if (size == 8) \ *var64 = cpu_to_le64(*var64); \ 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 { \ u16 *var16; \ u32 *var32; \ u64 *var64; \ var16 = (u16 *) p_dst; \ var32 = (u32 *) p_dst; \ var64 = (u64 *) p_dst; \ memcpy(p_dst, p_src, size); \ if (size == 2) \ *var16 = le16_to_cpu(*var16); \ if (size == 4) \ *var32 = le32_to_cpu(*var32); \ if (size == 8) \ *var64 = le64_to_cpu(*var64); \ 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 IS_ENABLED(CONFIG_CPU_BIG_ENDIAN) char *d; #endif 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, sizeof(u32)); #if IS_ENABLED(CONFIG_CPU_BIG_ENDIAN) d = (char *)&data_len_value; if (temp_ei->elem_size == 1) { d[0] = d[3]; d[1] = d[2] = d[3] = 0; } if (temp_ei->elem_size == 2) { d[0] = d[2]; d[1] = d[3]; d[2] = d[3] = 0; } #endif 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; #if IS_ENABLED(CONFIG_CPU_BIG_ENDIAN) data_len_value = (u32)*(u32 *)(buf_src); #endif 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; u16 string_len = 0; u16 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; } string_len = le16_to_cpu(string_len); 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; #if IS_ENABLED(CONFIG_CPU_BIG_ENDIAN) char *d; #endif 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_value = 0; 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); #if IS_ENABLED(CONFIG_CPU_BIG_ENDIAN) d = (char *)&data_len_value; if (temp_ei->elem_size == 1) { d[3] = d[0]; d[0] = d[1] = d[2] = 0; } if (temp_ei->elem_size == 2) { d[2] = d[0]; d[3] = d[1]; d[0] = d[1] = 0; } #endif 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 = cpu_to_le16(txn_id); hdr->msg_id = cpu_to_le16(msg_id); hdr->msg_len = cpu_to_le16(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");