Commit | Line | Data |
---|---|---|
9b8a11e8 BA |
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"); |