From 131656a2b76c73f0bf4e0e1568a9b4daf67c5ffa Mon Sep 17 00:00:00 2001
From: Subbaraman Narayanamurthy <subbaram@codeaurora.org>
Date: Wed, 12 May 2021 19:03:17 -0700
Subject: [PATCH] hwmon: qti_amoled_ecm: Support getting sdam_index for more
 than 2 SDAMs

Currently, sdam_num is used to read SDAM_INDEX for reading samples
from each SDAM. However, it works well only when 2 SDAMs are used.
If ECM supports more than 2 SDAMs, SDAM_INDEX read for the third
SDAM (and after) would be incorrect. Fix it.

Change-Id: Ibf2219b80a7480a0fda11df974786c1d123ba492
Signed-off-by: Subbaraman Narayanamurthy <subbaram@codeaurora.org>
---
 drivers/hwmon/qti_amoled_ecm.c | 26 ++++++++++++++++++++++++--
 1 file changed, 24 insertions(+), 2 deletions(-)

diff --git a/drivers/hwmon/qti_amoled_ecm.c b/drivers/hwmon/qti_amoled_ecm.c
index 0e2374deb9a0..8713e80846e6 100644
--- a/drivers/hwmon/qti_amoled_ecm.c
+++ b/drivers/hwmon/qti_amoled_ecm.c
@@ -55,6 +55,7 @@
 
 #define ECM_SDAM0_INDEX				0x52
 #define ECM_SDAM1_INDEX				0x53
+#define ECM_SDAM2_INDEX				0x61
 
 #define ECM_MODE				0x54
  #define ECM_CONTINUOUS				0
@@ -191,6 +192,7 @@ static struct amoled_ecm_sdam_config ecm_reset_config[] = {
 	{ ECM_STATUS_CLR,	0xFF },
 	{ ECM_SDAM0_INDEX,	0x6C },
 	{ ECM_SDAM1_INDEX,	0x46 },
+	{ ECM_SDAM2_INDEX,	0x46 },
 	{ ECM_MODE,		0x00 },
 };
 
@@ -555,6 +557,27 @@ static int handle_ecm_abort(struct amoled_ecm *ecm)
 	return rc;
 }
 
+static int get_sdam_index(struct nvmem_device *nvmem, int sdam_num, u8 *index)
+{
+	unsigned int addr;
+
+	switch (sdam_num) {
+	case 0:
+		addr = ECM_SDAM0_INDEX;
+		break;
+	case 1:
+		addr = ECM_SDAM1_INDEX;
+		break;
+	case 2:
+		addr = ECM_SDAM2_INDEX;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return nvmem_device_read(nvmem, addr, 1, index);
+}
+
 static irqreturn_t sdam_full_irq_handler(int irq, void *_ecm)
 {
 	struct amoled_ecm *ecm = _ecm;
@@ -593,8 +616,7 @@ static irqreturn_t sdam_full_irq_handler(int irq, void *_ecm)
 		}
 	}
 
-	rc = nvmem_device_read(ecm->sdam[0].nvmem,
-			(ECM_SDAM0_INDEX + sdam_num), 1, &sdam_index);
+	rc = get_sdam_index(ecm->sdam[0].nvmem, sdam_num, &sdam_index);
 	if (rc < 0) {
 		pr_err("Failed to read SDAM index, rc=%d\n", rc);
 		goto irq_exit;