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