qmi_encdec.c 24 KB

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