1// SPDX-License-Identifier: GPL-2.0 2/* 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 0x10 59 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 encoded 69 * @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 flag 73 * corresponding to an optional element is not set, then encoding the 74 * optional element can be skipped. This function can be used to perform 75 * 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 message 99 * @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 the 142 * 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 type 153 * @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 of 159 * size "elem_size" bytes from the source buffer "buf_src" and stores the 160 * encoded information in the destination buffer "buf_dst". The elements are 161 * of primary data type which include u8 - u64 or similar. This 162 * 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 type 181 * @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 of 189 * size "ei_array->elem_size" bytes from the source buffer "buf_src" and 190 * stores the encoded information in the destination buffer "buf_dst". The 191 * elements are of struct data type which includes any C structure. This 192 * function returns the number of bytes of encoded information. 193 * 194 * Return: The number of bytes of encoded information on success or negative 195 * 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 type 222 * @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 in 230 * the destination buffer "buf_dst". This function returns the number of bytes 231 * of encoded information. 232 * 233 * Return: The number of bytes of encoded information on success or negative 234 * 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 Function 282 * @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 negative 290 * 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 else 337 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 else 359 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 type 426 * @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 stores 433 * the decoded elements in the destination buffer "buf_dst". The elements are 434 * of primary data type which include u8 - u64 or similar. This 435 * 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 type 454 * @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 to 459 * 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". The 465 * elements are of struct data type which includes any C structure. This 466 * function returns the number of bytes of decoded information. 467 * 468 * Return: The total size of the decoded data elements on success, negative 469 * 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 type 502 * @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 to 506 * 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 length 510 * "ei_array->elem_len" from the source buffer "buf_src" and puts it into 511 * the destination buffer "buf_dst". This function returns number of bytes 512 * decoded from the input buffer. 513 * 514 * Return: The total size of the decoded data elements on success, negative 515 * 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 Type 557 * @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 type 561 * information associated with it. While decoding the QMI message, 562 * this function is used to find the struct info regarding the element 563 * that corresponds to the type being decoded. 564 * 565 * Return: Pointer to struct info, if found 566 */ 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 Function 583 * @ei_array: Struct info array describing the structure to be decoded. 584 * @out_c_struct: Buffer to hold the decoded C struct 585 * @in_buf: Buffer containing the QMI message to be decoded 586 * @in_buf_len: Length of the QMI message to be decoded 587 * @dec_level: Decode level to indicate the depth of the nested structure, 588 * within the main structure, being decoded 589 * 590 * Return: The number of bytes of decoded information on success, negative 591 * 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 nested 630 * 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 message 706 * @type: Type of QMI message 707 * @msg_id: Message ID of the message 708 * @len: Passed as max length of the message, updated to actual size 709 * @txn_id: Transaction ID 710 * @ei: QMI message descriptor 711 * @c_struct: Reference to structure to encode 712 * 713 * Return: Buffer with encoded message, or negative ERR_PTR() on error 714 */ 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 structure 761 * @buf: Buffer with encoded message 762 * @len: Amount of data in @buf 763 * @ei: QMI message descriptor 764 * @c_struct: Reference to structure to decode into 765 * 766 * Return: The number of bytes of decoded information on success, negative 767 * 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