[PATCH] pca954x: Reset if channel select fails

[Wojciech Siudy <wojciech.siudy@xxxxxxxxx>]

Channel selection that has timed out is a symptom of mux'es I2C
subsystem failure. Without sending reset pulse the POR of entire
system would be needed to restore the communication.

Signed-off-by: siudy <wojciech.siudy@xxxxxxxxx>
---
 drivers/i2c/muxes/i2c-mux-pca954x.c | 30 +++++++++++++++++++++--------
 1 file changed, 22 insertions(+), 8 deletions(-)

diff --git a/drivers/i2c/muxes/i2c-mux-pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index 6f8401825..0034e663b 100644
--- a/drivers/i2c/muxes/i2c-mux-pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -316,6 +316,22 @@ static u8 pca954x_regval(struct pca954x *data, u8 chan)
 		return 1 << chan;
 }
 
+static void pca954x_reset_deassert(struct pca954x *data)
+{
+	if (data->reset_cont)
+		reset_control_deassert(data->reset_cont);
+	else
+		gpiod_set_value_cansleep(data->reset_gpio, 0);
+}
+
+static void pca954x_reset_assert(struct pca954x *data)
+{
+	if (data->reset_cont)
+		reset_control_assert(data->reset_cont);
+	else
+		gpiod_set_value_cansleep(data->reset_gpio, 1);
+}
+
 static int pca954x_select_chan(struct i2c_mux_core *muxc, u32 chan)
 {
 	struct pca954x *data = i2c_mux_priv(muxc);
@@ -329,6 +345,12 @@ static int pca954x_select_chan(struct i2c_mux_core *muxc, u32 chan)
 		ret = pca954x_reg_write(muxc->parent, client, regval);
 		data->last_chan = ret < 0 ? 0 : regval;
 	}
+	if (ret == -ETIMEDOUT && (data->reset_cont || data->reset_gpio)) {
+		dev_warn(&client->dev, "channel select failed, resetting...\n");
+		pca954x_reset_assert(data);
+		udelay(10);
+		pca954x_reset_deassert(data);
+	}
 
 	return ret;
 }
@@ -543,14 +565,6 @@ static int pca954x_get_reset(struct device *dev, struct pca954x *data)
 	return 0;
 }
 
-static void pca954x_reset_deassert(struct pca954x *data)
-{
-	if (data->reset_cont)
-		reset_control_deassert(data->reset_cont);
-	else
-		gpiod_set_value_cansleep(data->reset_gpio, 0);
-}
-
 /*
  * I2C init/probing/exit functions
  */
-- 
2.34.1