From c03164c8a24b5c779f36244a5a4c9d8e7a35c4f4 Mon Sep 17 00:00:00 2001
From: Vijay Kumar Maddula <quic_vmaddula@quicinc.com>
Date: Thu, 15 Dec 2022 19:47:20 +0530
Subject: [PATCH] ASoC: rouleur-mbhc: Enhance impedance calculation logic

When headset insert, calculate impedance for multiple times
and stop calculating if the result is within the threshold.
Modify register write sequence as per rouleur software sequence.

Change-Id: Id035304963d0822f758d7a527a712aca500a8a78
Signed-off-by: Vijay Kumar Maddula <quic_vmaddula@quicinc.com>
---
 asoc/codecs/rouleur/rouleur-mbhc.c | 69 +++++++++++++++++-------------
 1 file changed, 40 insertions(+), 29 deletions(-)

diff --git a/asoc/codecs/rouleur/rouleur-mbhc.c b/asoc/codecs/rouleur/rouleur-mbhc.c
index 6d4b4db296c3..db0f23b8465d 100644
--- a/asoc/codecs/rouleur/rouleur-mbhc.c
+++ b/asoc/codecs/rouleur/rouleur-mbhc.c
@@ -478,9 +478,6 @@ static void rouleur_mbhc_zdet_start(struct snd_soc_component *component,
 	/* HPHL pull down switch to force OFF */
 	regmap_update_bits(rouleur->regmap,
 			  ROULEUR_ANA_HPHPA_CNP_CTL_2, 0x30, 0x00);
-	/* Averaging enable for reliable results */
-	regmap_update_bits(rouleur->regmap,
-			   ROULEUR_ANA_MBHC_ZDET_ANA_CTL, 0x80, 0x80);
 	/* ZDET left measurement enable */
 	regmap_update_bits(rouleur->regmap,
 			   ROULEUR_ANA_MBHC_ZDET, 0x80, 0x80);
@@ -489,8 +486,6 @@ static void rouleur_mbhc_zdet_start(struct snd_soc_component *component,
 
 	regmap_update_bits(rouleur->regmap,
 			   ROULEUR_ANA_MBHC_ZDET, 0x80, 0x00);
-	regmap_update_bits(rouleur->regmap,
-			   ROULEUR_ANA_MBHC_ZDET_ANA_CTL, 0x80, 0x00);
 	regmap_update_bits(rouleur->regmap,
 			  ROULEUR_ANA_HPHPA_CNP_CTL_2, 0x30, 0x20);
 
@@ -502,9 +497,6 @@ z_right:
 	/* HPHR pull down switch to force OFF */
 	regmap_update_bits(rouleur->regmap,
 			  ROULEUR_ANA_HPHPA_CNP_CTL_2, 0x0C, 0x00);
-	/* Averaging enable for reliable results */
-	regmap_update_bits(rouleur->regmap,
-			   ROULEUR_ANA_MBHC_ZDET_ANA_CTL, 0x80, 0x80);
 	/* ZDET right measurement enable */
 	regmap_update_bits(rouleur->regmap,
 			   ROULEUR_ANA_MBHC_ZDET, 0x40, 0x40);
@@ -514,8 +506,6 @@ z_right:
 
 	regmap_update_bits(rouleur->regmap,
 			   ROULEUR_ANA_MBHC_ZDET, 0x40, 0x00);
-	regmap_update_bits(rouleur->regmap,
-			   ROULEUR_ANA_MBHC_ZDET_ANA_CTL, 0x80, 0x00);
 	regmap_update_bits(rouleur->regmap,
 			  ROULEUR_ANA_HPHPA_CNP_CTL_2, 0x0C, 0x08);
 
@@ -527,22 +517,39 @@ static void rouleur_mbhc_impedance_fn(struct snd_soc_component *component,
 				      int32_t *zl, int32_t *zr)
 {
 	int i;
-	for (i = 0; i < IMPED_NUM_RETRY; i++) {
-		/* Start of left ch impedance calculation */
-		rouleur_mbhc_zdet_start(component, z1L, NULL);
-		if ((*z1L == ROULEUR_ZDET_FLOATING_IMPEDANCE) ||
-		    (*z1L > ROULEUR_ZDET_VAL_100K))
-			*zl = ROULEUR_ZDET_FLOATING_IMPEDANCE;
-		else
-			*zl = *z1L/1000;
+	bool is_zl_calculted = false;
+	bool is_zr_calculted = false;
 
-		/* Start of right ch impedance calculation */
-		rouleur_mbhc_zdet_start(component, NULL, z1R);
-		if ((*z1R == ROULEUR_ZDET_FLOATING_IMPEDANCE) ||
-		    (*z1R > ROULEUR_ZDET_VAL_100K))
-			*zr = ROULEUR_ZDET_FLOATING_IMPEDANCE;
-		else
-			*zr = *z1R/1000;
+	/*
+	 * Calculate impedance for multiple times until IMPED_NUM_RETRY
+	 * stop calculating if the result is within the threshold
+	 */
+	for (i = 0; i < IMPED_NUM_RETRY; i++) {
+		if (!is_zl_calculted) {
+			/* Start of left ch impedance calculation */
+			rouleur_mbhc_zdet_start(component, z1L, NULL);
+			if ((*z1L == ROULEUR_ZDET_FLOATING_IMPEDANCE) ||
+				(*z1L > ROULEUR_ZDET_VAL_100K))
+				*zl = ROULEUR_ZDET_FLOATING_IMPEDANCE;
+			else {
+				*zl = *z1L/1000;
+				is_zl_calculted = true;
+			}
+		}
+		if (!is_zr_calculted) {
+			/* Start of right ch impedance calculation */
+			rouleur_mbhc_zdet_start(component, NULL, z1R);
+			if ((*z1R == ROULEUR_ZDET_FLOATING_IMPEDANCE) ||
+				(*z1R > ROULEUR_ZDET_VAL_100K))
+				*zr = ROULEUR_ZDET_FLOATING_IMPEDANCE;
+			else {
+				*zr = *z1R/1000;
+				is_zr_calculted = true;
+			}
+		}
+
+		if (is_zl_calculted && is_zr_calculted)
+			break;
 	}
 
 	dev_dbg(component->dev, "%s: impedance on HPH_L = %d(ohms)\n",
@@ -600,12 +607,16 @@ static void rouleur_wcd_mbhc_calc_impedance(struct wcd_mbhc *mbhc, uint32_t *zl,
 	/* 1ms delay needed after disable surge protection */
 	usleep_range(1000, 1010);
 
-	/*
-	 * Call impedance detection routine multiple times
-	 * in order to avoid wrong impedance values.
-	 */
+	/* Averaging enable for reliable impedance results */
+	regmap_update_bits(rouleur->regmap,
+			   ROULEUR_ANA_MBHC_ZDET_ANA_CTL, 0x80, 0x80);
+
 	rouleur_mbhc_impedance_fn(component, &z1L, &z1R, zl, zr);
 
+	/* Disable averaging after impedance calculation */
+	regmap_update_bits(rouleur->regmap,
+			   ROULEUR_ANA_MBHC_ZDET_ANA_CTL, 0x80, 0x00);
+
 	/* Mono/stereo detection */
 	if ((*zl == ROULEUR_ZDET_FLOATING_IMPEDANCE) &&
 		(*zr == ROULEUR_ZDET_FLOATING_IMPEDANCE)) {