[PATCH linux dev-4.7 4/5] drivers/fsi: Add FSI bus error cleanup

[Christopher Bostic <cbostic@linux.vnet.ibm.com>]

On detect of bus fails attempt to clean them up and try the
command again.

Signed-off-by: Christopher Bostic <cbostic@linux.vnet.ibm.com>
---
 drivers/fsi/fsi-master-gpio.c | 100 +++++++++++++++++++++++++++++++++++-------
 1 file changed, 85 insertions(+), 15 deletions(-)

diff --git a/drivers/fsi/fsi-master-gpio.c b/drivers/fsi/fsi-master-gpio.c
index 92af53d..bb3983c 100644
--- a/drivers/fsi/fsi-master-gpio.c
+++ b/drivers/fsi/fsi-master-gpio.c
@@ -67,8 +67,16 @@
 
 #define	FSI_MASTER_GPIO_LINK_SIZE	0x00800000
 
+static int fsi_master_gpio_read(struct fsi_master *_master, int link,
+		uint8_t slave, uint32_t addr, void *val, size_t size);
+static int fsi_master_gpio_write(struct fsi_master *_master, int link,
+		uint8_t slave, uint32_t addr, const void *val, size_t size);
+static int fsi_master_gpio_break(struct fsi_master *_master, int link);
+
 static DEFINE_SPINLOCK(fsi_gpio_cmd_lock);	/* lock around fsi commands */
 
+uint8_t in_err_cleanup;
+
 struct fsi_master_gpio {
 	struct fsi_master	master;
 	struct gpio_desc	*gpio_clk;
@@ -358,24 +366,85 @@ static void build_abs_ar_command(struct fsi_gpio_msg *cmd, uint64_t mode,
 	cmd->msg >>= (64 - cmd->bits);
 }
 
+/*
+ * Clean up the FSI bus path on detection of error.
+ * todo: get rid of magic numbers
+ */
+static void fsi_master_gpio_handle_error(struct fsi_master_gpio *master,
+					uint32_t addr)
+{
+	uint32_t smode = 0xa0ff0100;
+
+	/* Avoid nested error handling */
+	if (in_err_cleanup)
+		return;
+
+	in_err_cleanup = 1;
+	fsi_master_gpio_break(&master->master, 0);
+	udelay(200);
+
+	/* Set slave ID in smode */
+	fsi_master_gpio_write(&master->master, 0, 0, 0x800, &smode,
+				sizeof(smode));
+
+	/* Reset the MFSI bridge */
+	smode = 0xc0000000;
+	fsi_master_gpio_write(&master->master, 0, 0, 0x35D0, &smode,
+				sizeof(smode));
+
+	if (addr >= 0x80000) {
+
+		/*
+		 * Break to hub link 1
+		 * todo: make this handle more arbitrary link topologies
+		 */
+		smode = 0xc0de0000;
+		fsi_master_gpio_write(&master->master, 0, 0, 0x100004, &smode,
+					sizeof(smode));
+
+		smode = 0xa0ff0100;
+		fsi_master_gpio_write(&master->master, 0, 0, 0x100800, &smode,
+					sizeof(smode));
+	}
+	in_err_cleanup = 0;
+}
+
+static int send_command(struct fsi_master_gpio *master,
+			struct fsi_gpio_msg *cmd, uint8_t expected,
+			size_t size, void *val)
+{
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&fsi_gpio_cmd_lock, flags);
+	serial_out(master, cmd);
+	echo_delay(master);
+	rc = poll_for_response(master, expected, size, val);
+	spin_unlock_irqrestore(&fsi_gpio_cmd_lock, flags);
+
+	return rc;
+}
+
 static int fsi_master_gpio_read(struct fsi_master *_master, int link,
 		uint8_t slave, uint32_t addr, void *val, size_t size)
 {
 	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
 	struct fsi_gpio_msg cmd;
 	int rc;
-	unsigned long flags;
 
 	if (link != 0)
 		return -ENODEV;
 
 	build_abs_ar_command(&cmd, FSI_GPIO_CMD_READ, slave, addr, size, NULL);
-
-	spin_lock_irqsave(&fsi_gpio_cmd_lock, flags);
-	serial_out(master, &cmd);
-	echo_delay(master);
-	rc = poll_for_response(master, FSI_GPIO_RESP_ACKD, size, val);
-	spin_unlock_irqrestore(&fsi_gpio_cmd_lock, flags);
+	rc = send_command(master, &cmd, FSI_GPIO_RESP_ACKD, size, val);
+	if (rc) {
+		fsi_master_gpio_handle_error(master, addr);
+
+		/* Try again */
+		rc = send_command(master, &cmd, FSI_GPIO_RESP_ACKD, size, val);
+		if (rc)
+			fsi_master_gpio_handle_error(master, addr);
+	}
 
 	return rc;
 }
@@ -386,18 +455,20 @@ static int fsi_master_gpio_write(struct fsi_master *_master, int link,
 	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
 	struct fsi_gpio_msg cmd;
 	int rc;
-	unsigned long flags;
 
 	if (link != 0)
 		return -ENODEV;
 
 	build_abs_ar_command(&cmd, FSI_GPIO_CMD_WRITE, slave, addr, size, val);
-
-	spin_lock_irqsave(&fsi_gpio_cmd_lock, flags);
-	serial_out(master, &cmd);
-	echo_delay(master);
-	rc = poll_for_response(master, FSI_GPIO_RESP_ACK, size, NULL);
-	spin_unlock_irqrestore(&fsi_gpio_cmd_lock, flags);
+	rc = send_command(master, &cmd, FSI_GPIO_RESP_ACK, size, NULL);
+	if (rc) {
+		fsi_master_gpio_handle_error(master, addr);
+
+		/* Try again */
+		rc = send_command(master, &cmd, FSI_GPIO_RESP_ACK, size, NULL);
+		if (rc)
+			fsi_master_gpio_handle_error(master, addr);
+	}
 
 	return rc;
 }
@@ -480,7 +551,6 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 	master = devm_kzalloc(&pdev->dev, sizeof(*master), GFP_KERNEL);
 	if (!master)
 		return -ENOMEM;
-	master->master.dev = &pdev->dev;
 
 	gpio = devm_gpiod_get(&pdev->dev, "clock", 0);
 	if (IS_ERR(gpio)) {
-- 
1.8.2.2