qmi_encdec.c 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648
  1. /* Copyright (c) 2012-2013, The Linux Foundation. All rights reserved.
  2. *
  3. * This program is free software; you can redistribute it and/or modify
  4. * it under the terms of the GNU General Public License version 2 and
  5. * only version 2 as published by the Free Software Foundation.
  6. *
  7. * This program is distributed in the hope that it will be useful,
  8. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  9. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  10. * GNU General Public License for more details.
  11. *
  12. */
  13. #include <linux/slab.h>
  14. #include <linux/uaccess.h>
  15. #include <linux/module.h>
  16. #include <linux/kernel.h>
  17. #include <linux/errno.h>
  18. #include <linux/io.h>
  19. #include <linux/string.h>
  20. #include <linux/qmi_encdec.h>
  21. #include "qmi_encdec_priv.h"
  22. #define TLV_LEN_SIZE sizeof(uint16_t)
  23. #define TLV_TYPE_SIZE sizeof(uint8_t)
  24. #ifdef CONFIG_QMI_ENCDEC_DEBUG
  25. #define qmi_encdec_dump(prefix_str, buf, buf_len) do { \
  26. const u8 *ptr = buf; \
  27. int i, linelen, remaining = buf_len; \
  28. int rowsize = 16, groupsize = 1; \
  29. unsigned char linebuf[256]; \
  30. for (i = 0; i < buf_len; i += rowsize) { \
  31. linelen = min(remaining, rowsize); \
  32. remaining -= linelen; \
  33. hex_dump_to_buffer(ptr + i, linelen, rowsize, groupsize, \
  34. linebuf, sizeof(linebuf), false); \
  35. pr_debug("%s: %s\n", prefix_str, linebuf); \
  36. } \
  37. } while (0)
  38. #define QMI_ENCODE_LOG_MSG(buf, buf_len) do { \
  39. qmi_encdec_dump("QMI_ENCODE_MSG", buf, buf_len); \
  40. } while (0)
  41. #define QMI_DECODE_LOG_MSG(buf, buf_len) do { \
  42. qmi_encdec_dump("QMI_DECODE_MSG", buf, buf_len); \
  43. } while (0)
  44. #define QMI_ENCODE_LOG_ELEM(level, elem_len, elem_size, buf) do { \
  45. pr_debug("QMI_ENCODE_ELEM lvl: %d, len: %d, size: %d\n", \
  46. level, elem_len, elem_size); \
  47. qmi_encdec_dump("QMI_ENCODE_ELEM", buf, (elem_len * elem_size)); \
  48. } while (0)
  49. #define QMI_DECODE_LOG_ELEM(level, elem_len, elem_size, buf) do { \
  50. pr_debug("QMI_DECODE_ELEM lvl: %d, len: %d, size: %d\n", \
  51. level, elem_len, elem_size); \
  52. qmi_encdec_dump("QMI_DECODE_ELEM", buf, (elem_len * elem_size)); \
  53. } while (0)
  54. #define QMI_ENCODE_LOG_TLV(tlv_type, tlv_len) do { \
  55. pr_debug("QMI_ENCODE_TLV type: %d, len: %d\n", tlv_type, tlv_len); \
  56. } while (0)
  57. #define QMI_DECODE_LOG_TLV(tlv_type, tlv_len) do { \
  58. pr_debug("QMI_DECODE_TLV type: %d, len: %d\n", tlv_type, tlv_len); \
  59. } while (0)
  60. #else
  61. #define QMI_ENCODE_LOG_MSG(buf, buf_len) { }
  62. #define QMI_DECODE_LOG_MSG(buf, buf_len) { }
  63. #define QMI_ENCODE_LOG_ELEM(level, elem_len, elem_size, buf) { }
  64. #define QMI_DECODE_LOG_ELEM(level, elem_len, elem_size, buf) { }
  65. #define QMI_ENCODE_LOG_TLV(tlv_type, tlv_len) { }
  66. #define QMI_DECODE_LOG_TLV(tlv_type, tlv_len) { }
  67. #endif
  68. static int _qmi_kernel_encode(struct elem_info *ei_array,
  69. void *out_buf, void *in_c_struct,
  70. uint32_t out_buf_len, int enc_level);
  71. static int _qmi_kernel_decode(struct elem_info *ei_array,
  72. void *out_c_struct,
  73. void *in_buf, uint32_t in_buf_len,
  74. int dec_level);
  75. /**
  76. * qmi_calc_max_msg_len() - Calculate the maximum length of a QMI message
  77. * @ei_array: Struct info array describing the structure.
  78. * @level: Level to identify the depth of the nested structures.
  79. *
  80. * @return: expected maximum length of the QMI message or 0 on failure.
  81. */
  82. static int qmi_calc_max_msg_len(struct elem_info *ei_array,
  83. int level)
  84. {
  85. int max_msg_len = 0;
  86. struct elem_info *temp_ei;
  87. if (!ei_array)
  88. return max_msg_len;
  89. for (temp_ei = ei_array; temp_ei->data_type != QMI_EOTI; temp_ei++) {
  90. /* Flag to identify the optional element is not encoded */
  91. if (temp_ei->data_type == QMI_OPT_FLAG)
  92. continue;
  93. if (temp_ei->data_type == QMI_DATA_LEN) {
  94. max_msg_len += (temp_ei->elem_size == sizeof(uint8_t) ?
  95. sizeof(uint8_t) : sizeof(uint16_t));
  96. continue;
  97. } else if (temp_ei->data_type == QMI_STRUCT) {
  98. max_msg_len += qmi_calc_max_msg_len(temp_ei->ei_array,
  99. (level + 1));
  100. } else {
  101. max_msg_len += (temp_ei->elem_len * temp_ei->elem_size);
  102. }
  103. /*
  104. * Type & Length info. not prepended for elements in the
  105. * nested structure.
  106. */
  107. if (level == 1)
  108. max_msg_len += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
  109. }
  110. return max_msg_len;
  111. }
  112. /**
  113. * qmi_verify_max_msg_len() - Verify the maximum length of a QMI message
  114. * @desc: Pointer to structure descriptor.
  115. *
  116. * @return: true if the maximum message length embedded in structure
  117. * descriptor matches the calculated value, else false.
  118. */
  119. bool qmi_verify_max_msg_len(struct msg_desc *desc)
  120. {
  121. int calc_max_msg_len;
  122. if (!desc)
  123. return false;
  124. calc_max_msg_len = qmi_calc_max_msg_len(desc->ei_array, 1);
  125. if (calc_max_msg_len != desc->max_msg_len) {
  126. pr_err("%s: Calc. len %d != Passed len %d\n",
  127. __func__, calc_max_msg_len, desc->max_msg_len);
  128. return false;
  129. }
  130. return true;
  131. }
  132. /**
  133. * qmi_kernel_encode() - Encode to QMI message wire format
  134. * @desc: Pointer to structure descriptor.
  135. * @out_buf: Buffer to hold the encoded QMI message.
  136. * @out_buf_len: Length of the out buffer.
  137. * @in_c_struct: C Structure to be encoded.
  138. *
  139. * @return: size of encoded message on success, < 0 for error.
  140. */
  141. int qmi_kernel_encode(struct msg_desc *desc,
  142. void *out_buf, uint32_t out_buf_len,
  143. void *in_c_struct)
  144. {
  145. int enc_level = 1;
  146. int ret, calc_max_msg_len;
  147. if (!desc || !desc->ei_array)
  148. return -EINVAL;
  149. if (!out_buf || !in_c_struct)
  150. return -EINVAL;
  151. if (desc->max_msg_len < out_buf_len)
  152. return -ETOOSMALL;
  153. ret = _qmi_kernel_encode(desc->ei_array, out_buf,
  154. in_c_struct, out_buf_len, enc_level);
  155. if (ret == -ETOOSMALL) {
  156. calc_max_msg_len = qmi_calc_max_msg_len(desc->ei_array, 1);
  157. pr_err("%s: Calc. len %d != Out buf len %d\n",
  158. __func__, calc_max_msg_len, out_buf_len);
  159. }
  160. return ret;
  161. }
  162. EXPORT_SYMBOL(qmi_kernel_encode);
  163. /**
  164. * qmi_encode_basic_elem() - Encodes elements of basic/primary data type
  165. * @buf_dst: Buffer to store the encoded information.
  166. * @buf_src: Buffer containing the elements to be encoded.
  167. * @elem_len: Number of elements, in the buf_src, to be encoded.
  168. * @elem_size: Size of a single instance of the element to be encoded.
  169. *
  170. * @return: number of bytes of encoded information.
  171. *
  172. * This function encodes the "elem_len" number of data elements, each of
  173. * size "elem_size" bytes from the source buffer "buf_src" and stores the
  174. * encoded information in the destination buffer "buf_dst". The elements are
  175. * of primary data type which include uint8_t - uint64_t or similar. This
  176. * function returns the number of bytes of encoded information.
  177. */
  178. static int qmi_encode_basic_elem(void *buf_dst, void *buf_src,
  179. uint32_t elem_len, uint32_t elem_size)
  180. {
  181. uint32_t i, rc = 0;
  182. for (i = 0; i < elem_len; i++) {
  183. QMI_ENCDEC_ENCODE_N_BYTES(buf_dst, buf_src, elem_size);
  184. rc += elem_size;
  185. }
  186. return rc;
  187. }
  188. /**
  189. * qmi_encode_struct_elem() - Encodes elements of struct data type
  190. * @ei_array: Struct info array descibing the struct element.
  191. * @buf_dst: Buffer to store the encoded information.
  192. * @buf_src: Buffer containing the elements to be encoded.
  193. * @elem_len: Number of elements, in the buf_src, to be encoded.
  194. * @out_buf_len: Available space in the encode buffer.
  195. * @enc_level: Depth of the nested structure from the main structure.
  196. *
  197. * @return: Mumber of bytes of encoded information, on success.
  198. * < 0 on error.
  199. *
  200. * This function encodes the "elem_len" number of struct elements, each of
  201. * size "ei_array->elem_size" bytes from the source buffer "buf_src" and
  202. * stores the encoded information in the destination buffer "buf_dst". The
  203. * elements are of struct data type which includes any C structure. This
  204. * function returns the number of bytes of encoded information.
  205. */
  206. static int qmi_encode_struct_elem(struct elem_info *ei_array,
  207. void *buf_dst, void *buf_src,
  208. uint32_t elem_len, uint32_t out_buf_len,
  209. int enc_level)
  210. {
  211. int i, rc, encoded_bytes = 0;
  212. struct elem_info *temp_ei = ei_array;
  213. for (i = 0; i < elem_len; i++) {
  214. rc = _qmi_kernel_encode(temp_ei->ei_array, buf_dst, buf_src,
  215. (out_buf_len - encoded_bytes),
  216. enc_level);
  217. if (rc < 0) {
  218. pr_err("%s: STRUCT Encode failure\n", __func__);
  219. return rc;
  220. }
  221. buf_dst = buf_dst + rc;
  222. buf_src = buf_src + temp_ei->elem_size;
  223. encoded_bytes += rc;
  224. }
  225. return encoded_bytes;
  226. }
  227. /**
  228. * skip_to_next_elem() - Skip to next element in the structure to be encoded
  229. * @ei_array: Struct info describing the element to be skipped.
  230. *
  231. * @return: Struct info of the next element that can be encoded.
  232. *
  233. * This function is used while encoding optional elements. If the flag
  234. * corresponding to an optional element is not set, then encoding the
  235. * optional element can be skipped. This function can be used to perform
  236. * that operation.
  237. */
  238. static struct elem_info *skip_to_next_elem(struct elem_info *ei_array)
  239. {
  240. struct elem_info *temp_ei = ei_array;
  241. uint8_t tlv_type;
  242. do {
  243. tlv_type = temp_ei->tlv_type;
  244. temp_ei = temp_ei + 1;
  245. } while (tlv_type == temp_ei->tlv_type);
  246. return temp_ei;
  247. }
  248. /**
  249. * _qmi_kernel_encode() - Core Encode Function
  250. * @ei_array: Struct info array describing the structure to be encoded.
  251. * @out_buf: Buffer to hold the encoded QMI message.
  252. * @in_c_struct: Pointer to the C structure to be encoded.
  253. * @out_buf_len: Available space in the encode buffer.
  254. * @enc_level: Encode level to indicate the depth of the nested structure,
  255. * within the main structure, being encoded.
  256. *
  257. * @return: Number of bytes of encoded information, on success.
  258. * < 0 on error.
  259. */
  260. static int _qmi_kernel_encode(struct elem_info *ei_array,
  261. void *out_buf, void *in_c_struct,
  262. uint32_t out_buf_len, int enc_level)
  263. {
  264. struct elem_info *temp_ei = ei_array;
  265. uint8_t opt_flag_value = 0;
  266. uint32_t data_len_value = 0, data_len_sz;
  267. uint8_t *buf_dst = (uint8_t *)out_buf;
  268. uint8_t *tlv_pointer;
  269. uint32_t tlv_len;
  270. uint8_t tlv_type;
  271. uint32_t encoded_bytes = 0;
  272. void *buf_src;
  273. int encode_tlv = 0;
  274. int rc;
  275. tlv_pointer = buf_dst;
  276. tlv_len = 0;
  277. buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE);
  278. while (temp_ei->data_type != QMI_EOTI) {
  279. buf_src = in_c_struct + temp_ei->offset;
  280. tlv_type = temp_ei->tlv_type;
  281. if (temp_ei->is_array == NO_ARRAY) {
  282. data_len_value = 1;
  283. } else if (temp_ei->is_array == STATIC_ARRAY) {
  284. data_len_value = temp_ei->elem_len;
  285. } else if (data_len_value <= 0 ||
  286. temp_ei->elem_len < data_len_value) {
  287. pr_err("%s: Invalid data length\n", __func__);
  288. return -EINVAL;
  289. }
  290. switch (temp_ei->data_type) {
  291. case QMI_OPT_FLAG:
  292. rc = qmi_encode_basic_elem(&opt_flag_value, buf_src,
  293. 1, sizeof(uint8_t));
  294. if (opt_flag_value)
  295. temp_ei = temp_ei + 1;
  296. else
  297. temp_ei = skip_to_next_elem(temp_ei);
  298. break;
  299. case QMI_DATA_LEN:
  300. memcpy(&data_len_value, buf_src, temp_ei->elem_size);
  301. data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ?
  302. sizeof(uint8_t) : sizeof(uint16_t);
  303. /* Check to avoid out of range buffer access */
  304. if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE +
  305. TLV_TYPE_SIZE) > out_buf_len) {
  306. pr_err("%s: Too Small Buffer @DATA_LEN\n",
  307. __func__);
  308. return -ETOOSMALL;
  309. }
  310. rc = qmi_encode_basic_elem(buf_dst, &data_len_value,
  311. 1, data_len_sz);
  312. if (data_len_value) {
  313. UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
  314. encoded_bytes, tlv_len, encode_tlv, rc);
  315. encode_tlv = 0;
  316. } else {
  317. temp_ei = skip_to_next_elem(temp_ei);
  318. }
  319. break;
  320. case QMI_UNSIGNED_1_BYTE:
  321. case QMI_UNSIGNED_2_BYTE:
  322. case QMI_UNSIGNED_4_BYTE:
  323. case QMI_UNSIGNED_8_BYTE:
  324. case QMI_SIGNED_2_BYTE_ENUM:
  325. case QMI_SIGNED_4_BYTE_ENUM:
  326. /* Check to avoid out of range buffer access */
  327. if (((data_len_value * temp_ei->elem_size) +
  328. encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) >
  329. out_buf_len) {
  330. pr_err("%s: Too Small Buffer @data_type:%d\n",
  331. __func__, temp_ei->data_type);
  332. return -ETOOSMALL;
  333. }
  334. rc = qmi_encode_basic_elem(buf_dst, buf_src,
  335. data_len_value, temp_ei->elem_size);
  336. QMI_ENCODE_LOG_ELEM(enc_level, data_len_value,
  337. temp_ei->elem_size, buf_src);
  338. UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
  339. encoded_bytes, tlv_len, encode_tlv, rc);
  340. break;
  341. case QMI_STRUCT:
  342. rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src,
  343. data_len_value, (out_buf_len - encoded_bytes),
  344. (enc_level + 1));
  345. if (rc < 0)
  346. return rc;
  347. UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
  348. encoded_bytes, tlv_len, encode_tlv, rc);
  349. break;
  350. default:
  351. pr_err("%s: Unrecognized data type\n", __func__);
  352. return -EINVAL;
  353. }
  354. if (encode_tlv && enc_level == 1) {
  355. QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer);
  356. QMI_ENCODE_LOG_TLV(tlv_type, tlv_len);
  357. encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
  358. tlv_pointer = buf_dst;
  359. tlv_len = 0;
  360. buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE;
  361. encode_tlv = 0;
  362. }
  363. }
  364. QMI_ENCODE_LOG_MSG(out_buf, encoded_bytes);
  365. return encoded_bytes;
  366. }
  367. /**
  368. * qmi_kernel_decode() - Decode to C Structure format
  369. * @desc: Pointer to structure descriptor.
  370. * @out_c_struct: Buffer to hold the decoded C structure.
  371. * @in_buf: Buffer containg the QMI message to be decoded.
  372. * @in_buf_len: Length of the incoming QMI message.
  373. *
  374. * @return: 0 on success, < 0 on error.
  375. */
  376. int qmi_kernel_decode(struct msg_desc *desc, void *out_c_struct,
  377. void *in_buf, uint32_t in_buf_len)
  378. {
  379. int dec_level = 1;
  380. int rc = 0;
  381. if (!desc || !desc->ei_array)
  382. return -EINVAL;
  383. if (!out_c_struct || !in_buf || !in_buf_len)
  384. return -EINVAL;
  385. if (desc->max_msg_len < in_buf_len)
  386. return -EINVAL;
  387. rc = _qmi_kernel_decode(desc->ei_array, out_c_struct,
  388. in_buf, in_buf_len, dec_level);
  389. if (rc < 0)
  390. return rc;
  391. else
  392. return 0;
  393. }
  394. EXPORT_SYMBOL(qmi_kernel_decode);
  395. /**
  396. * qmi_decode_basic_elem() - Decodes elements of basic/primary data type
  397. * @buf_dst: Buffer to store the decoded element.
  398. * @buf_src: Buffer containing the elements in QMI wire format.
  399. * @elem_len: Number of elements to be decoded.
  400. * @elem_size: Size of a single instance of the element to be decoded.
  401. *
  402. * @return: Total size of the decoded data elements, in bytes.
  403. *
  404. * This function decodes the "elem_len" number of elements in QMI wire format,
  405. * each of size "elem_size" bytes from the source buffer "buf_src" and stores
  406. * the decoded elements in the destination buffer "buf_dst". The elements are
  407. * of primary data type which include uint8_t - uint64_t or similar. This
  408. * function returns the number of bytes of decoded information.
  409. */
  410. static int qmi_decode_basic_elem(void *buf_dst, void *buf_src,
  411. uint32_t elem_len, uint32_t elem_size)
  412. {
  413. uint32_t i, rc = 0;
  414. for (i = 0; i < elem_len; i++) {
  415. QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size);
  416. rc += elem_size;
  417. }
  418. return rc;
  419. }
  420. /**
  421. * qmi_decode_struct_elem() - Decodes elements of struct data type
  422. * @ei_array: Struct info array descibing the struct element.
  423. * @buf_dst: Buffer to store the decoded element.
  424. * @buf_src: Buffer containing the elements in QMI wire format.
  425. * @elem_len: Number of elements to be decoded.
  426. * @tlv_len: Total size of the encoded inforation corresponding to
  427. * this struct element.
  428. * @dec_level: Depth of the nested structure from the main structure.
  429. *
  430. * @return: Total size of the decoded data elements, on success.
  431. * < 0 on error.
  432. *
  433. * This function decodes the "elem_len" number of elements in QMI wire format,
  434. * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src"
  435. * and stores the decoded elements in the destination buffer "buf_dst". The
  436. * elements are of struct data type which includes any C structure. This
  437. * function returns the number of bytes of decoded information.
  438. */
  439. static int qmi_decode_struct_elem(struct elem_info *ei_array, void *buf_dst,
  440. void *buf_src, uint32_t elem_len,
  441. uint32_t tlv_len, int dec_level)
  442. {
  443. int i, rc, decoded_bytes = 0;
  444. struct elem_info *temp_ei = ei_array;
  445. for (i = 0; i < elem_len; i++) {
  446. rc = _qmi_kernel_decode(temp_ei->ei_array, buf_dst, buf_src,
  447. (tlv_len/elem_len), dec_level);
  448. if (rc < 0)
  449. return rc;
  450. if (rc != (tlv_len/elem_len)) {
  451. pr_err("%s: Fault in decoding\n", __func__);
  452. return -EFAULT;
  453. }
  454. buf_src = buf_src + rc;
  455. buf_dst = buf_dst + temp_ei->elem_size;
  456. decoded_bytes += rc;
  457. }
  458. return decoded_bytes;
  459. }
  460. /**
  461. * find_ei() - Find element info corresponding to TLV Type
  462. * @ei_array: Struct info array of the message being decoded.
  463. * @type: TLV Type of the element being searched.
  464. *
  465. * @return: Pointer to struct info, if found
  466. *
  467. * Every element that got encoded in the QMI message will have a type
  468. * information associated with it. While decoding the QMI message,
  469. * this function is used to find the struct info regarding the element
  470. * that corresponds to the type being decoded.
  471. */
  472. static struct elem_info *find_ei(struct elem_info *ei_array,
  473. uint32_t type)
  474. {
  475. struct elem_info *temp_ei = ei_array;
  476. while (temp_ei->data_type != QMI_EOTI) {
  477. if (temp_ei->tlv_type == (uint8_t)type)
  478. return temp_ei;
  479. temp_ei = temp_ei + 1;
  480. }
  481. return NULL;
  482. }
  483. /**
  484. * _qmi_kernel_decode() - Core Decode Function
  485. * @ei_array: Struct info array describing the structure to be decoded.
  486. * @out_c_struct: Buffer to hold the decoded C struct
  487. * @in_buf: Buffer containing the QMI message to be decoded
  488. * @in_buf_len: Length of the QMI message to be decoded
  489. * @dec_level: Decode level to indicate the depth of the nested structure,
  490. * within the main structure, being decoded
  491. *
  492. * @return: Number of bytes of decoded information, on success
  493. * < 0 on error.
  494. */
  495. static int _qmi_kernel_decode(struct elem_info *ei_array,
  496. void *out_c_struct,
  497. void *in_buf, uint32_t in_buf_len,
  498. int dec_level)
  499. {
  500. struct elem_info *temp_ei = ei_array;
  501. uint8_t opt_flag_value = 1;
  502. uint32_t data_len_value = 0, data_len_sz = 0;
  503. uint8_t *buf_dst = out_c_struct;
  504. uint8_t *tlv_pointer;
  505. uint32_t tlv_len = 0;
  506. uint32_t tlv_type;
  507. uint32_t decoded_bytes = 0;
  508. void *buf_src = in_buf;
  509. int rc;
  510. QMI_DECODE_LOG_MSG(in_buf, in_buf_len);
  511. while (decoded_bytes < in_buf_len) {
  512. if (dec_level == 1) {
  513. tlv_pointer = buf_src;
  514. QMI_ENCDEC_DECODE_TLV(&tlv_type,
  515. &tlv_len, tlv_pointer);
  516. QMI_DECODE_LOG_TLV(tlv_type, tlv_len);
  517. buf_src += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
  518. decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
  519. temp_ei = find_ei(ei_array, tlv_type);
  520. if (!temp_ei) {
  521. pr_err("%s: Inval element info\n", __func__);
  522. return -EINVAL;
  523. }
  524. }
  525. buf_dst = out_c_struct + temp_ei->offset;
  526. if (temp_ei->data_type == QMI_OPT_FLAG) {
  527. memcpy(buf_dst, &opt_flag_value, sizeof(uint8_t));
  528. temp_ei = temp_ei + 1;
  529. buf_dst = out_c_struct + temp_ei->offset;
  530. }
  531. if (temp_ei->data_type == QMI_DATA_LEN) {
  532. data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ?
  533. sizeof(uint8_t) : sizeof(uint16_t);
  534. rc = qmi_decode_basic_elem(&data_len_value, buf_src,
  535. 1, data_len_sz);
  536. memcpy(buf_dst, &data_len_value, sizeof(uint32_t));
  537. temp_ei = temp_ei + 1;
  538. buf_dst = out_c_struct + temp_ei->offset;
  539. UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
  540. }
  541. if (temp_ei->is_array == NO_ARRAY) {
  542. data_len_value = 1;
  543. } else if (temp_ei->is_array == STATIC_ARRAY) {
  544. data_len_value = temp_ei->elem_len;
  545. } else if (data_len_value > temp_ei->elem_len) {
  546. pr_err("%s: Data len %d > max spec %d\n",
  547. __func__, data_len_value, temp_ei->elem_len);
  548. return -ETOOSMALL;
  549. }
  550. switch (temp_ei->data_type) {
  551. case QMI_UNSIGNED_1_BYTE:
  552. case QMI_UNSIGNED_2_BYTE:
  553. case QMI_UNSIGNED_4_BYTE:
  554. case QMI_UNSIGNED_8_BYTE:
  555. case QMI_SIGNED_2_BYTE_ENUM:
  556. case QMI_SIGNED_4_BYTE_ENUM:
  557. rc = qmi_decode_basic_elem(buf_dst, buf_src,
  558. data_len_value, temp_ei->elem_size);
  559. QMI_DECODE_LOG_ELEM(dec_level, data_len_value,
  560. temp_ei->elem_size, buf_dst);
  561. UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
  562. break;
  563. case QMI_STRUCT:
  564. rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src,
  565. data_len_value, tlv_len, (dec_level + 1));
  566. if (rc < 0)
  567. return rc;
  568. UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
  569. break;
  570. default:
  571. pr_err("%s: Unrecognized data type\n", __func__);
  572. return -EINVAL;
  573. }
  574. temp_ei = temp_ei + 1;
  575. }
  576. return decoded_bytes;
  577. }
  578. MODULE_DESCRIPTION("QMI kernel enc/dec");
  579. MODULE_LICENSE("GPL v2");