MLK-22333: ASoC: fsl_easrc: Support FLOAT_LE

The audio float point data range is (-1, 1), the asrc would output
all zero for float point input and integer output case, that is to
drop the fractional part of the data directly.

In order to support float to int conversion or int to float conversion
we need to do special operation on the coefficient to enlarge/reduce
the data to the expected range.

For float to int case:
Up sampling:
1. Create a 1 tap filter with center tap (only tap) of 2^31
   in 64 bits floating point.
   double value = (double)(((uint64_t)1) << 31);
2. Program 1 tap prefilter with center tap above.

Down sampling,
1. If the filter is single stage filter, add "shift" to the exponent of
   stage 1 coefficients.
2. If the filter is two stage filter , add "shift" to the exponent of
   stage 2 coefficients.

The "shift" is 31, same for int16, int24, int32 case.

For int to float case:
Up sampling:
1. Create a 1 tap filter with center tap (only tap) of 2^-31
   in 64 bits floating point.
2. Program 1 tap prefilter with center tap above.

Down sampling,
1. If the filter is single stage filter, subtract "shift" to the
   exponent of stage 1 coefficients.
2. If the filter is two stage filter , subtract "shift" to the
   exponent of stage 2 coefficients.

The "shift" is 15,23,31, different for int16, int24, int32 case.

Signed-off-by: Shengjiu Wang <shengjiu.wang@nxp.com>
(cherry picked from commit 8a18a7a2dd1184814c6c61cb116f4d868b003447)

3 files changed, 199 insertions, 98 deletions

diff --git a/sound/soc/fsl/fsl_easrc.c b/sound/soc/fsl/fsl_easrc.c
index cea61f774bc7..682bffdb6373 100644
--- a/sound/soc/fsl/fsl_easrc.c
+++ b/sound/soc/fsl/fsl_easrc.c
@@ -43,6 +43,7 @@ extern struct snd_soc_component_driver fsl_easrc_dma_component;
 				 SNDRV_PCM_FMTBIT_U32_LE | \
 				 SNDRV_PCM_FMTBIT_S20_3LE | \
 				 SNDRV_PCM_FMTBIT_U20_3LE | \
+				 SNDRV_PCM_FMTBIT_FLOAT_LE | \
 				 SNDRV_PCM_FMTBIT_IEC958_SUBFRAME_LE)
 
 static int fsl_easrc_iec958_put_bits(struct snd_kcontrol *kcontrol,
@@ -321,14 +322,62 @@ static int fsl_easrc_resampler_config(struct fsl_easrc *easrc)
 	return 0;
 }
 
