diff --git a/dsp/q6afe.c b/dsp/q6afe.c index 6c188d7ab0..4855c6a343 100644 --- a/dsp/q6afe.c +++ b/dsp/q6afe.c @@ -1466,7 +1466,8 @@ done: } static int q6afe_svc_set_params(int index, struct mem_mapping_hdr *mem_hdr, - u8 *packed_param_data, u32 packed_data_size) + u8 *packed_param_data, u32 packed_data_size, + bool is_iid_supported) { int ret; @@ -1476,7 +1477,7 @@ static int q6afe_svc_set_params(int index, struct mem_mapping_hdr *mem_hdr, return ret; } - if (q6common_is_instance_id_supported()) + if (is_iid_supported) return q6afe_svc_set_params_v2(index, mem_hdr, packed_param_data, packed_data_size); @@ -1494,13 +1495,15 @@ static int q6afe_svc_pack_and_set_param_in_band(int index, u32 packed_data_size = sizeof(struct param_hdr_v3) + param_hdr.param_size; int ret = 0; + bool is_iid_supported = q6common_is_instance_id_supported(); packed_param_data = kzalloc(packed_data_size, GFP_KERNEL); if (!packed_param_data) return -ENOMEM; - ret = q6common_pack_pp_params(packed_param_data, ¶m_hdr, param_data, - &packed_data_size); + ret = q6common_pack_pp_params_v2(packed_param_data, ¶m_hdr, + param_data, &packed_data_size, + is_iid_supported); if (ret) { pr_err("%s: Failed to pack parameter header and data, error %d\n", __func__, ret); @@ -1508,7 +1511,7 @@ static int q6afe_svc_pack_and_set_param_in_band(int index, } ret = q6afe_svc_set_params(index, NULL, packed_param_data, - packed_data_size); + packed_data_size, is_iid_supported); done: kfree(packed_param_data); @@ -2595,6 +2598,7 @@ static int afe_send_codec_reg_config( struct param_hdr_v3 param_hdr; int idx = 0; int ret = -EINVAL; + bool is_iid_supported = q6common_is_instance_id_supported(); memset(¶m_hdr, 0, sizeof(param_hdr)); max_single_param = sizeof(struct param_hdr_v3) + @@ -2617,10 +2621,10 @@ static int afe_send_codec_reg_config( while (packed_data_size + max_single_param < max_data_size && idx < cdc_reg_cfg->num_registers) { - ret = q6common_pack_pp_params( + ret = q6common_pack_pp_params_v2( packed_param_data + packed_data_size, ¶m_hdr, (u8 *) &cdc_reg_cfg->reg_data[idx], - &single_param_size); + &single_param_size, is_iid_supported); if (ret) { pr_err("%s: Failed to pack parameters with error %d\n", __func__, ret); @@ -2631,7 +2635,8 @@ static int afe_send_codec_reg_config( } ret = q6afe_svc_set_params(IDX_GLOBAL_CFG, NULL, - packed_param_data, packed_data_size); + packed_param_data, packed_data_size, + is_iid_supported); if (ret) { pr_err("%s: AFE_PARAM_ID_CDC_REG_CFG failed %d\n", __func__, ret); diff --git a/dsp/q6common.c b/dsp/q6common.c index 8270054e8e..b5444b1347 100644 --- a/dsp/q6common.c +++ b/dsp/q6common.c @@ -1,6 +1,6 @@ // SPDX-License-Identifier: GPL-2.0-only /* - * Copyright (c) 2017-2018, The Linux Foundation. All rights reserved. + * Copyright (c) 2017-2019, The Linux Foundation. All rights reserved. */ #include @@ -104,3 +104,74 @@ int q6common_pack_pp_params(u8 *dest, struct param_hdr_v3 *v3_hdr, return 0; } EXPORT_SYMBOL(q6common_pack_pp_params); + +/** + * q6common_pack_pp_params_v2 + * + * Populate params header based on instance ID support and pack + * it with payload. + * Instance ID support - + * yes - param_hdr_v3 + payload + * no - param_hdr_v1 + payload + * + * @dest: destination data pointer to be packed into + * @v3_hdr: param header v3 + * @param_data: param payload + * @total_size: total size of packed data (hdr + payload) + * @iid_supported: Instance ID supported or not + * + * Returns 0 on success or error on failure + */ +int q6common_pack_pp_params_v2(u8 *dest, struct param_hdr_v3 *v3_hdr, + u8 *param_data, u32 *total_size, + bool iid_supported) +{ + struct param_hdr_v1 *v1_hdr = NULL; + u32 packed_size = 0; + u32 param_size = 0; + + if (dest == NULL) { + pr_err("%s: Received NULL pointer for destination\n", __func__); + return -EINVAL; + } else if (v3_hdr == NULL) { + pr_err("%s: Received NULL pointer for header\n", __func__); + return -EINVAL; + } else if (total_size == NULL) { + pr_err("%s: Received NULL pointer for total size\n", __func__); + return -EINVAL; + } + + param_size = v3_hdr->param_size; + packed_size = iid_supported ? sizeof(struct param_hdr_v3) : + sizeof(struct param_hdr_v1); + + if (iid_supported) { + memcpy(dest, v3_hdr, packed_size); + } else { + v1_hdr = (struct param_hdr_v1 *) dest; + v1_hdr->module_id = v3_hdr->module_id; + v1_hdr->param_id = v3_hdr->param_id; + + if (param_size > U16_MAX) { + pr_err("%s: Invalid param size for V1 %d\n", __func__, + param_size); + return -EINVAL; + } + v1_hdr->param_size = param_size; + v1_hdr->reserved = 0; + } + + /* + * Make param_data optional for cases when there is no data + * present as in some set cases and all get cases. + */ + if (param_data != NULL) { + memcpy(dest + packed_size, param_data, param_size); + packed_size += param_size; + } + + *total_size = packed_size; + + return 0; +} +EXPORT_SYMBOL(q6common_pack_pp_params_v2); diff --git a/include/dsp/q6common.h b/include/dsp/q6common.h index c21ac5cc81..7250a138ed 100644 --- a/include/dsp/q6common.h +++ b/include/dsp/q6common.h @@ -1,6 +1,6 @@ /* SPDX-License-Identifier: GPL-2.0-only */ /* - * Copyright (c) 2017-2018, The Linux Foundation. All rights reserved. + * Copyright (c) 2017-2019, The Linux Foundation. All rights reserved. */ #ifndef __Q6COMMON_H__ @@ -12,5 +12,7 @@ void q6common_update_instance_id_support(bool supported); bool q6common_is_instance_id_supported(void); int q6common_pack_pp_params(u8 *dest, struct param_hdr_v3 *v3_hdr, u8 *param_data, u32 *total_size); - +int q6common_pack_pp_params_v2(u8 *dest, struct param_hdr_v3 *v3_hdr, + u8 *param_data, u32 *total_size, + bool iid_supported); #endif /* __Q6COMMON_H__ */