Merge "dsp: Fix payload mismatch issue in AFE clock setting"

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, &param_hdr, param_data,
+	ret = q6common_pack_pp_params_v2(packed_param_data, &param_hdr,
-				      &packed_data_size);
+					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(&param_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,
 				&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);
@@ -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);

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