+/*****************************************************************************
+ *  Scale filter coefficients (64 bits float)
+ *  For input float32 normalized range (1.0,-1.0) -> output int[16,24,32]:
+ *      scale it by multiplying filter coefficients by 2^31
+ *  For input int[16, 24, 32] -> output float32
+ *      scale it by multiplying filter coefficients by 2^-15, 2^-23, 2^-31
+ *  input:
+ *      easrc:  Structure pointer of fsl_easrc
+ *      filterIn : Pointer to non-scaled input filter
+ *      shift:  The multiply factor
+ *  output:
+ *      filterOut: scaled filter
+ *****************************************************************************/
+static int NormalizedFilterForFloat32InIntOut(struct fsl_easrc *easrc,
+					      uint64_t *filterIn,
+					      uint64_t *filterOut,
+					      int shift)
+{
+	struct device *dev = &easrc->pdev->dev;
+	uint64_t coef = *filterIn;
+	int64_t exp  = (coef & 0x7ff0000000000000ll) >> 52;
+	uint64_t coefOut;
+
+	/*
+	 * If exponent is zero (value == 0), or 7ff (value == NaNs)
+	 * dont touch the content
+	 */
+	if (((coef & 0x7ff0000000000000ll) == 0) ||
+	    ((coef & 0x7ff0000000000000ll) == ((uint64_t)0x7ff << 52))) {
+		*filterOut = coef;
+	} else {
+		if ((shift > 0 && (shift + exp) >= 2047) ||
+		    (shift < 0 && (exp + shift) <= 0)) {
+			dev_err(dev, "coef error\n");
+			return -EINVAL;
+		}
+
+		/* coefficient * 2^shift ==>  coefficient_exp + shift */
+		exp += shift;
+		coefOut = (uint64_t)(coef & 0x800FFFFFFFFFFFFFll) +
+					((uint64_t)exp << 52);
+		*filterOut = coefOut;
+	}
+
+	return 0;
+}
+
 static int write_pf_coeff_mem(struct fsl_easrc *easrc, int ctx_id,
-			      u64 *arr, int n_taps)
+			      u64 *arr, int n_taps, int shift)
 {
 	struct device *dev = &easrc->pdev->dev;
 	int ret = 0;
 	int i;
 	u32 *r;
 	u32 r0, r1;
+	u64 tmp;
 
 	/* If STx_NUM_TAPS is set to 0x0 then return */
 	if (!n_taps)
@@ -347,7 +396,15 @@ static int write_pf_coeff_mem(struct fsl_easrc *easrc, int ctx_id,
 		return ret;
 
 	for (i = 0; i < (n_taps + 1) / 2; i++) {
-		r = (uint32_t *)&arr[i];
+		ret = NormalizedFilterForFloat32InIntOut(easrc,
+						   &arr[i],
+						   &tmp,
+						   shift);
+		if (ret)
+			return ret;
+
+		r = (uint32_t *)&tmp;
+
 		r0 = r[0] & EASRC_32b_MASK;
 		r1 = r[1] & EASRC_32b_MASK;
 
@@ -375,6 +432,7 @@ static int fsl_easrc_prefilter_config(struct fsl_easrc *easrc,
 	struct device *dev;
 	u32 inrate, outrate, offset = 0;
 	int ret, i;
+	u64 coeff = 0x3FF0000000000000; /* Used by float->int */
 
 	/* to modify prefilter coeficients, the user must perform
 	 * a write in ASRC_PRE_COEFF_FIFO[COEFF_DATA] while the
@@ -412,24 +470,6 @@ static int fsl_easrc_prefilter_config(struct fsl_easrc *easrc,
 	 * When out_rate >= in_rate, pf will be in bypass mode
 	 */
 	if (ctx->out_params.sample_rate >= ctx->in_params.sample_rate) {
-		/* set pf in bypass mode */
-		ret = regmap_update_bits(easrc->regmap,
-					 REG_EASRC_CCE1(ctx_id),
-					 EASRC_CCE1_PF_BYPASS_MASK,
-					 EASRC_CCE1_PF_BYPASS);
-		if (ret)
-			return ret;
-
-		/* PF_EXPANSION_FACTOR must be set to 0x0 when
-		 * operating in bypass mode
-		 */
-		ret = regmap_update_bits(easrc->regmap,
-					 REG_EASRC_CCE1(ctx_id),
-					 EASRC_CCE1_PF_EXP_MASK,
-					 EASRC_CCE1_PF_EXP(0));
-		if (ret)
-			return ret;
-
 		if (ctx->out_params.sample_rate == ctx->in_params.sample_rate) {
 			ret = regmap_update_bits(easrc->regmap,
 						 REG_EASRC_CCE1(ctx_id),
@@ -439,48 +479,97 @@ static int fsl_easrc_prefilter_config(struct fsl_easrc *easrc,
 				return ret;
 		}
 
-		return 0;
-	}
-
-	inrate = ctx->in_params.norm_rate;
-	outrate = ctx->out_params.norm_rate;
+		if (ctx->in_params.sample_format == SNDRV_PCM_FORMAT_FLOAT_LE &&
+		    ctx->out_params.sample_format != SNDRV_PCM_FORMAT_FLOAT_LE) {
+			ctx->st1_num_taps = 1;
+			ctx->st1_coeff    = &coeff;
+			ctx->st1_num_exp  = 1;
+			ctx->st2_num_taps = 0;
+			ctx->st1_addexp = 31;
+		} else if (ctx->in_params.sample_format != SNDRV_PCM_FORMAT_FLOAT_LE &&
+			   ctx->out_params.sample_format == SNDRV_PCM_FORMAT_FLOAT_LE) {
+			ctx->st1_num_taps = 1;
+			ctx->st1_coeff    = &coeff;
+			ctx->st1_num_exp  = 1;
+			ctx->st2_num_taps = 0;
+			ctx->st1_addexp -= ctx->in_params.fmt.addexp;
+		} else {
+			/* set pf in bypass mode */
+			ret = regmap_update_bits(easrc->regmap,
+						 REG_EASRC_CCE1(ctx_id),
+						 EASRC_CCE1_PF_BYPASS_MASK,
+						 EASRC_CCE1_PF_BYPASS);
+			if (ret)
+				return ret;
 
-	hdr = easrc->firmware_hdr;
-	prefil = easrc->prefil;
+			/* PF_EXPANSION_FACTOR must be set to 0x0 when
+			 * operating in bypass mode
+			 */
+			ret = regmap_update_bits(easrc->regmap,
+						 REG_EASRC_CCE1(ctx_id),
+						 EASRC_CCE1_PF_EXP_MASK,
+						 EASRC_CCE1_PF_EXP(0));
+			if (ret)
+				return ret;
 
-	for (i = 0; i < hdr->prefil_scen; i++) {
-		if (inrate == prefil[i].insr && outrate == prefil[i].outsr) {
-			selected_prefil = &prefil[i];
-			dev_dbg(dev, "Selected prefilter: %u insr, %u outsr, %u st1_taps, %u st2_taps\n",
-				selected_prefil->insr,
-				selected_prefil->outsr,
-				selected_prefil->st1_taps,
-				selected_prefil->st2_taps);
-			break;
+			return 0;
 		}
-	}
 
-	if (!selected_prefil) {
-		dev_err(dev, "Conversion from in ratio %u(%u) to out ratio %u(%u) is not supported\n",
-			ctx->in_params.sample_rate,
-			inrate,
-			ctx->out_params.sample_rate, outrate);
-		return -EINVAL;
-	}
+	} else {
+		inrate = ctx->in_params.norm_rate;
+		outrate = ctx->out_params.norm_rate;
+
+		hdr = easrc->firmware_hdr;
+		prefil = easrc->prefil;
+
+		for (i = 0; i < hdr->prefil_scen; i++) {
+			if (inrate == prefil[i].insr && outrate == prefil[i].outsr) {
+				selected_prefil = &prefil[i];
+				dev_dbg(dev, "Selected prefilter: %u insr, %u outsr, %u st1_taps, %u st2_taps\n",
+					selected_prefil->insr,
+					selected_prefil->outsr,
+					selected_prefil->st1_taps,
+					selected_prefil->st2_taps);
+				break;
+			}
+		}
 
-	/* in prefilter coeff array, first st1_num_taps represent the
-	 * stage1 prefilter coefficients followed by next st2_num_taps
-	 * representing stage 2 coefficients
-	 */
-	ctx->st1_num_taps = selected_prefil->st1_taps;
-	ctx->st1_coeff    = selected_prefil->coeff;
-	ctx->st1_num_exp  = selected_prefil->st1_exp;
+		if (!selected_prefil) {
+			dev_err(dev, "Conversion from in ratio %u(%u) to out ratio %u(%u) is not supported\n",
+				ctx->in_params.sample_rate,
+				inrate,
+				ctx->out_params.sample_rate, outrate);
+			return -EINVAL;
+		}
 
-	offset = ((selected_prefil->st1_taps + 1) / 2) *
-			sizeof(selected_prefil->coeff[0]);
-	ctx->st2_num_taps = selected_prefil->st2_taps;
-	ctx->st2_coeff    = (uint64_t *)((uint64_t)selected_prefil->coeff +
-								offset);
+		/* in prefilter coeff array, first st1_num_taps represent the
+		 * stage1 prefilter coefficients followed by next st2_num_taps
+		 * representing stage 2 coefficients
+		 */
+		ctx->st1_num_taps = selected_prefil->st1_taps;
+		ctx->st1_coeff    = selected_prefil->coeff;
+		ctx->st1_num_exp  = selected_prefil->st1_exp;
+
+		offset = ((selected_prefil->st1_taps + 1) / 2) *
+				sizeof(selected_prefil->coeff[0]);
+		ctx->st2_num_taps = selected_prefil->st2_taps;
+		ctx->st2_coeff    = (uint64_t *)((uint64_t)selected_prefil->coeff + offset);
+
+		if (ctx->in_params.sample_format == SNDRV_PCM_FORMAT_FLOAT_LE &&
+		    ctx->out_params.sample_format != SNDRV_PCM_FORMAT_FLOAT_LE) {
+			/* only change stage2 coefficient for 2 stage case */
+			if (ctx->st2_num_taps > 0)
+				ctx->st2_addexp = 31;
+			else
+				ctx->st1_addexp = 31;
+		} else if (ctx->in_params.sample_format != SNDRV_PCM_FORMAT_FLOAT_LE &&
+			   ctx->out_params.sample_format == SNDRV_PCM_FORMAT_FLOAT_LE) {
+			if (ctx->st2_num_taps > 0)
+				ctx->st2_addexp -= ctx->in_params.fmt.addexp;
+			else
+				ctx->st1_addexp -= ctx->in_params.fmt.addexp;
+		}
+	}
 
 	ctx->in_filled_sample += (ctx->st1_num_taps / 2) * ctx->st1_num_exp +
 				  ctx->st2_num_taps / 2;
@@ -525,7 +614,9 @@ static int fsl_easrc_prefilter_config(struct fsl_easrc *easrc,
 		goto ctx_error;
 
 	ret = write_pf_coeff_mem(easrc, ctx_id,
-				 ctx->st1_coeff, ctx->st1_num_taps);
+				 ctx->st1_coeff,
+				 ctx->st1_num_taps,
+				 ctx->st1_addexp);
 	if (ret)
 		goto ctx_error;
 
@@ -543,6 +634,16 @@ static int fsl_easrc_prefilter_config(struct fsl_easrc *easrc,
 					 EASRC_CCE1_PF_TSEN);
 		if (ret)
 			goto ctx_error;
+		/*
+		 * Enable prefilter stage1 writeback floating point
+		 * which is used for FLOAT_LE case
+		 */
+		ret = regmap_update_bits(easrc->regmap,
+					 REG_EASRC_CCE1(ctx_id),
+					 EASRC_CCE1_PF_ST1_WBFP_MASK,
+					 EASRC_CCE1_PF_ST1_WBFP);
+		if (ret)
+			goto ctx_error;
 
 		ret = regmap_update_bits(easrc->regmap,
 					 REG_EASRC_CCE1(ctx_id),
@@ -568,7 +669,9 @@ static int fsl_easrc_prefilter_config(struct fsl_easrc *easrc,
 			goto ctx_error;
 
 		ret = write_pf_coeff_mem(easrc, ctx_id,
-					 ctx->st2_coeff, ctx->st2_num_taps);
+					 ctx->st2_coeff,
+					 ctx->st2_num_taps,
+					 ctx->st2_addexp);
 		if (ret)
 			goto ctx_error;
 	}
@@ -1166,15 +1269,19 @@ static int fsl_easrc_process_format(struct fsl_easrc *easrc,
 	switch (snd_pcm_format_width(raw_fmt)) {
 	case 16:
 		fmt->width = EASRC_WIDTH_16_BIT;
+		fmt->addexp = 15;
 		break;
 	case 20:
 		fmt->width = EASRC_WIDTH_20_BIT;
+		fmt->addexp = 19;
 		break;
 	case 24:
 		fmt->width = EASRC_WIDTH_24_BIT;
+		fmt->addexp = 23;
 		break;
 	case 32:
 		fmt->width = EASRC_WIDTH_32_BIT;
+		fmt->addexp = 31;
 		break;
 	default:
 		return -EINVAL;
@@ -1185,12 +1292,16 @@ static int fsl_easrc_process_format(struct fsl_easrc *easrc,
 		fmt->width = easrc->bps_iec958;
 		fmt->iec958 = 1;
 		fmt->floating_point = 0;
-		if (fmt->width == EASRC_WIDTH_16_BIT)
+		if (fmt->width == EASRC_WIDTH_16_BIT) {
 			fmt->sample_pos = 12;
-		else if (fmt->width == EASRC_WIDTH_20_BIT)
+			fmt->addexp = 15;
+		} else if (fmt->width == EASRC_WIDTH_20_BIT) {
 			fmt->sample_pos = 8;
-		else if (fmt->width == EASRC_WIDTH_24_BIT)
+			fmt->addexp = 19;
+		} else if (fmt->width == EASRC_WIDTH_24_BIT) {
 			fmt->sample_pos = 4;
+			fmt->addexp = 23;
+		}
 		break;
 	default:
 		break;
@@ -1625,12 +1736,6 @@ static int fsl_easrc_hw_params(struct snd_pcm_substream *substream,
 	ctx->in_params.fifo_wtmk  = 0x20;
 	ctx->out_params.fifo_wtmk = 0x20;
 
-	ret = fsl_easrc_config_context(easrc, ctx->index);
-	if (ret) {
-		dev_err(dev, "failed to config context\n");
-		return ret;
-	}
-
 	/* do only rate conversion and keep the same format for input
 	 * and output data
 	 */
@@ -1642,6 +1747,12 @@ static int fsl_easrc_hw_params(struct snd_pcm_substream *substream,
 		return ret;
 	}
 
+	ret = fsl_easrc_config_context(easrc, ctx->index);
+	if (ret) {
+		dev_err(dev, "failed to config context\n");
+		return ret;
+	}
+
 	ctx->in_params.iterations = 1;
 	ctx->in_params.group_len = ctx->channels;
 	ctx->in_params.access_len = ctx->channels;
@@ -2313,15 +2424,17 @@ static int fsl_easrc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	if (width != 16 && width != 24) {
+	if (width != 16 && width != 24 && width != 32 && width != 20) {
 		dev_warn(&pdev->dev, "unsupported width, switching to 24bit\n");
 		width = 24;
 	}
 
 	if (width == 24)
 		easrc->easrc_format = SNDRV_PCM_FORMAT_S24_LE;
-	else
+	else if (width == 16)
 		easrc->easrc_format = SNDRV_PCM_FORMAT_S16_LE;
+	else
+		easrc->easrc_format = SNDRV_PCM_FORMAT_S32_LE;
 
 	platform_set_drvdata(pdev, easrc);
 	pm_runtime_enable(&pdev->dev);
@@ -2437,13 +2550,17 @@ static int fsl_easrc_runtime_resume(struct device *dev)
 					% ctx->in_params.sample_rate != 0)
 				ctx->out_missed_sample += 1;
 
-			ret = write_pf_coeff_mem(easrc, i, ctx->st1_coeff,
-						 ctx->st1_num_taps);
+			ret = write_pf_coeff_mem(easrc, i,
+						 ctx->st1_coeff,
+						 ctx->st1_num_taps,
+						 ctx->st1_addexp);
 			if (ret)
 				goto disable_mem_clk;
 
-			ret = write_pf_coeff_mem(easrc, i, ctx->st2_coeff,
-						 ctx->st2_num_taps);
+			ret = write_pf_coeff_mem(easrc, i,
+						 ctx->st2_coeff,
+						 ctx->st2_num_taps,
+						 ctx->st2_addexp);
 			if (ret)
 				goto disable_mem_clk;
 		}
diff --git a/sound/soc/fsl/fsl_easrc.h b/sound/soc/fsl/fsl_easrc.h
index f9d836a8d525..bf31f1559a61 100644
--- a/sound/soc/fsl/fsl_easrc.h
+++ b/sound/soc/fsl/fsl_easrc.h
@@ -587,6 +587,7 @@ struct fsl_easrc_data_fmt {
 	unsigned int floating_point : 1;
 	unsigned int iec958: 1;
 	unsigned int sample_pos: 5;
+	unsigned int addexp;
 };
 
 struct fsl_easrc_io_params {
@@ -629,6 +630,8 @@ struct fsl_easrc_context {
 	u64 *st2_coeff;
 	int in_filled_sample;
 	int out_missed_sample;
+	int st1_addexp;
+	int st2_addexp;
 	void *private_data;
 };
 
diff --git a/sound/soc/fsl/fsl_easrc_m2m.c b/sound/soc/fsl/fsl_easrc_m2m.c
index bb82dbd99e55..af088d8b4975 100644
--- a/sound/soc/fsl/fsl_easrc_m2m.c
+++ b/sound/soc/fsl/fsl_easrc_m2m.c
@@ -484,37 +484,18 @@ static long fsl_easrc_ioctl_config_context(struct fsl_easrc_m2m *m2m,
 	ctx->rs_init_mode = 0x2;
 	ctx->pf_init_mode = 0x2;
 
-	ret = fsl_easrc_config_context(easrc, index);
-	if (ret) {
-		dev_err(dev, "failed to config context %d\n", ret);
+	ret = fsl_easrc_set_ctx_format(ctx,
+				       &ctx->in_params.sample_format,
+				       &ctx->out_params.sample_format);
+	if (ret)
 		return ret;
-	}
 
-	ret = fsl_easrc_process_format(easrc, &ctx->in_params.fmt,
-				 config.input_format);
-	if (ret) {
-		dev_err(dev, "input format error %d\n", ret);
-		return ret;
-	}
-
-	ret = fsl_easrc_process_format(easrc, &ctx->out_params.fmt,
-				 config.output_format);
+	ret = fsl_easrc_config_context(easrc, index);
 	if (ret) {
-		dev_err(dev, "output format error %d\n", ret);
+		dev_err(dev, "failed to config context %d\n", ret);
 		return ret;
 	}
 
-	/* FIXME - fix sample position?
-	 * if the input sample is 16-bits wide and left-justified on a
-	 * 32-bit boundary, then this register should be set to 16. If
-	 * the input sample is 16-bits wide and right-
-	 * justified on a 32-bit boundary, then this register should be
-	 * set to 0
-	 */
-	ret = fsl_easrc_set_ctx_format(ctx, NULL, NULL);
-	if (ret)
-		return ret;
-
 	ctx->in_params.iterations = 1;
 	ctx->in_params.group_len = ctx->channels;
 	ctx->in_params.access_len = ctx->channels;