dsp: Fix payload mismatch issue in AFE clock setting

IID support is set from user space during boot up
which can come in parallel with AFE set clock requests
due to which param payload is set in difference with
the actual set param command as per instance id support.

Change-Id: I07e45fda0943cac814f25c16da930dfae5d209a6
Signed-off-by: Aditya Bavanari <[email protected]>

dsp/q6afe.c

@@ -1461,7 +1461,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;
 
@@ -1471,7 +1472,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);
@@ -1489,13 +1490,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, &param_hdr, param_data,
-				      &packed_data_size);
+	ret = q6common_pack_pp_params_v2(packed_param_data, &param_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);
@@ -1503,7 +1506,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);
@@ -2590,6 +2593,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(&param_hdr, 0, sizeof(param_hdr));
 	max_single_param = sizeof(struct param_hdr_v3) +
@@ -2612,10 +2616,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,
 				&param_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);
@@ -2626,7 +2630,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);

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 <dsp/q6common.h>
@@ -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);

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__ */