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(const struct qmi_elem_info *ei_array, void *out_buf, |
61 | const void *in_c_struct, u32 out_buf_len, |
62 | int enc_level); |
63 | |
64 | static int qmi_decode(const struct qmi_elem_info *ei_array, void *out_c_struct, |
65 | const void *in_buf, u32 in_buf_len, int dec_level); |
66 | |
67 | /** |
68 | * skip_to_next_elem() - Skip to next element in the structure to be encoded |
69 | * @ei_array: Struct info describing the element to be skipped. |
70 | * @level: Depth level of encoding/decoding to identify nested structures. |
71 | * |
72 | * This function is used while encoding optional elements. If the flag |
73 | * corresponding to an optional element is not set, then encoding the |
74 | * optional element can be skipped. This function can be used to perform |
75 | * that operation. |
76 | * |
77 | * Return: struct info of the next element that can be encoded. |
78 | */ |
79 | static const struct qmi_elem_info * |
80 | skip_to_next_elem(const struct qmi_elem_info *ei_array, int level) |
81 | { |
82 | const struct qmi_elem_info *temp_ei = ei_array; |
83 | u8 tlv_type; |
84 | |
85 | if (level > 1) { |
86 | temp_ei = temp_ei + 1; |
87 | } else { |
88 | do { |
89 | tlv_type = temp_ei->tlv_type; |
90 | temp_ei = temp_ei + 1; |
91 | } while (tlv_type == temp_ei->tlv_type); |
92 | } |
93 | |
94 | return temp_ei; |
95 | } |
96 | |
97 | /** |
98 | * qmi_calc_min_msg_len() - Calculate the minimum length of a QMI message |
99 | * @ei_array: Struct info array describing the structure. |
100 | * @level: Level to identify the depth of the nested structures. |
101 | * |
102 | * Return: Expected minimum length of the QMI message or 0 on error. |
103 | */ |
104 | static int qmi_calc_min_msg_len(const struct qmi_elem_info *ei_array, |
105 | int level) |
106 | { |
107 | int min_msg_len = 0; |
108 | const struct qmi_elem_info *temp_ei = ei_array; |
109 | |
110 | if (!ei_array) |
111 | return min_msg_len; |
112 | |
113 | while (temp_ei->data_type != QMI_EOTI) { |
114 | /* Optional elements do not count in minimum length */ |
115 | if (temp_ei->data_type == QMI_OPT_FLAG) { |
116 | temp_ei = skip_to_next_elem(ei_array: 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(ei_array: temp_ei->ei_array, |
127 | level: (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(const struct qmi_elem_info *ei_array, |
198 | void *buf_dst, const void *buf_src, |
199 | u32 elem_len, u32 out_buf_len, |
200 | int enc_level) |
201 | { |
202 | int i, rc, encoded_bytes = 0; |
203 | const struct qmi_elem_info *temp_ei = ei_array; |
204 | |
205 | for (i = 0; i < elem_len; i++) { |
206 | rc = qmi_encode(ei_array: temp_ei->ei_array, out_buf: buf_dst, in_c_struct: buf_src, |
207 | out_buf_len: 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(const struct qmi_elem_info *ei_array, |
237 | void *buf_dst, const void *buf_src, |
238 | u32 out_buf_len, int enc_level) |
239 | { |
240 | int rc; |
241 | int encoded_bytes = 0; |
242 | const struct qmi_elem_info *temp_ei = ei_array; |
243 | u32 string_len = 0; |
244 | u32 string_len_sz = 0; |
245 | |
246 | string_len = strlen(buf_src); |
247 | string_len_sz = temp_ei->elem_len <= U8_MAX ? |
248 | sizeof(u8) : sizeof(u16); |
249 | if (string_len > temp_ei->elem_len) { |
250 | pr_err("%s: String to be encoded is longer - %d > %d\n" , |
251 | __func__, string_len, temp_ei->elem_len); |
252 | return -EINVAL; |
253 | } |
254 | |
255 | if (enc_level == 1) { |
256 | if (string_len + TLV_LEN_SIZE + TLV_TYPE_SIZE > |
257 | out_buf_len) { |
258 | pr_err("%s: Output len %d > Out Buf len %d\n" , |
259 | __func__, string_len, out_buf_len); |
260 | return -ETOOSMALL; |
261 | } |
262 | } else { |
263 | if (string_len + string_len_sz > out_buf_len) { |
264 | pr_err("%s: Output len %d > Out Buf len %d\n" , |
265 | __func__, string_len, out_buf_len); |
266 | return -ETOOSMALL; |
267 | } |
268 | rc = qmi_encode_basic_elem(buf_dst, buf_src: &string_len, |
269 | elem_len: 1, elem_size: string_len_sz); |
270 | encoded_bytes += rc; |
271 | } |
272 | |
273 | rc = qmi_encode_basic_elem(buf_dst: buf_dst + encoded_bytes, buf_src, |
274 | elem_len: string_len, elem_size: 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(const struct qmi_elem_info *ei_array, void *out_buf, |
293 | const void *in_c_struct, u32 out_buf_len, |
294 | int enc_level) |
295 | { |
296 | const struct qmi_elem_info *temp_ei = ei_array; |
297 | u8 opt_flag_value = 0; |
298 | u32 data_len_value = 0, data_len_sz; |
299 | u8 *buf_dst = (u8 *)out_buf; |
300 | u8 *tlv_pointer; |
301 | u32 tlv_len; |
302 | u8 tlv_type; |
303 | u32 encoded_bytes = 0; |
304 | const void *buf_src; |
305 | int encode_tlv = 0; |
306 | int rc; |
307 | |
308 | if (!ei_array) |
309 | return 0; |
310 | |
311 | tlv_pointer = buf_dst; |
312 | tlv_len = 0; |
313 | if (enc_level == 1) |
314 | buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE); |
315 | |
316 | while (temp_ei->data_type != QMI_EOTI) { |
317 | buf_src = in_c_struct + temp_ei->offset; |
318 | tlv_type = temp_ei->tlv_type; |
319 | |
320 | if (temp_ei->array_type == NO_ARRAY) { |
321 | data_len_value = 1; |
322 | } else if (temp_ei->array_type == STATIC_ARRAY) { |
323 | data_len_value = temp_ei->elem_len; |
324 | } else if (data_len_value <= 0 || |
325 | temp_ei->elem_len < data_len_value) { |
326 | pr_err("%s: Invalid data length\n" , __func__); |
327 | return -EINVAL; |
328 | } |
329 | |
330 | switch (temp_ei->data_type) { |
331 | case QMI_OPT_FLAG: |
332 | rc = qmi_encode_basic_elem(buf_dst: &opt_flag_value, buf_src, |
333 | elem_len: 1, elem_size: sizeof(u8)); |
334 | if (opt_flag_value) |
335 | temp_ei = temp_ei + 1; |
336 | else |
337 | temp_ei = skip_to_next_elem(ei_array: temp_ei, level: 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, buf_src: &data_len_value, |
352 | elem_len: 1, elem_size: 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(ei_array: temp_ei, level: 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 | elem_len: data_len_value, |
378 | elem_size: 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(ei_array: temp_ei, buf_dst, buf_src, |
386 | elem_len: data_len_value, |
387 | out_buf_len: out_buf_len - encoded_bytes, |
388 | enc_level: 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(ei_array: temp_ei, buf_dst, buf_src, |
398 | out_buf_len: 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 describing the struct element. |
455 | * @buf_dst: Buffer to store the decoded element. |
456 | * @buf_src: Buffer containing the elements in QMI wire format. |
457 | * @elem_len: Number of elements to be decoded. |
458 | * @tlv_len: Total size of the encoded information corresponding to |
459 | * this struct element. |
460 | * @dec_level: Depth of the nested structure from the main structure. |
461 | * |
462 | * This function decodes the "elem_len" number of elements in QMI wire format, |
463 | * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src" |
464 | * and stores the decoded elements in the destination buffer "buf_dst". The |
465 | * elements are of struct data type which includes any C structure. This |
466 | * function returns the number of bytes of decoded information. |
467 | * |
468 | * Return: The total size of the decoded data elements on success, negative |
469 | * errno on error. |
470 | */ |
471 | static int qmi_decode_struct_elem(const struct qmi_elem_info *ei_array, |
472 | void *buf_dst, const void *buf_src, |
473 | u32 elem_len, u32 tlv_len, |
474 | int dec_level) |
475 | { |
476 | int i, rc, decoded_bytes = 0; |
477 | const struct qmi_elem_info *temp_ei = ei_array; |
478 | |
479 | for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) { |
480 | rc = qmi_decode(ei_array: temp_ei->ei_array, out_c_struct: buf_dst, in_buf: buf_src, |
481 | in_buf_len: tlv_len - decoded_bytes, dec_level); |
482 | if (rc < 0) |
483 | return rc; |
484 | buf_src = buf_src + rc; |
485 | buf_dst = buf_dst + temp_ei->elem_size; |
486 | decoded_bytes += rc; |
487 | } |
488 | |
489 | if ((dec_level <= 2 && decoded_bytes != tlv_len) || |
490 | (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) { |
491 | pr_err("%s: Fault in decoding: dl(%d), db(%d), tl(%d), i(%d), el(%d)\n" , |
492 | __func__, dec_level, decoded_bytes, tlv_len, |
493 | i, elem_len); |
494 | return -EFAULT; |
495 | } |
496 | |
497 | return decoded_bytes; |
498 | } |
499 | |
500 | /** |
501 | * qmi_decode_string_elem() - Decodes elements of string data type |
502 | * @ei_array: Struct info array describing the string element. |
503 | * @buf_dst: Buffer to store the decoded element. |
504 | * @buf_src: Buffer containing the elements in QMI wire format. |
505 | * @tlv_len: Total size of the encoded information corresponding to |
506 | * this string element. |
507 | * @dec_level: Depth of the string element from the main structure. |
508 | * |
509 | * This function decodes the string element of maximum length |
510 | * "ei_array->elem_len" from the source buffer "buf_src" and puts it into |
511 | * the destination buffer "buf_dst". This function returns number of bytes |
512 | * decoded from the input buffer. |
513 | * |
514 | * Return: The total size of the decoded data elements on success, negative |
515 | * errno on error. |
516 | */ |
517 | static int qmi_decode_string_elem(const struct qmi_elem_info *ei_array, |
518 | void *buf_dst, const void *buf_src, |
519 | u32 tlv_len, int dec_level) |
520 | { |
521 | int rc; |
522 | int decoded_bytes = 0; |
523 | u32 string_len = 0; |
524 | u32 string_len_sz = 0; |
525 | const struct qmi_elem_info *temp_ei = ei_array; |
526 | |
527 | if (dec_level == 1) { |
528 | string_len = tlv_len; |
529 | } else { |
530 | string_len_sz = temp_ei->elem_len <= U8_MAX ? |
531 | sizeof(u8) : sizeof(u16); |
532 | rc = qmi_decode_basic_elem(buf_dst: &string_len, buf_src, |
533 | elem_len: 1, elem_size: 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: buf_src + decoded_bytes, |
548 | elem_len: string_len, elem_size: 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 const struct qmi_elem_info *find_ei(const struct qmi_elem_info *ei_array, |
568 | u32 type) |
569 | { |
570 | const struct qmi_elem_info *temp_ei = ei_array; |
571 | |
572 | while (temp_ei->data_type != QMI_EOTI) { |
573 | if (temp_ei->tlv_type == (u8)type) |
574 | return temp_ei; |
575 | temp_ei = temp_ei + 1; |
576 | } |
577 | |
578 | return NULL; |
579 | } |
580 | |
581 | /** |
582 | * qmi_decode() - Core Decode Function |
583 | * @ei_array: Struct info array describing the structure to be decoded. |
584 | * @out_c_struct: Buffer to hold the decoded C struct |
585 | * @in_buf: Buffer containing the QMI message to be decoded |
586 | * @in_buf_len: Length of the QMI message to be decoded |
587 | * @dec_level: Decode level to indicate the depth of the nested structure, |
588 | * within the main structure, being decoded |
589 | * |
590 | * Return: The number of bytes of decoded information on success, negative |
591 | * errno on error. |
592 | */ |
593 | static int qmi_decode(const struct qmi_elem_info *ei_array, void *out_c_struct, |
594 | const void *in_buf, u32 in_buf_len, |
595 | int dec_level) |
596 | { |
597 | const struct qmi_elem_info *temp_ei = ei_array; |
598 | u8 opt_flag_value = 1; |
599 | u32 data_len_value = 0, data_len_sz = 0; |
600 | u8 *buf_dst = out_c_struct; |
601 | const u8 *tlv_pointer; |
602 | u32 tlv_len = 0; |
603 | u32 tlv_type; |
604 | u32 decoded_bytes = 0; |
605 | const void *buf_src = in_buf; |
606 | int rc; |
607 | |
608 | while (decoded_bytes < in_buf_len) { |
609 | if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI) |
610 | return decoded_bytes; |
611 | |
612 | if (dec_level == 1) { |
613 | tlv_pointer = buf_src; |
614 | QMI_ENCDEC_DECODE_TLV(&tlv_type, |
615 | &tlv_len, tlv_pointer); |
616 | buf_src += (TLV_TYPE_SIZE + TLV_LEN_SIZE); |
617 | decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE); |
618 | temp_ei = find_ei(ei_array, type: 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(buf_dst: &data_len_value, buf_src, |
646 | elem_len: 1, elem_size: 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 | elem_len: data_len_value, |
673 | elem_size: 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(ei_array: temp_ei, buf_dst, buf_src, |
679 | elem_len: data_len_value, tlv_len, |
680 | dec_level: 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(ei_array: 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, const struct qmi_elem_info *ei, |
717 | const void *c_struct) |
718 | { |
719 | struct qmi_header *hdr; |
720 | ssize_t msglen = 0; |
721 | void *msg; |
722 | int ret; |
723 | |
724 | /* Check the possibility of a zero length QMI message */ |
725 | if (!c_struct) { |
726 | ret = qmi_calc_min_msg_len(ei_array: ei, level: 1); |
727 | if (ret) { |
728 | pr_err("%s: Calc. len %d != 0, but NULL c_struct\n" , |
729 | __func__, ret); |
730 | return ERR_PTR(error: -EINVAL); |
731 | } |
732 | } |
733 | |
734 | msg = kzalloc(size: sizeof(*hdr) + *len, GFP_KERNEL); |
735 | if (!msg) |
736 | return ERR_PTR(error: -ENOMEM); |
737 | |
738 | /* Encode message, if we have a message */ |
739 | if (c_struct) { |
740 | msglen = qmi_encode(ei_array: ei, out_buf: msg + sizeof(*hdr), in_c_struct: c_struct, out_buf_len: *len, enc_level: 1); |
741 | if (msglen < 0) { |
742 | kfree(objp: msg); |
743 | return ERR_PTR(error: 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_GPL(qmi_encode_message); |
758 | |
759 | /** |
760 | * qmi_decode_message() - Decode QMI encoded message to C structure |
761 | * @buf: Buffer with encoded message |
762 | * @len: Amount of data in @buf |
763 | * @ei: QMI message descriptor |
764 | * @c_struct: Reference to structure to decode into |
765 | * |
766 | * Return: The number of bytes of decoded information on success, negative |
767 | * errno on error. |
768 | */ |
769 | int qmi_decode_message(const void *buf, size_t len, |
770 | const struct qmi_elem_info *ei, void *c_struct) |
771 | { |
772 | if (!ei) |
773 | return -EINVAL; |
774 | |
775 | if (!c_struct || !buf || !len) |
776 | return -EINVAL; |
777 | |
778 | return qmi_decode(ei_array: ei, out_c_struct: c_struct, in_buf: buf + sizeof(struct qmi_header), |
779 | in_buf_len: len - sizeof(struct qmi_header), dec_level: 1); |
780 | } |
781 | EXPORT_SYMBOL_GPL(qmi_decode_message); |
782 | |
783 | /* Common header in all QMI responses */ |
784 | const 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_GPL(qmi_response_type_v01_ei); |
814 | |
815 | MODULE_DESCRIPTION("QMI encoder/decoder helper" ); |
816 | MODULE_LICENSE("GPL v2" ); |
817 | |