drivers/soc/qcom/qmi_encdec.c

   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) \
  27 do { \
  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) \
  34 do { \
  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) \
  42 do { \
  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) \
  51 do { \
  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
  60 static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf,
  61                       const void *in_c_struct, u32 out_buf_len,
  62                       int enc_level);
  63
  64 static int qmi_decode(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  */
  79 static struct qmi_elem_info *skip_to_next_elem(struct qmi_elem_info *ei_array,
  80                                                int level)
  81 {
  82         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  */
 104 static int qmi_calc_min_msg_len(struct qmi_elem_info *ei_array,
 105                                 int level)
 106 {
 107         int min_msg_len = 0;
 108         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  */
 166 static 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  */
 197 static int qmi_encode_struct_elem(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         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  */
 236 static int qmi_encode_string_elem(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         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  */
 292 static int qmi_encode(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         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  */
 439 static 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 descibing 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 inforation 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  */
 471 static int qmi_decode_struct_elem(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         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 descibing 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 inforation 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  */
 517 static int qmi_decode_string_elem(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         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  */
 567 static struct qmi_elem_info *find_ei(struct qmi_elem_info *ei_array,
 568                                      u32 type)
 569 {
 570         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  */
 593 static int qmi_decode(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         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  */
 715 void *qmi_encode_message(int type, unsigned int msg_id, size_t *len,
 716                          unsigned int txn_id, 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 }
 757 EXPORT_SYMBOL(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  */
 769 int qmi_decode_message(const void *buf, size_t len,
 770                        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 }
 781 EXPORT_SYMBOL(qmi_decode_message);
 782
 783 /* Common header in all QMI responses */
 784 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 };
 813 EXPORT_SYMBOL(qmi_response_type_v01_ei);
 814
 815 MODULE_DESCRIPTION("QMI encoder/decoder helper");
 816 MODULE_LICENSE("GPL v2");