817 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.4 * Copyright (C) 2017 Linaro Ltd.5 */6#include <linux/slab.h>7#include <linux/uaccess.h>8#include <linux/module.h>9#include <linux/kernel.h>10#include <linux/errno.h>11#include <linux/string.h>12#include <linux/soc/qcom/qmi.h>13 14#define QMI_ENCDEC_ENCODE_TLV(type, length, p_dst) do { \15 *p_dst++ = type; \16 *p_dst++ = ((u8)((length) & 0xFF)); \17 *p_dst++ = ((u8)(((length) >> 8) & 0xFF)); \18} while (0)19 20#define QMI_ENCDEC_DECODE_TLV(p_type, p_length, p_src) do { \21 *p_type = (u8)*p_src++; \22 *p_length = (u8)*p_src++; \23 *p_length |= ((u8)*p_src) << 8; \24} while (0)25 26#define QMI_ENCDEC_ENCODE_N_BYTES(p_dst, p_src, size) \27do { \28 memcpy(p_dst, p_src, size); \29 p_dst = (u8 *)p_dst + size; \30 p_src = (u8 *)p_src + size; \31} while (0)32 33#define QMI_ENCDEC_DECODE_N_BYTES(p_dst, p_src, size) \34do { \35 memcpy(p_dst, p_src, size); \36 p_dst = (u8 *)p_dst + size; \37 p_src = (u8 *)p_src + size; \38} while (0)39 40#define UPDATE_ENCODE_VARIABLES(temp_si, buf_dst, \41 encoded_bytes, tlv_len, encode_tlv, rc) \42do { \43 buf_dst = (u8 *)buf_dst + rc; \44 encoded_bytes += rc; \45 tlv_len += rc; \46 temp_si = temp_si + 1; \47 encode_tlv = 1; \48} while (0)49 50#define UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc) \51do { \52 buf_src = (u8 *)buf_src + rc; \53 decoded_bytes += rc; \54} while (0)55 56#define TLV_LEN_SIZE sizeof(u16)57#define TLV_TYPE_SIZE sizeof(u8)58#define OPTIONAL_TLV_TYPE_START 0x1059 60static int qmi_encode(const struct qmi_elem_info *ei_array, void *out_buf,61 const void *in_c_struct, u32 out_buf_len,62 int enc_level);63 64static int qmi_decode(const struct qmi_elem_info *ei_array, void *out_c_struct,65 const void *in_buf, u32 in_buf_len, int dec_level);66 67/**68 * skip_to_next_elem() - Skip to next element in the structure to be encoded69 * @ei_array: Struct info describing the element to be skipped.70 * @level: Depth level of encoding/decoding to identify nested structures.71 *72 * This function is used while encoding optional elements. If the flag73 * corresponding to an optional element is not set, then encoding the74 * optional element can be skipped. This function can be used to perform75 * that operation.76 *77 * Return: struct info of the next element that can be encoded.78 */79static const struct qmi_elem_info *80skip_to_next_elem(const struct qmi_elem_info *ei_array, int level)81{82 const struct qmi_elem_info *temp_ei = ei_array;83 u8 tlv_type;84 85 if (level > 1) {86 temp_ei = temp_ei + 1;87 } else {88 do {89 tlv_type = temp_ei->tlv_type;90 temp_ei = temp_ei + 1;91 } while (tlv_type == temp_ei->tlv_type);92 }93 94 return temp_ei;95}96 97/**98 * qmi_calc_min_msg_len() - Calculate the minimum length of a QMI message99 * @ei_array: Struct info array describing the structure.100 * @level: Level to identify the depth of the nested structures.101 *102 * Return: Expected minimum length of the QMI message or 0 on error.103 */104static int qmi_calc_min_msg_len(const struct qmi_elem_info *ei_array,105 int level)106{107 int min_msg_len = 0;108 const struct qmi_elem_info *temp_ei = ei_array;109 110 if (!ei_array)111 return min_msg_len;112 113 while (temp_ei->data_type != QMI_EOTI) {114 /* Optional elements do not count in minimum length */115 if (temp_ei->data_type == QMI_OPT_FLAG) {116 temp_ei = skip_to_next_elem(temp_ei, level);117 continue;118 }119 120 if (temp_ei->data_type == QMI_DATA_LEN) {121 min_msg_len += (temp_ei->elem_size == sizeof(u8) ?122 sizeof(u8) : sizeof(u16));123 temp_ei++;124 continue;125 } else if (temp_ei->data_type == QMI_STRUCT) {126 min_msg_len += qmi_calc_min_msg_len(temp_ei->ei_array,127 (level + 1));128 temp_ei++;129 } else if (temp_ei->data_type == QMI_STRING) {130 if (level > 1)131 min_msg_len += temp_ei->elem_len <= U8_MAX ?132 sizeof(u8) : sizeof(u16);133 min_msg_len += temp_ei->elem_len * temp_ei->elem_size;134 temp_ei++;135 } else {136 min_msg_len += (temp_ei->elem_len * temp_ei->elem_size);137 temp_ei++;138 }139 140 /*141 * Type & Length info. not prepended for elements in the142 * nested structure.143 */144 if (level == 1)145 min_msg_len += (TLV_TYPE_SIZE + TLV_LEN_SIZE);146 }147 148 return min_msg_len;149}150 151/**152 * qmi_encode_basic_elem() - Encodes elements of basic/primary data type153 * @buf_dst: Buffer to store the encoded information.154 * @buf_src: Buffer containing the elements to be encoded.155 * @elem_len: Number of elements, in the buf_src, to be encoded.156 * @elem_size: Size of a single instance of the element to be encoded.157 *158 * This function encodes the "elem_len" number of data elements, each of159 * size "elem_size" bytes from the source buffer "buf_src" and stores the160 * encoded information in the destination buffer "buf_dst". The elements are161 * of primary data type which include u8 - u64 or similar. This162 * function returns the number of bytes of encoded information.163 *164 * Return: The number of bytes of encoded information.165 */166static int qmi_encode_basic_elem(void *buf_dst, const void *buf_src,167 u32 elem_len, u32 elem_size)168{169 u32 i, rc = 0;170 171 for (i = 0; i < elem_len; i++) {172 QMI_ENCDEC_ENCODE_N_BYTES(buf_dst, buf_src, elem_size);173 rc += elem_size;174 }175 176 return rc;177}178 179/**180 * qmi_encode_struct_elem() - Encodes elements of struct data type181 * @ei_array: Struct info array descibing the struct element.182 * @buf_dst: Buffer to store the encoded information.183 * @buf_src: Buffer containing the elements to be encoded.184 * @elem_len: Number of elements, in the buf_src, to be encoded.185 * @out_buf_len: Available space in the encode buffer.186 * @enc_level: Depth of the nested structure from the main structure.187 *188 * This function encodes the "elem_len" number of struct elements, each of189 * size "ei_array->elem_size" bytes from the source buffer "buf_src" and190 * stores the encoded information in the destination buffer "buf_dst". The191 * elements are of struct data type which includes any C structure. This192 * function returns the number of bytes of encoded information.193 *194 * Return: The number of bytes of encoded information on success or negative195 * errno on error.196 */197static int qmi_encode_struct_elem(const struct qmi_elem_info *ei_array,198 void *buf_dst, const void *buf_src,199 u32 elem_len, u32 out_buf_len,200 int enc_level)201{202 int i, rc, encoded_bytes = 0;203 const struct qmi_elem_info *temp_ei = ei_array;204 205 for (i = 0; i < elem_len; i++) {206 rc = qmi_encode(temp_ei->ei_array, buf_dst, buf_src,207 out_buf_len - encoded_bytes, enc_level);208 if (rc < 0) {209 pr_err("%s: STRUCT Encode failure\n", __func__);210 return rc;211 }212 buf_dst = buf_dst + rc;213 buf_src = buf_src + temp_ei->elem_size;214 encoded_bytes += rc;215 }216 217 return encoded_bytes;218}219 220/**221 * qmi_encode_string_elem() - Encodes elements of string data type222 * @ei_array: Struct info array descibing the string element.223 * @buf_dst: Buffer to store the encoded information.224 * @buf_src: Buffer containing the elements to be encoded.225 * @out_buf_len: Available space in the encode buffer.226 * @enc_level: Depth of the string element from the main structure.227 *228 * This function encodes a string element of maximum length "ei_array->elem_len"229 * bytes from the source buffer "buf_src" and stores the encoded information in230 * the destination buffer "buf_dst". This function returns the number of bytes231 * of encoded information.232 *233 * Return: The number of bytes of encoded information on success or negative234 * errno on error.235 */236static int qmi_encode_string_elem(const struct qmi_elem_info *ei_array,237 void *buf_dst, const void *buf_src,238 u32 out_buf_len, int enc_level)239{240 int rc;241 int encoded_bytes = 0;242 const struct qmi_elem_info *temp_ei = ei_array;243 u32 string_len = 0;244 u32 string_len_sz = 0;245 246 string_len = strlen(buf_src);247 string_len_sz = temp_ei->elem_len <= U8_MAX ?248 sizeof(u8) : sizeof(u16);249 if (string_len > temp_ei->elem_len) {250 pr_err("%s: String to be encoded is longer - %d > %d\n",251 __func__, string_len, temp_ei->elem_len);252 return -EINVAL;253 }254 255 if (enc_level == 1) {256 if (string_len + TLV_LEN_SIZE + TLV_TYPE_SIZE >257 out_buf_len) {258 pr_err("%s: Output len %d > Out Buf len %d\n",259 __func__, string_len, out_buf_len);260 return -ETOOSMALL;261 }262 } else {263 if (string_len + string_len_sz > out_buf_len) {264 pr_err("%s: Output len %d > Out Buf len %d\n",265 __func__, string_len, out_buf_len);266 return -ETOOSMALL;267 }268 rc = qmi_encode_basic_elem(buf_dst, &string_len,269 1, string_len_sz);270 encoded_bytes += rc;271 }272 273 rc = qmi_encode_basic_elem(buf_dst + encoded_bytes, buf_src,274 string_len, temp_ei->elem_size);275 encoded_bytes += rc;276 277 return encoded_bytes;278}279 280/**281 * qmi_encode() - Core Encode Function282 * @ei_array: Struct info array describing the structure to be encoded.283 * @out_buf: Buffer to hold the encoded QMI message.284 * @in_c_struct: Pointer to the C structure to be encoded.285 * @out_buf_len: Available space in the encode buffer.286 * @enc_level: Encode level to indicate the depth of the nested structure,287 * within the main structure, being encoded.288 *289 * Return: The number of bytes of encoded information on success or negative290 * errno on error.291 */292static int qmi_encode(const struct qmi_elem_info *ei_array, void *out_buf,293 const void *in_c_struct, u32 out_buf_len,294 int enc_level)295{296 const struct qmi_elem_info *temp_ei = ei_array;297 u8 opt_flag_value = 0;298 u32 data_len_value = 0, data_len_sz;299 u8 *buf_dst = (u8 *)out_buf;300 u8 *tlv_pointer;301 u32 tlv_len;302 u8 tlv_type;303 u32 encoded_bytes = 0;304 const void *buf_src;305 int encode_tlv = 0;306 int rc;307 308 if (!ei_array)309 return 0;310 311 tlv_pointer = buf_dst;312 tlv_len = 0;313 if (enc_level == 1)314 buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE);315 316 while (temp_ei->data_type != QMI_EOTI) {317 buf_src = in_c_struct + temp_ei->offset;318 tlv_type = temp_ei->tlv_type;319 320 if (temp_ei->array_type == NO_ARRAY) {321 data_len_value = 1;322 } else if (temp_ei->array_type == STATIC_ARRAY) {323 data_len_value = temp_ei->elem_len;324 } else if (data_len_value <= 0 ||325 temp_ei->elem_len < data_len_value) {326 pr_err("%s: Invalid data length\n", __func__);327 return -EINVAL;328 }329 330 switch (temp_ei->data_type) {331 case QMI_OPT_FLAG:332 rc = qmi_encode_basic_elem(&opt_flag_value, buf_src,333 1, sizeof(u8));334 if (opt_flag_value)335 temp_ei = temp_ei + 1;336 else337 temp_ei = skip_to_next_elem(temp_ei, enc_level);338 break;339 340 case QMI_DATA_LEN:341 memcpy(&data_len_value, buf_src, temp_ei->elem_size);342 data_len_sz = temp_ei->elem_size == sizeof(u8) ?343 sizeof(u8) : sizeof(u16);344 /* Check to avoid out of range buffer access */345 if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE +346 TLV_TYPE_SIZE) > out_buf_len) {347 pr_err("%s: Too Small Buffer @DATA_LEN\n",348 __func__);349 return -ETOOSMALL;350 }351 rc = qmi_encode_basic_elem(buf_dst, &data_len_value,352 1, data_len_sz);353 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,354 encoded_bytes, tlv_len,355 encode_tlv, rc);356 if (!data_len_value)357 temp_ei = skip_to_next_elem(temp_ei, enc_level);358 else359 encode_tlv = 0;360 break;361 362 case QMI_UNSIGNED_1_BYTE:363 case QMI_UNSIGNED_2_BYTE:364 case QMI_UNSIGNED_4_BYTE:365 case QMI_UNSIGNED_8_BYTE:366 case QMI_SIGNED_2_BYTE_ENUM:367 case QMI_SIGNED_4_BYTE_ENUM:368 /* Check to avoid out of range buffer access */369 if (((data_len_value * temp_ei->elem_size) +370 encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) >371 out_buf_len) {372 pr_err("%s: Too Small Buffer @data_type:%d\n",373 __func__, temp_ei->data_type);374 return -ETOOSMALL;375 }376 rc = qmi_encode_basic_elem(buf_dst, buf_src,377 data_len_value,378 temp_ei->elem_size);379 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,380 encoded_bytes, tlv_len,381 encode_tlv, rc);382 break;383 384 case QMI_STRUCT:385 rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src,386 data_len_value,387 out_buf_len - encoded_bytes,388 enc_level + 1);389 if (rc < 0)390 return rc;391 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,392 encoded_bytes, tlv_len,393 encode_tlv, rc);394 break;395 396 case QMI_STRING:397 rc = qmi_encode_string_elem(temp_ei, buf_dst, buf_src,398 out_buf_len - encoded_bytes,399 enc_level);400 if (rc < 0)401 return rc;402 UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,403 encoded_bytes, tlv_len,404 encode_tlv, rc);405 break;406 default:407 pr_err("%s: Unrecognized data type\n", __func__);408 return -EINVAL;409 }410 411 if (encode_tlv && enc_level == 1) {412 QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer);413 encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);414 tlv_pointer = buf_dst;415 tlv_len = 0;416 buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE;417 encode_tlv = 0;418 }419 }420 421 return encoded_bytes;422}423 424/**425 * qmi_decode_basic_elem() - Decodes elements of basic/primary data type426 * @buf_dst: Buffer to store the decoded element.427 * @buf_src: Buffer containing the elements in QMI wire format.428 * @elem_len: Number of elements to be decoded.429 * @elem_size: Size of a single instance of the element to be decoded.430 *431 * This function decodes the "elem_len" number of elements in QMI wire format,432 * each of size "elem_size" bytes from the source buffer "buf_src" and stores433 * the decoded elements in the destination buffer "buf_dst". The elements are434 * of primary data type which include u8 - u64 or similar. This435 * function returns the number of bytes of decoded information.436 *437 * Return: The total size of the decoded data elements, in bytes.438 */439static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src,440 u32 elem_len, u32 elem_size)441{442 u32 i, rc = 0;443 444 for (i = 0; i < elem_len; i++) {445 QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size);446 rc += elem_size;447 }448 449 return rc;450}451 452/**453 * qmi_decode_struct_elem() - Decodes elements of struct data type454 * @ei_array: Struct info array describing the struct element.455 * @buf_dst: Buffer to store the decoded element.456 * @buf_src: Buffer containing the elements in QMI wire format.457 * @elem_len: Number of elements to be decoded.458 * @tlv_len: Total size of the encoded information corresponding to459 * this struct element.460 * @dec_level: Depth of the nested structure from the main structure.461 *462 * This function decodes the "elem_len" number of elements in QMI wire format,463 * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src"464 * and stores the decoded elements in the destination buffer "buf_dst". The465 * elements are of struct data type which includes any C structure. This466 * function returns the number of bytes of decoded information.467 *468 * Return: The total size of the decoded data elements on success, negative469 * errno on error.470 */471static int qmi_decode_struct_elem(const struct qmi_elem_info *ei_array,472 void *buf_dst, const void *buf_src,473 u32 elem_len, u32 tlv_len,474 int dec_level)475{476 int i, rc, decoded_bytes = 0;477 const struct qmi_elem_info *temp_ei = ei_array;478 479 for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) {480 rc = qmi_decode(temp_ei->ei_array, buf_dst, buf_src,481 tlv_len - decoded_bytes, dec_level);482 if (rc < 0)483 return rc;484 buf_src = buf_src + rc;485 buf_dst = buf_dst + temp_ei->elem_size;486 decoded_bytes += rc;487 }488 489 if ((dec_level <= 2 && decoded_bytes != tlv_len) ||490 (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) {491 pr_err("%s: Fault in decoding: dl(%d), db(%d), tl(%d), i(%d), el(%d)\n",492 __func__, dec_level, decoded_bytes, tlv_len,493 i, elem_len);494 return -EFAULT;495 }496 497 return decoded_bytes;498}499 500/**501 * qmi_decode_string_elem() - Decodes elements of string data type502 * @ei_array: Struct info array describing the string element.503 * @buf_dst: Buffer to store the decoded element.504 * @buf_src: Buffer containing the elements in QMI wire format.505 * @tlv_len: Total size of the encoded information corresponding to506 * this string element.507 * @dec_level: Depth of the string element from the main structure.508 *509 * This function decodes the string element of maximum length510 * "ei_array->elem_len" from the source buffer "buf_src" and puts it into511 * the destination buffer "buf_dst". This function returns number of bytes512 * decoded from the input buffer.513 *514 * Return: The total size of the decoded data elements on success, negative515 * errno on error.516 */517static int qmi_decode_string_elem(const struct qmi_elem_info *ei_array,518 void *buf_dst, const void *buf_src,519 u32 tlv_len, int dec_level)520{521 int rc;522 int decoded_bytes = 0;523 u32 string_len = 0;524 u32 string_len_sz = 0;525 const struct qmi_elem_info *temp_ei = ei_array;526 527 if (dec_level == 1) {528 string_len = tlv_len;529 } else {530 string_len_sz = temp_ei->elem_len <= U8_MAX ?531 sizeof(u8) : sizeof(u16);532 rc = qmi_decode_basic_elem(&string_len, buf_src,533 1, string_len_sz);534 decoded_bytes += rc;535 }536 537 if (string_len >= temp_ei->elem_len) {538 pr_err("%s: String len %d >= Max Len %d\n",539 __func__, string_len, temp_ei->elem_len);540 return -ETOOSMALL;541 } else if (string_len > tlv_len) {542 pr_err("%s: String len %d > Input Buffer Len %d\n",543 __func__, string_len, tlv_len);544 return -EFAULT;545 }546 547 rc = qmi_decode_basic_elem(buf_dst, buf_src + decoded_bytes,548 string_len, temp_ei->elem_size);549 *((char *)buf_dst + string_len) = '\0';550 decoded_bytes += rc;551 552 return decoded_bytes;553}554 555/**556 * find_ei() - Find element info corresponding to TLV Type557 * @ei_array: Struct info array of the message being decoded.558 * @type: TLV Type of the element being searched.559 *560 * Every element that got encoded in the QMI message will have a type561 * information associated with it. While decoding the QMI message,562 * this function is used to find the struct info regarding the element563 * that corresponds to the type being decoded.564 *565 * Return: Pointer to struct info, if found566 */567static const struct qmi_elem_info *find_ei(const struct qmi_elem_info *ei_array,568 u32 type)569{570 const struct qmi_elem_info *temp_ei = ei_array;571 572 while (temp_ei->data_type != QMI_EOTI) {573 if (temp_ei->tlv_type == (u8)type)574 return temp_ei;575 temp_ei = temp_ei + 1;576 }577 578 return NULL;579}580 581/**582 * qmi_decode() - Core Decode Function583 * @ei_array: Struct info array describing the structure to be decoded.584 * @out_c_struct: Buffer to hold the decoded C struct585 * @in_buf: Buffer containing the QMI message to be decoded586 * @in_buf_len: Length of the QMI message to be decoded587 * @dec_level: Decode level to indicate the depth of the nested structure,588 * within the main structure, being decoded589 *590 * Return: The number of bytes of decoded information on success, negative591 * errno on error.592 */593static int qmi_decode(const struct qmi_elem_info *ei_array, void *out_c_struct,594 const void *in_buf, u32 in_buf_len,595 int dec_level)596{597 const struct qmi_elem_info *temp_ei = ei_array;598 u8 opt_flag_value = 1;599 u32 data_len_value = 0, data_len_sz = 0;600 u8 *buf_dst = out_c_struct;601 const u8 *tlv_pointer;602 u32 tlv_len = 0;603 u32 tlv_type;604 u32 decoded_bytes = 0;605 const void *buf_src = in_buf;606 int rc;607 608 while (decoded_bytes < in_buf_len) {609 if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI)610 return decoded_bytes;611 612 if (dec_level == 1) {613 tlv_pointer = buf_src;614 QMI_ENCDEC_DECODE_TLV(&tlv_type,615 &tlv_len, tlv_pointer);616 buf_src += (TLV_TYPE_SIZE + TLV_LEN_SIZE);617 decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);618 temp_ei = find_ei(ei_array, tlv_type);619 if (!temp_ei && tlv_type < OPTIONAL_TLV_TYPE_START) {620 pr_err("%s: Inval element info\n", __func__);621 return -EINVAL;622 } else if (!temp_ei) {623 UPDATE_DECODE_VARIABLES(buf_src,624 decoded_bytes, tlv_len);625 continue;626 }627 } else {628 /*629 * No length information for elements in nested630 * structures. So use remaining decodable buffer space.631 */632 tlv_len = in_buf_len - decoded_bytes;633 }634 635 buf_dst = out_c_struct + temp_ei->offset;636 if (temp_ei->data_type == QMI_OPT_FLAG) {637 memcpy(buf_dst, &opt_flag_value, sizeof(u8));638 temp_ei = temp_ei + 1;639 buf_dst = out_c_struct + temp_ei->offset;640 }641 642 if (temp_ei->data_type == QMI_DATA_LEN) {643 data_len_sz = temp_ei->elem_size == sizeof(u8) ?644 sizeof(u8) : sizeof(u16);645 rc = qmi_decode_basic_elem(&data_len_value, buf_src,646 1, data_len_sz);647 memcpy(buf_dst, &data_len_value, sizeof(u32));648 temp_ei = temp_ei + 1;649 buf_dst = out_c_struct + temp_ei->offset;650 tlv_len -= data_len_sz;651 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);652 }653 654 if (temp_ei->array_type == NO_ARRAY) {655 data_len_value = 1;656 } else if (temp_ei->array_type == STATIC_ARRAY) {657 data_len_value = temp_ei->elem_len;658 } else if (data_len_value > temp_ei->elem_len) {659 pr_err("%s: Data len %d > max spec %d\n",660 __func__, data_len_value, temp_ei->elem_len);661 return -ETOOSMALL;662 }663 664 switch (temp_ei->data_type) {665 case QMI_UNSIGNED_1_BYTE:666 case QMI_UNSIGNED_2_BYTE:667 case QMI_UNSIGNED_4_BYTE:668 case QMI_UNSIGNED_8_BYTE:669 case QMI_SIGNED_2_BYTE_ENUM:670 case QMI_SIGNED_4_BYTE_ENUM:671 rc = qmi_decode_basic_elem(buf_dst, buf_src,672 data_len_value,673 temp_ei->elem_size);674 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);675 break;676 677 case QMI_STRUCT:678 rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src,679 data_len_value, tlv_len,680 dec_level + 1);681 if (rc < 0)682 return rc;683 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);684 break;685 686 case QMI_STRING:687 rc = qmi_decode_string_elem(temp_ei, buf_dst, buf_src,688 tlv_len, dec_level);689 if (rc < 0)690 return rc;691 UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);692 break;693 694 default:695 pr_err("%s: Unrecognized data type\n", __func__);696 return -EINVAL;697 }698 temp_ei = temp_ei + 1;699 }700 701 return decoded_bytes;702}703 704/**705 * qmi_encode_message() - Encode C structure as QMI encoded message706 * @type: Type of QMI message707 * @msg_id: Message ID of the message708 * @len: Passed as max length of the message, updated to actual size709 * @txn_id: Transaction ID710 * @ei: QMI message descriptor711 * @c_struct: Reference to structure to encode712 *713 * Return: Buffer with encoded message, or negative ERR_PTR() on error714 */715void *qmi_encode_message(int type, unsigned int msg_id, size_t *len,716 unsigned int txn_id, const struct qmi_elem_info *ei,717 const void *c_struct)718{719 struct qmi_header *hdr;720 ssize_t msglen = 0;721 void *msg;722 int ret;723 724 /* Check the possibility of a zero length QMI message */725 if (!c_struct) {726 ret = qmi_calc_min_msg_len(ei, 1);727 if (ret) {728 pr_err("%s: Calc. len %d != 0, but NULL c_struct\n",729 __func__, ret);730 return ERR_PTR(-EINVAL);731 }732 }733 734 msg = kzalloc(sizeof(*hdr) + *len, GFP_KERNEL);735 if (!msg)736 return ERR_PTR(-ENOMEM);737 738 /* Encode message, if we have a message */739 if (c_struct) {740 msglen = qmi_encode(ei, msg + sizeof(*hdr), c_struct, *len, 1);741 if (msglen < 0) {742 kfree(msg);743 return ERR_PTR(msglen);744 }745 }746 747 hdr = msg;748 hdr->type = type;749 hdr->txn_id = txn_id;750 hdr->msg_id = msg_id;751 hdr->msg_len = msglen;752 753 *len = sizeof(*hdr) + msglen;754 755 return msg;756}757EXPORT_SYMBOL_GPL(qmi_encode_message);758 759/**760 * qmi_decode_message() - Decode QMI encoded message to C structure761 * @buf: Buffer with encoded message762 * @len: Amount of data in @buf763 * @ei: QMI message descriptor764 * @c_struct: Reference to structure to decode into765 *766 * Return: The number of bytes of decoded information on success, negative767 * errno on error.768 */769int qmi_decode_message(const void *buf, size_t len,770 const struct qmi_elem_info *ei, void *c_struct)771{772 if (!ei)773 return -EINVAL;774 775 if (!c_struct || !buf || !len)776 return -EINVAL;777 778 return qmi_decode(ei, c_struct, buf + sizeof(struct qmi_header),779 len - sizeof(struct qmi_header), 1);780}781EXPORT_SYMBOL_GPL(qmi_decode_message);782 783/* Common header in all QMI responses */784const struct qmi_elem_info qmi_response_type_v01_ei[] = {785 {786 .data_type = QMI_SIGNED_2_BYTE_ENUM,787 .elem_len = 1,788 .elem_size = sizeof(u16),789 .array_type = NO_ARRAY,790 .tlv_type = QMI_COMMON_TLV_TYPE,791 .offset = offsetof(struct qmi_response_type_v01, result),792 .ei_array = NULL,793 },794 {795 .data_type = QMI_SIGNED_2_BYTE_ENUM,796 .elem_len = 1,797 .elem_size = sizeof(u16),798 .array_type = NO_ARRAY,799 .tlv_type = QMI_COMMON_TLV_TYPE,800 .offset = offsetof(struct qmi_response_type_v01, error),801 .ei_array = NULL,802 },803 {804 .data_type = QMI_EOTI,805 .elem_len = 0,806 .elem_size = 0,807 .array_type = NO_ARRAY,808 .tlv_type = QMI_COMMON_TLV_TYPE,809 .offset = 0,810 .ei_array = NULL,811 },812};813EXPORT_SYMBOL_GPL(qmi_response_type_v01_ei);814 815MODULE_DESCRIPTION("QMI encoder/decoder helper");816MODULE_LICENSE("GPL v2");817