From b7f73d188d977f56ca31482f41479de808aa3f1a Mon Sep 17 00:00:00 2001
From: Laxminath Kasam <lkasam@codeaurora.org>
Date: Thu, 23 Feb 2017 18:21:49 +0530
Subject: [PATCH] ASoC: wsa881x: Add retry logic for temperature read

When temperature read happens, swr logical address
query failing sometimes and next retry is successful.
Add retry logic to handle this.
Also, temperature result can be out of range,
add retry of 3 attempts with 20ms gap to handle this.

CRs-Fixed: 2003278
Change-Id: Ie1b91d181e07d293864ea82d8b09419cc32e0854
Signed-off-by: Laxminath Kasam <lkasam@codeaurora.org>
---
 sound/soc/codecs/wsa881x-temp-sensor.c | 10 ++++++++--
 sound/soc/codecs/wsa881x.c             | 24 ++++++++++++++++++------
 2 files changed, 26 insertions(+), 8 deletions(-)

diff --git a/sound/soc/codecs/wsa881x-temp-sensor.c b/sound/soc/codecs/wsa881x-temp-sensor.c
index 0079d0f6cd52..5ab0ecfdc022 100644
--- a/sound/soc/codecs/wsa881x-temp-sensor.c
+++ b/sound/soc/codecs/wsa881x-temp-sensor.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015, 2017 The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -23,7 +23,7 @@
 #define LOW_TEMP_THRESHOLD 5
 #define HIGH_TEMP_THRESHOLD 45
 #define TEMP_INVALID	0xFFFF
-
+#define WSA881X_TEMP_RETRY 3
 /*
  * wsa881x_get_temp - get wsa temperature
  * @thermal: thermal zone device
@@ -44,6 +44,7 @@ int wsa881x_get_temp(struct thermal_zone_device *thermal,
 	int temp_val;
 	int t1 = T1_TEMP;
 	int t2 = T2_TEMP;
+	u8 retry = WSA881X_TEMP_RETRY;
 
 	if (!thermal)
 		return -EINVAL;
@@ -60,6 +61,7 @@ int wsa881x_get_temp(struct thermal_zone_device *thermal,
 		pr_err("%s: pdata is NULL\n", __func__);
 		return -EINVAL;
 	}
+temp_retry:
 	if (pdata->wsa_temp_reg_read) {
 		ret = pdata->wsa_temp_reg_read(codec, &reg);
 		if (ret) {
@@ -101,6 +103,10 @@ int wsa881x_get_temp(struct thermal_zone_device *thermal,
 		printk_ratelimited("%s: T0: %d is out of range[%d, %d]\n",
 				   __func__, temp_val, LOW_TEMP_THRESHOLD,
 				   HIGH_TEMP_THRESHOLD);
+		if (retry--) {
+			msleep(20);
+			goto temp_retry;
+		}
 	}
 	if (temp)
 		*temp = temp_val;
diff --git a/sound/soc/codecs/wsa881x.c b/sound/soc/codecs/wsa881x.c
index 62547f25bfa1..6addbde34545 100644
--- a/sound/soc/codecs/wsa881x.c
+++ b/sound/soc/codecs/wsa881x.c
@@ -986,6 +986,7 @@ static int32_t wsa881x_temp_reg_read(struct snd_soc_codec *codec,
 {
 	struct wsa881x_priv *wsa881x = snd_soc_codec_get_drvdata(codec);
 	struct swr_device *dev;
+	u8 retry = WSA881X_NUM_RETRY;
 	u8 devnum = 0;
 
 	if (!wsa881x) {
@@ -994,7 +995,12 @@ static int32_t wsa881x_temp_reg_read(struct snd_soc_codec *codec,
 	}
 	dev = wsa881x->swr_slave;
 	if (dev && (wsa881x->state == WSA881X_DEV_DOWN)) {
-		if (swr_get_logical_dev_num(dev, dev->addr, &devnum)) {
+		while (swr_get_logical_dev_num(dev, dev->addr, &devnum) &&
+		       retry--) {
+			/* Retry after 1 msec delay */
+			usleep_range(1000, 1100);
+		}
+		if (retry == 0) {
 			dev_err(codec->dev,
 				"%s get devnum %d for dev addr %lx failed\n",
 				__func__, devnum, dev->addr);
@@ -1088,6 +1094,7 @@ static int wsa881x_swr_startup(struct swr_device *swr_dev)
 {
 	int ret = 0;
 	u8 devnum = 0;
+	u8 retry = WSA881X_NUM_RETRY;
 	struct wsa881x_priv *wsa881x;
 
 	wsa881x = swr_get_dev_data(swr_dev);
@@ -1102,11 +1109,16 @@ static int wsa881x_swr_startup(struct swr_device *swr_dev)
 	 * as per HW requirement.
 	 */
 	usleep_range(5000, 5010);
-	ret = swr_get_logical_dev_num(swr_dev, swr_dev->addr, &devnum);
-	if (ret) {
-		dev_dbg(&swr_dev->dev, "%s failed to get devnum, err:%d\n",
-			__func__, ret);
-		goto err;
+	while (swr_get_logical_dev_num(swr_dev, swr_dev->addr, &devnum) &&
+	       retry--) {
+		/* Retry after 1 msec delay */
+		usleep_range(1000, 1100);
+	}
+	if (retry == 0) {
+		dev_err(&swr_dev->dev,
+			"%s get devnum %d for dev addr %lx failed\n",
+			__func__, devnum, swr_dev->addr);
+		return -EINVAL;
 	}
 	swr_dev->dev_num = devnum;