diff --git a/arch/arm/boot/dts/aspeed-bmc-opp-romulus.dts b/arch/arm/boot/dts/aspeed-bmc-opp-romulus.dts
index 389f5f83bef9..0b9b37d4d6ef 100644
--- a/arch/arm/boot/dts/aspeed-bmc-opp-romulus.dts
+++ b/arch/arm/boot/dts/aspeed-bmc-opp-romulus.dts
@@ -52,6 +52,7 @@
 		compatible = "fsi-master-gpio", "fsi-master";
 		#address-cells = <2>;
 		#size-cells = <0>;
+		no-gpio-delays;
 
 		clock-gpios = <&gpio ASPEED_GPIO(AA, 0) GPIO_ACTIVE_HIGH>;
 		data-gpios = <&gpio ASPEED_GPIO(AA, 2) GPIO_ACTIVE_HIGH>;
diff --git a/arch/arm/boot/dts/aspeed-bmc-opp-witherspoon.dts b/arch/arm/boot/dts/aspeed-bmc-opp-witherspoon.dts
index 78a511e6e482..656036106001 100644
--- a/arch/arm/boot/dts/aspeed-bmc-opp-witherspoon.dts
+++ b/arch/arm/boot/dts/aspeed-bmc-opp-witherspoon.dts
@@ -153,6 +153,7 @@
 		compatible = "fsi-master-gpio", "fsi-master";
 		#address-cells = <2>;
 		#size-cells = <0>;
+		no-gpio-delays;
 
 		clock-gpios = <&gpio ASPEED_GPIO(AA, 0) GPIO_ACTIVE_HIGH>;
 		data-gpios = <&gpio ASPEED_GPIO(E, 0) GPIO_ACTIVE_HIGH>;
diff --git a/arch/arm/boot/dts/aspeed-bmc-opp-zaius.dts b/arch/arm/boot/dts/aspeed-bmc-opp-zaius.dts
index ccbf645ab84d..2c5aa90a546d 100644
--- a/arch/arm/boot/dts/aspeed-bmc-opp-zaius.dts
+++ b/arch/arm/boot/dts/aspeed-bmc-opp-zaius.dts
@@ -91,6 +91,7 @@
 		compatible = "fsi-master-gpio", "fsi-master";
 		#address-cells = <2>;
 		#size-cells = <0>;
+		no-gpio-delays;
 
 		trans-gpios = <&gpio ASPEED_GPIO(O, 6) GPIO_ACTIVE_HIGH>;
 		enable-gpios = <&gpio ASPEED_GPIO(D, 0) GPIO_ACTIVE_HIGH>;
diff --git a/drivers/fsi/Kconfig b/drivers/fsi/Kconfig
index a326ed663d3c..24f84a96b8b9 100644
--- a/drivers/fsi/Kconfig
+++ b/drivers/fsi/Kconfig
@@ -32,4 +32,11 @@ config FSI_SCOM
 	---help---
 	This option enables an FSI based SCOM device driver.
 
+config FSI_SBEFIFO
+	tristate "SBEFIFO FSI client device driver"
+	---help---
+	This option enables an FSI based SBEFIFO device driver. The SBEFIFO is
+	a pipe-like FSI device for communicating with the self boot engine
+	(SBE) on POWER processors.
+
 endif
diff --git a/drivers/fsi/Makefile b/drivers/fsi/Makefile
index 65eb99dfafdb..851182e1cd9e 100644
--- a/drivers/fsi/Makefile
+++ b/drivers/fsi/Makefile
@@ -3,3 +3,4 @@ obj-$(CONFIG_FSI) += fsi-core.o
 obj-$(CONFIG_FSI_MASTER_HUB) += fsi-master-hub.o
 obj-$(CONFIG_FSI_MASTER_GPIO) += fsi-master-gpio.o
 obj-$(CONFIG_FSI_SCOM) += fsi-scom.o
+obj-$(CONFIG_FSI_SBEFIFO) += fsi-sbefifo.o
diff --git a/drivers/fsi/fsi-master-gpio.c b/drivers/fsi/fsi-master-gpio.c
index 3f487449a277..084e9da8d151 100644
--- a/drivers/fsi/fsi-master-gpio.c
+++ b/drivers/fsi/fsi-master-gpio.c
@@ -8,11 +8,11 @@
 #include <linux/fsi.h>
 #include <linux/gpio/consumer.h>
 #include <linux/io.h>
+#include <linux/irqflags.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/spinlock.h>
 
 #include "fsi-master.h"
 
@@ -22,45 +22,48 @@
 #define	FSI_BREAK_CLOCKS	256	/* Number of clocks to issue break */
 #define	FSI_POST_BREAK_CLOCKS	16000	/* Number clocks to set up cfam */
 #define	FSI_INIT_CLOCKS		5000	/* Clock out any old data */
+#define	FSI_GPIO_DPOLL_CLOCKS	50      /* < 21 will cause slave to hang */
+#define	FSI_GPIO_EPOLL_CLOCKS	50      /* Number of clocks for E_POLL retry */
 #define	FSI_GPIO_STD_DELAY	10	/* Standard GPIO delay in nS */
 					/* todo: adjust down as low as */
 					/* possible or eliminate */
+#define FSI_CRC_ERR_RETRIES	10
+
 #define	FSI_GPIO_CMD_DPOLL      0x2
+#define	FSI_GPIO_CMD_EPOLL      0x3
 #define	FSI_GPIO_CMD_TERM	0x3f
 #define FSI_GPIO_CMD_ABS_AR	0x4
+#define FSI_GPIO_CMD_REL_AR	0x5
+#define FSI_GPIO_CMD_SAME_AR	0x3	/* but only a 2-bit opcode... */
 
-#define	FSI_GPIO_DPOLL_CLOCKS	100      /* < 21 will cause slave to hang */
-
-/* Bus errors */
-#define	FSI_GPIO_ERR_BUSY	1	/* Slave stuck in busy state */
+/* Slave responses */
+#define	FSI_GPIO_RESP_ACK	0	/* Success */
+#define	FSI_GPIO_RESP_BUSY	1	/* Slave busy */
 #define	FSI_GPIO_RESP_ERRA	2	/* Any (misc) Error */
 #define	FSI_GPIO_RESP_ERRC	3	/* Slave reports master CRC error */
-#define	FSI_GPIO_MTOE		4	/* Master time out error */
-#define	FSI_GPIO_CRC_INVAL	5	/* Master reports slave CRC error */
 
-/* Normal slave responses */
-#define	FSI_GPIO_RESP_BUSY	1
-#define	FSI_GPIO_RESP_ACK	0
-#define	FSI_GPIO_RESP_ACKD	4
-
-#define	FSI_GPIO_MAX_BUSY	100
+#define	FSI_GPIO_MAX_BUSY	200
 #define	FSI_GPIO_MTOE_COUNT	1000
 #define	FSI_GPIO_DRAIN_BITS	20
 #define	FSI_GPIO_CRC_SIZE	4
 #define	FSI_GPIO_MSG_ID_SIZE		2
 #define	FSI_GPIO_MSG_RESPID_SIZE	2
-#define	FSI_GPIO_PRIME_SLAVE_CLOCKS	100
+#define	FSI_GPIO_PRIME_SLAVE_CLOCKS	20
+
+#define LAST_ADDR_INVALID		0x1
 
 struct fsi_master_gpio {
 	struct fsi_master	master;
 	struct device		*dev;
-	spinlock_t		cmd_lock;	/* Lock for commands */
+	struct mutex		cmd_lock;	/* mutex for command ordering */
 	struct gpio_desc	*gpio_clk;
 	struct gpio_desc	*gpio_data;
 	struct gpio_desc	*gpio_trans;	/* Voltage translator */
 	struct gpio_desc	*gpio_enable;	/* FSI enable */
 	struct gpio_desc	*gpio_mux;	/* Mux control */
 	bool			external_mode;
+	bool			no_delays;
+	uint32_t		last_addr;
 };
 
 #define CREATE_TRACE_POINTS
@@ -78,19 +81,31 @@ static void clock_toggle(struct fsi_master_gpio *master, int count)
 	int i;
 
 	for (i = 0; i < count; i++) {
-		ndelay(FSI_GPIO_STD_DLY);
+		if (!master->no_delays)
+			ndelay(FSI_GPIO_STD_DLY);
 		gpiod_set_value(master->gpio_clk, 0);
-		ndelay(FSI_GPIO_STD_DLY);
+		if (!master->no_delays)
+			ndelay(FSI_GPIO_STD_DLY);
 		gpiod_set_value(master->gpio_clk, 1);
 	}
 }
 
-static int sda_in(struct fsi_master_gpio *master)
+static int sda_clock_in(struct fsi_master_gpio *master)
 {
 	int in;
 
-	ndelay(FSI_GPIO_STD_DLY);
+	if (!master->no_delays)
+		ndelay(FSI_GPIO_STD_DLY);
+	gpiod_set_value(master->gpio_clk, 0);
+
+	/* Dummy read to feed the synchronizers */
+	gpiod_get_value(master->gpio_data);
+
+	/* Actual data read */
 	in = gpiod_get_value(master->gpio_data);
+	if (!master->no_delays)
+		ndelay(FSI_GPIO_STD_DLY);
+	gpiod_set_value(master->gpio_clk, 1);
 	return in ? 1 : 0;
 }
 
@@ -125,8 +140,7 @@ static void serial_in(struct fsi_master_gpio *master, struct fsi_gpio_msg *msg,
 	set_sda_input(master);
 
 	for (bit = 0; bit < num_bits; bit++) {
-		clock_toggle(master, 1);
-		in_bit = sda_in(master);
+		in_bit = sda_clock_in(master);
 		msg->msg <<= 1;
 		msg->msg |= ~in_bit & 0x1;	/* Data is active low */
 	}
@@ -191,22 +205,89 @@ static void msg_push_crc(struct fsi_gpio_msg *msg)
 	msg_push_bits(msg, crc, 4);
 }
 
-/*
- * Encode an Absolute Address command
- */
-static void build_abs_ar_command(struct fsi_gpio_msg *cmd,
-		uint8_t id, uint32_t addr, size_t size, const void *data)
+static bool check_same_address(struct fsi_master_gpio *master, int id,
+		uint32_t addr)
 {
+	/* this will also handle LAST_ADDR_INVALID */
+	return master->last_addr == (((id & 0x3) << 21) | (addr & ~0x3));
+}
+
+static bool check_relative_address(struct fsi_master_gpio *master, int id,
+		uint32_t addr, uint32_t *rel_addrp)
+{
+	uint32_t last_addr = master->last_addr;
+	int32_t rel_addr;
+
+	if (last_addr == LAST_ADDR_INVALID)
+		return false;
+
+	/* We may be in 23-bit addressing mode, which uses the id as the
+	 * top two address bits. So, if we're referencing a different ID,
+	 * use absolute addresses.
+	 */
+	if (((last_addr >> 21) & 0x3) != id)
+		return false;
+
+	/* remove the top two bits from any 23-bit addressing */
+	last_addr &= (1 << 21) - 1;
+
+	/* We know that the addresses are limited to 21 bits, so this won't
+	 * overflow the signed rel_addr */
+	rel_addr = addr - last_addr;
+	if (rel_addr > 255 || rel_addr < -256)
+		return false;
+
+	*rel_addrp = (uint32_t)rel_addr;
+
+	return true;
+}
+
+static void last_address_update(struct fsi_master_gpio *master,
+		int id, bool valid, uint32_t addr)
+{
+	if (!valid)
+		master->last_addr = LAST_ADDR_INVALID;
+	else
+		master->last_addr = ((id & 0x3) << 21) | (addr & ~0x3);
+}
+
+/*
+ * Encode an Absolute/Relative/Same Address command
+ */
+static void build_ar_command(struct fsi_master_gpio *master,
+		struct fsi_gpio_msg *cmd, uint8_t id,
+		uint32_t addr, size_t size, const void *data)
+{
+	int i, addr_bits, opcode_bits;
 	bool write = !!data;
-	uint8_t ds;
-	int i;
+	uint8_t ds, opcode;
+	uint32_t rel_addr;
 
 	cmd->bits = 0;
 	cmd->msg = 0;
 
-	msg_push_bits(cmd, id, 2);
-	msg_push_bits(cmd, FSI_GPIO_CMD_ABS_AR, 3);
-	msg_push_bits(cmd, write ? 0 : 1, 1);
+	/* we have 21 bits of address max */
+	addr &= ((1 << 21) - 1);
+
+	/* cmd opcodes are variable length - SAME_AR is only two bits */
+	opcode_bits = 3;
+
+	if (check_same_address(master, id, addr)) {
+		/* we still address the byte offset within the word */
+		addr_bits = 2;
+		opcode_bits = 2;
+		opcode = FSI_GPIO_CMD_SAME_AR;
+
+	} else if (check_relative_address(master, id, addr, &rel_addr)) {
+		/* 8 bits plus sign */
+		addr_bits = 9;
+		addr = rel_addr;
+		opcode = FSI_GPIO_CMD_REL_AR;
+
+	} else {
+		addr_bits = 21;
+		opcode = FSI_GPIO_CMD_ABS_AR;
+	}
 
 	/*
 	 * The read/write size is encoded in the lower bits of the address
@@ -223,7 +304,10 @@ static void build_abs_ar_command(struct fsi_gpio_msg *cmd,
 	if (size == 4)
 		addr |= 1;
 
-	msg_push_bits(cmd, addr & ((1 << 21) - 1), 21);
+	msg_push_bits(cmd, id, 2);
+	msg_push_bits(cmd, opcode, opcode_bits);
+	msg_push_bits(cmd, write ? 0 : 1, 1);
+	msg_push_bits(cmd, addr, addr_bits);
 	msg_push_bits(cmd, ds, 1);
 	for (i = 0; write && i < size; i++)
 		msg_push_bits(cmd, ((uint8_t *)data)[i], 8);
@@ -241,6 +325,16 @@ static void build_dpoll_command(struct fsi_gpio_msg *cmd, uint8_t slave_id)
 	msg_push_crc(cmd);
 }
 
+static void build_epoll_command(struct fsi_gpio_msg *cmd, uint8_t slave_id)
+{
+	cmd->bits = 0;
+	cmd->msg = 0;
+
+	msg_push_bits(cmd, slave_id, 2);
+	msg_push_bits(cmd, FSI_GPIO_CMD_EPOLL, 3);
+	msg_push_crc(cmd);
+}
+
 static void echo_delay(struct fsi_master_gpio *master)
 {
 	set_sda_output(master, 1);
@@ -258,22 +352,22 @@ static void build_term_command(struct fsi_gpio_msg *cmd, uint8_t slave_id)
 }
 
 /*
- * Store information on master errors so handler can detect and clean
- * up the bus
+ * Note: callers rely specifically on this returning -EAGAIN for
+ * a CRC error detected in the response. Use other error code
+ * for other situations. It will be converted to something else
+ * higher up the stack before it reaches userspace.
  */
-static void fsi_master_gpio_error(struct fsi_master_gpio *master, int error)
-{
-
-}
-
 static int read_one_response(struct fsi_master_gpio *master,
 		uint8_t data_size, struct fsi_gpio_msg *msgp, uint8_t *tagp)
 {
 	struct fsi_gpio_msg msg;
-	uint8_t id, tag;
+	unsigned long flags;
 	uint32_t crc;
+	uint8_t tag;
 	int i;
 
+	local_irq_save(flags);
+
 	/* wait for the start bit */
 	for (i = 0; i < FSI_GPIO_MTOE_COUNT; i++) {
 		msg.bits = 0;
@@ -285,8 +379,8 @@ static int read_one_response(struct fsi_master_gpio *master,
 	if (i == FSI_GPIO_MTOE_COUNT) {
 		dev_dbg(master->dev,
 			"Master time out waiting for response\n");
-		fsi_master_gpio_error(master, FSI_GPIO_MTOE);
-		return -EIO;
+		local_irq_restore(flags);
+		return -ETIMEDOUT;
 	}
 
 	msg.bits = 0;
@@ -295,7 +389,6 @@ static int read_one_response(struct fsi_master_gpio *master,
 	/* Read slave ID & response tag */
 	serial_in(master, &msg, 4);
 
-	id = (msg.msg >> FSI_GPIO_MSG_RESPID_SIZE) & 0x3;
 	tag = msg.msg & 0x3;
 
 	/* If we have an ACK and we're expecting data, clock the data in too */
@@ -305,13 +398,18 @@ static int read_one_response(struct fsi_master_gpio *master,
 	/* read CRC */
 	serial_in(master, &msg, FSI_GPIO_CRC_SIZE);
 
+	local_irq_restore(flags);
+
 	/* we have a whole message now; check CRC */
 	crc = crc4(0, 1, 1);
 	crc = crc4(crc, msg.msg, msg.bits);
 	if (crc) {
-		dev_dbg(master->dev, "ERR response CRC\n");
-		fsi_master_gpio_error(master, FSI_GPIO_CRC_INVAL);
-		return -EIO;
+		/* Check if it's all 1's, that probably means the host is off */
+		if (((~msg.msg) & ((1ull << msg.bits) - 1)) == 0)
+			return -ENODEV;
+		dev_dbg(master->dev, "ERR response CRC msg: 0x%016llx (%d bits)\n",
+			msg.msg, msg.bits);
+		return -EAGAIN;
 	}
 
 	if (msgp)
@@ -325,12 +423,16 @@ static int read_one_response(struct fsi_master_gpio *master,
 static int issue_term(struct fsi_master_gpio *master, uint8_t slave)
 {
 	struct fsi_gpio_msg cmd;
+	unsigned long flags;
 	uint8_t tag;
 	int rc;
 
 	build_term_command(&cmd, slave);
+
+	local_irq_save(flags);
 	serial_out(master, &cmd);
 	echo_delay(master);
+	local_irq_restore(flags);
 
 	rc = read_one_response(master, 0, NULL, &tag);
 	if (rc < 0) {
@@ -350,13 +452,36 @@ static int poll_for_response(struct fsi_master_gpio *master,
 {
 	struct fsi_gpio_msg response, cmd;
 	int busy_count = 0, rc, i;
+	unsigned long flags;
 	uint8_t tag;
 	uint8_t *data_byte = data;
-
+	int crc_err_retries = 0;
 retry:
 	rc = read_one_response(master, size, &response, &tag);
-	if (rc)
-		return rc;
+
+	/* Handle retries on CRC errors */
+	if (rc == -EAGAIN) {
+		/* Too many retries ? */
+		if (crc_err_retries++ > FSI_CRC_ERR_RETRIES) {
+			/*
+			 * Pass it up as a -EIO otherwise upper level will retry
+			 * the whole command which isn't what we want here.
+			 */
+			rc = -EIO;
+			goto fail;
+		}
+		dev_dbg(master->dev,
+			 "CRC error retry %d\n", crc_err_retries);
+		trace_fsi_master_gpio_crc_rsp_error(master);
+		build_epoll_command(&cmd, slave);
+		local_irq_save(flags);
+		clock_zeros(master, FSI_GPIO_EPOLL_CLOCKS);
+		serial_out(master, &cmd);
+		echo_delay(master);
+		local_irq_restore(flags);
+		goto retry;
+	} else if (rc)
+		goto fail;
 
 	switch (tag) {
 	case FSI_GPIO_RESP_ACK:
@@ -378,52 +503,81 @@ retry:
 		 * d-poll, not indicated in the hardware protocol
 		 * spec. < 20 clocks causes slave to hang, 21 ok.
 		 */
-		clock_zeros(master, FSI_GPIO_DPOLL_CLOCKS);
 		if (busy_count++ < FSI_GPIO_MAX_BUSY) {
 			build_dpoll_command(&cmd, slave);
+			local_irq_save(flags);
+			clock_zeros(master, FSI_GPIO_DPOLL_CLOCKS);
 			serial_out(master, &cmd);
 			echo_delay(master);
+			local_irq_restore(flags);
 			goto retry;
 		}
 		dev_warn(master->dev,
 			"ERR slave is stuck in busy state, issuing TERM\n");
+		local_irq_save(flags);
+		clock_zeros(master, FSI_GPIO_DPOLL_CLOCKS);
+		local_irq_restore(flags);
 		issue_term(master, slave);
 		rc = -EIO;
 		break;
 
 	case FSI_GPIO_RESP_ERRA:
-	case FSI_GPIO_RESP_ERRC:
-		dev_dbg(master->dev, "ERR%c received: 0x%x\n",
-			tag == FSI_GPIO_RESP_ERRA ? 'A' : 'C',
-			(int)response.msg);
-		fsi_master_gpio_error(master, response.msg);
+		dev_dbg(master->dev, "ERRA received: 0x%x\n", (int)response.msg);
 		rc = -EIO;
 		break;
+	case FSI_GPIO_RESP_ERRC:
+		dev_dbg(master->dev, "ERRC received: 0x%x\n", (int)response.msg);
+		trace_fsi_master_gpio_crc_cmd_error(master);
+		rc = -EAGAIN;
+		break;
 	}
 
+	if (busy_count > 0)
+		trace_fsi_master_gpio_poll_response_busy(master, busy_count);
+ fail:
 	/* Clock the slave enough to be ready for next operation */
+	local_irq_save(flags);
 	clock_zeros(master, FSI_GPIO_PRIME_SLAVE_CLOCKS);
+	local_irq_restore(flags);
+
 	return rc;
 }
 
+static int send_request(struct fsi_master_gpio *master,
+		struct fsi_gpio_msg *cmd)
+{
+	unsigned long flags;
+
+	if (master->external_mode)
+		return -EBUSY;
+
+	local_irq_save(flags);
+	serial_out(master, cmd);
+	echo_delay(master);
+	local_irq_restore(flags);
+
+	return 0;
+}
+
 static int fsi_master_gpio_xfer(struct fsi_master_gpio *master, uint8_t slave,
 		struct fsi_gpio_msg *cmd, size_t resp_len, void *resp)
 {
-	unsigned long flags;
-	int rc;
+	int rc = -EAGAIN, retries = 0;
 
-	spin_lock_irqsave(&master->cmd_lock, flags);
+	while ((retries++) < FSI_CRC_ERR_RETRIES) {
+		rc = send_request(master, cmd);
+		if (rc)
+			break;
+		rc = poll_for_response(master, slave, resp_len, resp);
+		if (rc != -EAGAIN)
+			break;
+		rc = -EIO;
+		dev_warn(master->dev, "ECRC retry %d\n", retries);
 
-	if (master->external_mode) {
-		spin_unlock_irqrestore(&master->cmd_lock, flags);
-		return -EBUSY;
+		/* Pace it a bit before retry */
+		msleep(1);
 	}
 
-	serial_out(master, cmd);
-	echo_delay(master);
-	rc = poll_for_response(master, slave, resp_len, resp);
-	spin_unlock_irqrestore(&master->cmd_lock, flags);
-
 	return rc;
 }
 
@@ -432,12 +586,18 @@ static int fsi_master_gpio_read(struct fsi_master *_master, int link,
 {
 	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
 	struct fsi_gpio_msg cmd;
+	int rc;
 
 	if (link != 0)
 		return -ENODEV;
 
-	build_abs_ar_command(&cmd, id, addr, size, NULL);
-	return fsi_master_gpio_xfer(master, id, &cmd, size, val);
+	mutex_lock(&master->cmd_lock);
+	build_ar_command(master, &cmd, id, addr, size, NULL);
+	rc = fsi_master_gpio_xfer(master, id, &cmd, size, val);
+	last_address_update(master, id, rc == 0, addr);
+	mutex_unlock(&master->cmd_lock);
+
+	return rc;
 }
 
 static int fsi_master_gpio_write(struct fsi_master *_master, int link,
@@ -445,12 +605,18 @@ 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;
 
 	if (link != 0)
 		return -ENODEV;
 
-	build_abs_ar_command(&cmd, id, addr, size, val);
-	return fsi_master_gpio_xfer(master, id, &cmd, 0, NULL);
+	mutex_lock(&master->cmd_lock);
+	build_ar_command(master, &cmd, id, addr, size, val);
+	rc = fsi_master_gpio_xfer(master, id, &cmd, 0, NULL);
+	last_address_update(master, id, rc == 0, addr);
+	mutex_unlock(&master->cmd_lock);
+
+	return rc;
 }
 
 static int fsi_master_gpio_term(struct fsi_master *_master,
@@ -458,12 +624,18 @@ static int fsi_master_gpio_term(struct fsi_master *_master,
 {
 	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
 	struct fsi_gpio_msg cmd;
+	int rc;
 
 	if (link != 0)
 		return -ENODEV;
 
+	mutex_lock(&master->cmd_lock);
 	build_term_command(&cmd, id);
-	return fsi_master_gpio_xfer(master, id, &cmd, 0, NULL);
+	rc = fsi_master_gpio_xfer(master, id, &cmd, 0, NULL);
+	last_address_update(master, id, false, 0);
+	mutex_unlock(&master->cmd_lock);
+
+	return rc;
 }
 
 static int fsi_master_gpio_break(struct fsi_master *_master, int link)
@@ -476,11 +648,14 @@ static int fsi_master_gpio_break(struct fsi_master *_master, int link)
 
 	trace_fsi_master_gpio_break(master);
 
-	spin_lock_irqsave(&master->cmd_lock, flags);
+	mutex_lock(&master->cmd_lock);
 	if (master->external_mode) {
-		spin_unlock_irqrestore(&master->cmd_lock, flags);
+		mutex_unlock(&master->cmd_lock);
 		return -EBUSY;
 	}
+
+	local_irq_save(flags);
+
 	set_sda_output(master, 1);
 	sda_out(master, 1);
 	clock_toggle(master, FSI_PRE_BREAK_CLOCKS);
@@ -489,7 +664,11 @@ static int fsi_master_gpio_break(struct fsi_master *_master, int link)
 	echo_delay(master);
 	sda_out(master, 1);
 	clock_toggle(master, FSI_POST_BREAK_CLOCKS);
-	spin_unlock_irqrestore(&master->cmd_lock, flags);
+
+	local_irq_restore(flags);
+
+	last_address_update(master, 0, false, 0);
+	mutex_unlock(&master->cmd_lock);
 
 	/* Wait for logic reset to take effect */
 	udelay(200);
@@ -499,6 +678,8 @@ static int fsi_master_gpio_break(struct fsi_master *_master, int link)
 
 static void fsi_master_gpio_init(struct fsi_master_gpio *master)
 {
+	unsigned long flags;
+
 	gpiod_direction_output(master->gpio_mux, 1);
 	gpiod_direction_output(master->gpio_trans, 1);
 	gpiod_direction_output(master->gpio_enable, 1);
@@ -506,7 +687,9 @@ static void fsi_master_gpio_init(struct fsi_master_gpio *master)
 	gpiod_direction_output(master->gpio_data, 1);
 
 	/* todo: evaluate if clocks can be reduced */
+	local_irq_save(flags);
 	clock_zeros(master, FSI_INIT_CLOCKS);
+	local_irq_restore(flags);
 }
 
 static void fsi_master_gpio_init_external(struct fsi_master_gpio *master)
@@ -521,18 +704,17 @@ static void fsi_master_gpio_init_external(struct fsi_master_gpio *master)
 static int fsi_master_gpio_link_enable(struct fsi_master *_master, int link)
 {
 	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
-	unsigned long flags;
 	int rc = -EBUSY;
 
 	if (link != 0)
 		return -ENODEV;
 
-	spin_lock_irqsave(&master->cmd_lock, flags);
+	mutex_lock(&master->cmd_lock);
 	if (!master->external_mode) {
 		gpiod_set_value(master->gpio_enable, 1);
 		rc = 0;
 	}
-	spin_unlock_irqrestore(&master->cmd_lock, flags);
+	mutex_unlock(&master->cmd_lock);
 
 	return rc;
 }
@@ -550,7 +732,7 @@ static ssize_t external_mode_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct fsi_master_gpio *master = dev_get_drvdata(dev);
-	unsigned long flags, val;
+	unsigned long val;
 	bool external_mode;
 	int err;
 
@@ -560,10 +742,10 @@ static ssize_t external_mode_store(struct device *dev,
 
 	external_mode = !!val;
 
-	spin_lock_irqsave(&master->cmd_lock, flags);
+	mutex_lock(&master->cmd_lock);
 
 	if (external_mode == master->external_mode) {
-		spin_unlock_irqrestore(&master->cmd_lock, flags);
+		mutex_unlock(&master->cmd_lock);
 		return count;
 	}
 
@@ -572,7 +754,8 @@ static ssize_t external_mode_store(struct device *dev,
 		fsi_master_gpio_init_external(master);
 	else
 		fsi_master_gpio_init(master);
-	spin_unlock_irqrestore(&master->cmd_lock, flags);
+
+	mutex_unlock(&master->cmd_lock);
 
 	fsi_master_rescan(&master->master);
 
@@ -595,6 +778,7 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 	master->dev = &pdev->dev;
 	master->master.dev.parent = master->dev;
 	master->master.dev.of_node = of_node_get(dev_of_node(master->dev));
+	master->last_addr = LAST_ADDR_INVALID;
 
 	gpio = devm_gpiod_get(&pdev->dev, "clock", 0);
 	if (IS_ERR(gpio)) {
@@ -632,6 +816,13 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 	}
 	master->gpio_mux = gpio;
 
+	/*
+	 * Check if GPIO block is slow enought that no extra delays
+	 * are necessary. This improves performance on ast2500 by
+	 * an order of magnitude.
+	 */
+	master->no_delays = device_property_present(&pdev->dev, "no-gpio-delays");
+
 	master->master.n_links = 1;
 	master->master.flags = FSI_MASTER_FLAG_SWCLOCK;
 	master->master.read = fsi_master_gpio_read;
@@ -640,7 +831,7 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 	master->master.send_break = fsi_master_gpio_break;
 	master->master.link_enable = fsi_master_gpio_link_enable;
 	platform_set_drvdata(pdev, master);
-	spin_lock_init(&master->cmd_lock);
+	mutex_init(&master->cmd_lock);
 
 	fsi_master_gpio_init(master);
 
diff --git a/drivers/fsi/fsi-sbefifo.c b/drivers/fsi/fsi-sbefifo.c
new file mode 100644
index 000000000000..9b8b6b346af6
--- /dev/null
+++ b/drivers/fsi/fsi-sbefifo.c
@@ -0,0 +1,1005 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) IBM Corporation 2017
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERGCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/fsi.h>
+#include <linux/fsi-sbefifo.h>
+#include <linux/idr.h>
+#include <linux/kernel.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/uio.h>
+#include <linux/vmalloc.h>
+
+/*
+ * The SBEFIFO is a pipe-like FSI device for communicating with
+ * the self boot engine on POWER processors.
+ */
+
+#define DEVICE_NAME		"sbefifo"
+#define FSI_ENGID_SBE		0x22
+
+/*
+ * Register layout
+ */
+
+/* Register banks */
+#define SBEFIFO_UP		0x00		/* FSI -> Host */
+#define SBEFIFO_DOWN		0x40		/* Host -> FSI */
+
+/* Per-bank registers */
+#define SBEFIFO_FIFO		0x00		/* The FIFO itself */
+#define SBEFIFO_STS		0x04		/* Status register */
+#define   SBEFIFO_STS_PARITY_ERR	0x20000000
+#define   SBEFIFO_STS_RESET_REQ		0x02000000
+#define   SBEFIFO_STS_GOT_EOT		0x00800000
+#define   SBEFIFO_STS_MAX_XFER_LIMIT	0x00400000
+#define   SBEFIFO_STS_FULL		0x00200000
+#define   SBEFIFO_STS_EMPTY		0x00100000
+#define   SBEFIFO_STS_ECNT_MASK		0x000f0000
+#define   SBEFIFO_STS_ECNT_SHIFT	16
+#define   SBEFIFO_STS_VALID_MASK	0x0000ff00
+#define   SBEFIFO_STS_VALID_SHIFT	8
+#define   SBEFIFO_STS_EOT_MASK		0x000000ff
+#define   SBEFIFO_STS_EOT_SHIFT		0
+#define SBEFIFO_EOT_RAISE	0x08		/* (Up only) Set End Of Transfer */
+#define SBEFIFO_REQ_RESET	0x0C		/* (Up only) Reset Request */
+#define SBEFIFO_PERFORM_RESET	0x10		/* (Down only) Perform Reset */
+#define SBEFIFO_EOT_ACK		0x14		/* (Down only) Acknowledge EOT */
+#define SBEFIFO_DOWN_MAX	0x18		/* (Down only) Max transfer */
+
+/* CFAM GP Mailbox SelfBoot Message register */
+#define CFAM_GP_MBOX_SBM_ADDR	0x2824	/* Converted 0x2809 */
+
+#define CFAM_SBM_SBE_BOOTED		0x80000000
+#define CFAM_SBM_SBE_ASYNC_FFDC		0x40000000
+#define CFAM_SBM_SBE_STATE_MASK		0x00f00000
+#define CFAM_SBM_SBE_STATE_SHIFT	20
+
+enum sbe_state
+{
+	SBE_STATE_UNKNOWN = 0x0, // Unkown, initial state
+	SBE_STATE_IPLING  = 0x1, // IPL'ing - autonomous mode (transient)
+	SBE_STATE_ISTEP   = 0x2, // ISTEP - Running IPL by steps (transient)
+	SBE_STATE_MPIPL   = 0x3, // MPIPL
+	SBE_STATE_RUNTIME = 0x4, // SBE Runtime
+	SBE_STATE_DMT     = 0x5, // Dead Man Timer State (transient)
+	SBE_STATE_DUMP    = 0x6, // Dumping
+	SBE_STATE_FAILURE = 0x7, // Internal SBE failure
+	SBE_STATE_QUIESCE = 0x8, // Final state - needs SBE reset to get out
+};
+
+/* FIFO depth */
+#define SBEFIFO_FIFO_DEPTH		8
+
+/* Helpers */
+#define sbefifo_empty(sts)	((sts) & SBEFIFO_STS_EMPTY)
+#define sbefifo_full(sts)	((sts) & SBEFIFO_STS_FULL)
+#define sbefifo_parity_err(sts)	((sts) & SBEFIFO_STS_PARITY_ERR)
+#define sbefifo_populated(sts)	(((sts) & SBEFIFO_STS_ECNT_MASK) >> SBEFIFO_STS_ECNT_SHIFT)
+#define sbefifo_vacant(sts)	(SBEFIFO_FIFO_DEPTH - sbefifo_populated(sts))
+#define sbefifo_eot_set(sts)	(((sts) & SBEFIFO_STS_EOT_MASK) >> SBEFIFO_STS_EOT_SHIFT)
+
+/* Reset request timeout in ms */
+#define SBEFIFO_RESET_TIMEOUT		10000
+
+/* Timeouts for commands in ms */
+#define SBEFIFO_TIMEOUT_START_CMD	10000
+#define SBEFIFO_TIMEOUT_IN_CMD		1000
+#define SBEFIFO_TIMEOUT_START_RSP	10000
+#define SBEFIFO_TIMEOUT_IN_RSP		1000
+
+/* Other constants */
+#define SBEFIFO_MAX_CMD_LEN		PAGE_SIZE
+#define SBEFIFO_RESET_MAGIC		0x52534554 /* "RSET" */
+
+struct sbefifo {
+	uint32_t		magic;
+#define SBEFIFO_MAGIC		0x53424546 /* "SBEF" */
+	struct fsi_device	*fsi_dev;
+	struct miscdevice	mdev;
+	struct mutex		lock;
+	char			name[32];
+	int			idx;
+	bool			broken;
+	bool			async_ffdc;
+};
+
+struct sbefifo_user {
+	struct sbefifo		*sbefifo;
+	struct mutex		file_lock;
+	void			*pending_cmd;
+	size_t			pending_len;
+};
+
+static DEFINE_IDA(sbefifo_ida);
+static DEFINE_MUTEX(sbefifo_ffdc_mutex);
+
+
+static void sbefifo_dump_ffdc(struct device *dev, const __be32 *ffdc,
+			      size_t ffdc_sz, bool internal)
+{
+	int pack = 0;
+#define FFDC_LSIZE	60
+	static char ffdc_line[FFDC_LSIZE];
+	char *p = ffdc_line;
+
+	mutex_lock(&sbefifo_ffdc_mutex);
+	while (ffdc_sz) {
+		u32 w0, w1, w2, i;
+		if (ffdc_sz < 3) {
+			dev_err(dev, "SBE invalid FFDC package size %zd\n", ffdc_sz);
+			return;
+		}
+		w0 = be32_to_cpu(*(ffdc++));
+		w1 = be32_to_cpu(*(ffdc++));
+		w2 = be32_to_cpu(*(ffdc++));
+		ffdc_sz -= 3;
+		if ((w0 >> 16) != 0xFFDC) {
+			dev_err(dev, "SBE invalid FFDC package signature %08x %08x %08x\n",
+				w0, w1, w2);
+			break;
+		}
+		w0 &= 0xffff;
+		if (w0 > ffdc_sz) {
+			dev_err(dev, "SBE FFDC package len %d words but only %zd remaining\n",
+				w0, ffdc_sz);
+			w0 = ffdc_sz;
+			break;
+		}
+		if (internal) {
+			dev_warn(dev, "+---- SBE FFDC package %d for async err -----+\n",
+				 pack++);
+		} else {
+			dev_warn(dev, "+---- SBE FFDC package %d for cmd %02x:%02x -----+\n",
+				 pack++, (w1 >> 8) & 0xff, w1 & 0xff);
+		}
+		dev_warn(dev, "| Response code: %08x                   |\n", w2);
+		dev_warn(dev, "|-------------------------------------------|\n");
+		for (i = 0; i < w0; i++) {
+			if ((i & 3) == 0) {
+				p = ffdc_line;
+				p += sprintf(p, "| %04x:", i << 4);
+			}
+			p += sprintf(p, " %08x", be32_to_cpu(*(ffdc++)));
+			ffdc_sz--;
+			if ((i & 3) == 3 || i == (w0 - 1)) {
+				while ((i & 3) < 3) {
+					p += sprintf(p, "         ");
+					i++;
+				}
+				dev_warn(dev, "%s |\n", ffdc_line);
+			}
+		}
+		dev_warn(dev, "+-------------------------------------------+\n");
+	}
+}
+
+int sbefifo_parse_status(struct device *dev, u16 cmd, __be32 *response,
+			 size_t resp_len, size_t *data_len)
+{
+	u32 dh, s0, s1;
+	size_t ffdc_sz;
+
+	if (resp_len < 3) {
+		pr_debug("sbefifo: cmd %04x, response too small: %zd\n",
+			 cmd, resp_len);
+		return -ENXIO;
+	}
+	dh = be32_to_cpu(response[resp_len - 1]);
+	if (dh > resp_len || dh < 3) {
+		dev_err(dev, "SBE cmd %02x:%02x status offset out of range: %d/%zd\n",
+			cmd >> 8, cmd & 0xff, dh, resp_len);
+		return -ENXIO;
+	}
+	s0 = be32_to_cpu(response[resp_len - dh]);
+	s1 = be32_to_cpu(response[resp_len - dh + 1]);
+	if (((s0 >> 16) != 0xC0DE) || ((s0 & 0xffff) != cmd)) {
+		dev_err(dev, "SBE cmd %02x:%02x, status signature invalid: 0x%08x 0x%08x\n",
+			cmd >> 8, cmd & 0xff, s0, s1);
+		return -ENXIO;
+	}
+	if (s1 != 0) {
+		ffdc_sz = dh - 3;
+		dev_warn(dev, "SBE error cmd %02x:%02x status=%04x:%04x\n",
+			 cmd >> 8, cmd & 0xff, s1 >> 16, s1 & 0xffff);
+		if (ffdc_sz)
+			sbefifo_dump_ffdc(dev, &response[resp_len - dh + 2],
+					  ffdc_sz, false);
+	}
+	if (data_len)
+		*data_len = resp_len - dh;
+
+	/*
+	 * Primary status don't have the top bit set, so can't be confused with
+	 * Linux negative error codes, so return the status word whole.
+	 */
+	return s1;
+}
+EXPORT_SYMBOL_GPL(sbefifo_parse_status);
+
+static int sbefifo_regr(struct sbefifo *sbefifo, int reg, u32 *word)
+{
+	__be32 raw_word;
+	int rc;
+
+	rc = fsi_device_read(sbefifo->fsi_dev, reg, &raw_word,
+			     sizeof(raw_word));
+	if (rc)
+		return rc;
+
+	*word = be32_to_cpu(raw_word);
+
+	return 0;
+}
+
+static int sbefifo_regw(struct sbefifo *sbefifo, int reg, u32 word)
+{
+	__be32 raw_word = cpu_to_be32(word);
+
+	return fsi_device_write(sbefifo->fsi_dev, reg, &raw_word,
+				sizeof(raw_word));
+}
+
+static int sbefifo_check_sbe_state(struct sbefifo *sbefifo)
+{
+	__be32 raw_word;
+	u32 sbm;
+	int rc;
+
+	rc = fsi_slave_read(sbefifo->fsi_dev->slave, CFAM_GP_MBOX_SBM_ADDR,
+			    &raw_word, sizeof(raw_word));
+	if (rc)
+		return rc;
+	sbm = be32_to_cpu(raw_word);
+
+	/* SBE booted at all ? */
+	if (!(sbm & CFAM_SBM_SBE_BOOTED))
+		return -ESHUTDOWN;
+
+	/* Check its state */
+	switch ((sbm & CFAM_SBM_SBE_STATE_MASK) >> CFAM_SBM_SBE_STATE_SHIFT) {
+	case SBE_STATE_UNKNOWN:
+		return -ESHUTDOWN;
+	case SBE_STATE_IPLING:
+	case SBE_STATE_ISTEP:
+	case SBE_STATE_MPIPL:
+	case SBE_STATE_DMT:
+		return -EBUSY;
+	case SBE_STATE_RUNTIME:
+	case SBE_STATE_DUMP: /* Not sure about that one */
+		break;
+	case SBE_STATE_FAILURE:
+	case SBE_STATE_QUIESCE:
+		return -ESHUTDOWN;
+	}
+
+	/* Is there async FFDC available ? Remember it */
+	if (sbm & CFAM_SBM_SBE_ASYNC_FFDC)
+		sbefifo->async_ffdc = true;
+
+	return 0;
+}
+
+/* Don't flip endianness of data to/from FIFO, just pass through. */
+static int sbefifo_down_read(struct sbefifo *sbefifo, __be32 *word)
+{
+	return fsi_device_read(sbefifo->fsi_dev, SBEFIFO_DOWN, word,
+			       sizeof(*word));
+}
+
+static int sbefifo_up_write(struct sbefifo *sbefifo, __be32 word)
+{
+	return fsi_device_write(sbefifo->fsi_dev, SBEFIFO_UP, &word,
+				sizeof(word));
+}
+
+static int sbefifo_request_reset(struct sbefifo *sbefifo)
+{
+	struct device *dev = &sbefifo->fsi_dev->dev;
+	u32 status, timeout;
+	int rc;
+
+	dev_dbg(dev, "Requesting FIFO reset\n");
+
+	/* Mark broken first, will be cleared if reset succeeds */
+	sbefifo->broken = true;
+
+	/* Send reset request */
+	rc = sbefifo_regw(sbefifo, SBEFIFO_UP | SBEFIFO_REQ_RESET, 1);
+	if (rc) {
+		dev_err(dev, "Sending reset request failed, rc=%d\n", rc);
+		return rc;
+	}
+
+	/* Wait for it to complete */
+	for (timeout = 0; timeout < SBEFIFO_RESET_TIMEOUT; timeout++) {
+		rc = sbefifo_regr(sbefifo, SBEFIFO_UP | SBEFIFO_STS, &status);
+		if (rc) {
+			dev_err(dev, "Failed to read UP fifo status during reset"
+				" , rc=%d\n", rc);
+			return rc;
+		}
+
+		if (!(status & SBEFIFO_STS_RESET_REQ)) {
+			dev_dbg(dev, "FIFO reset done\n");
+			sbefifo->broken = false;
+			return 0;
+		}
+
+		msleep(1);
+	}
+	dev_err(dev, "FIFO reset timed out\n");
+
+	return -ETIMEDOUT;
+}
+
+static int sbefifo_cleanup_hw(struct sbefifo *sbefifo)
+{
+	struct device *dev = &sbefifo->fsi_dev->dev;
+	u32 up_status, down_status;
+	bool need_reset = false;
+	int rc;
+
+	rc = sbefifo_check_sbe_state(sbefifo);
+	if (rc) {
+		dev_dbg(dev, "SBE state=%d\n", rc);
+		return rc;
+	}
+
+	/* If broken, we don't need to look at status, go straight to reset */
+	if (sbefifo->broken)
+		goto do_reset;
+
+	rc = sbefifo_regr(sbefifo, SBEFIFO_UP | SBEFIFO_STS, &up_status);
+	if (rc) {
+		dev_err(dev, "Cleanup: Reading UP status failed, rc=%d\n", rc);
+
+		/* Will try reset again on next attempt at using it */
+		sbefifo->broken = true;
+		return rc;
+	}
+
+	rc = sbefifo_regr(sbefifo, SBEFIFO_DOWN | SBEFIFO_STS, &down_status);
+	if (rc) {
+		dev_err(dev, "Cleanup: Reading DOWN status failed, rc=%d\n", rc);
+
+		/* Will try reset again on next attempt at using it */
+		sbefifo->broken = true;
+		return rc;
+	}
+
+	/* The FIFO already contains a reset request from the SBE ? */
+	if (down_status & SBEFIFO_STS_RESET_REQ) {
+		dev_info(dev, "Cleanup: FIFO reset request set, resetting\n");
+		rc = sbefifo_regw(sbefifo, SBEFIFO_UP, SBEFIFO_PERFORM_RESET);
+		if (rc) {
+			sbefifo->broken = true;
+			dev_err(dev, "Cleanup: Reset reg write failed, rc=%d\n", rc);
+			return rc;
+		}
+		sbefifo->broken = false;
+		return 0;
+	}
+
+	/* Parity error on either FIFO ? */
+	if ((up_status | down_status) & SBEFIFO_STS_PARITY_ERR)
+		need_reset = true;
+
+	/* Either FIFO not empty ? */
+	if (!((up_status & down_status) & SBEFIFO_STS_EMPTY))
+		need_reset = true;
+
+	if (!need_reset)
+		return 0;
+
+	dev_info(dev, "Cleanup: FIFO not clean (up=0x%08x down=0x%08x)\n",
+		 up_status, down_status);
+
+ do_reset:
+
+	/* Mark broken, will be cleared if/when reset succeeds */
+	return sbefifo_request_reset(sbefifo);
+}
+
+static int sbefifo_wait(struct sbefifo *sbefifo, bool up,
+			u32 *status, unsigned long timeout)
+{
+	struct device *dev = &sbefifo->fsi_dev->dev;
+	unsigned long end_time;
+	bool ready = false;
+	u32 addr, sts = 0;
+	int rc;
+
+	dev_vdbg(dev, "Wait on %s fifo...\n", up ? "up" : "down");
+
+	addr = (up ? SBEFIFO_UP : SBEFIFO_DOWN) | SBEFIFO_STS;
+
+	end_time = jiffies + timeout;
+	while (!time_after(jiffies, end_time)) {
+		cond_resched();
+		rc = sbefifo_regr(sbefifo, addr, &sts);
+		if (rc < 0) {
+			dev_err(dev, "FSI error %d reading status register\n", rc);
+			return rc;
+		}
+		if (!up && sbefifo_parity_err(sts)) {
+			dev_err(dev, "Parity error in DOWN FIFO\n");
+			return -ENXIO;
+		}
+		ready = !(up ? sbefifo_full(sts) : sbefifo_empty(sts));
+		if (ready)
+			break;
+	}
+	if (!ready) {
+		dev_err(dev, "%s FIFO Timeout ! status=%08x\n", up ? "UP" : "DOWN", sts);
+		return -ETIMEDOUT;
+	}
+	dev_vdbg(dev, "End of wait status: %08x\n", sts);
+
+	*status = sts;
+
+	return 0;
+}
+
+static int sbefifo_send_command(struct sbefifo *sbefifo,
+				const __be32 *command, size_t cmd_len)
+{
+	struct device *dev = &sbefifo->fsi_dev->dev;
+	size_t len, chunk, vacant = 0, remaining = cmd_len;
+	unsigned long timeout;
+	u32 status;
+	int rc;
+
+	dev_vdbg(dev, "sending command (%zd words, cmd=%04x)\n",
+		 cmd_len, be32_to_cpu(command[1]));
+
+	/* As long as there's something to send */
+	timeout = msecs_to_jiffies(SBEFIFO_TIMEOUT_START_CMD);
+	while (remaining) {
+		/* Wait for room in the FIFO */
+		rc = sbefifo_wait(sbefifo, true, &status, timeout);
+		if (rc < 0)
+			return rc;
+		timeout = msecs_to_jiffies(SBEFIFO_TIMEOUT_IN_CMD);
+
+		vacant = sbefifo_vacant(status);
+		len = chunk = min(vacant, remaining);
+
+		dev_vdbg(dev, "  status=%08x vacant=%zd chunk=%zd\n",
+			 status, vacant, chunk);
+
+		/* Write as much as we can */
+		while (len--) {
+			rc = sbefifo_up_write(sbefifo, *(command++));
+			if (rc) {
+				dev_err(dev, "FSI error %d writing UP FIFO\n", rc);
+				return rc;
+			}
+		}
+		remaining -= chunk;
+		vacant -= chunk;
+	}
+
+	/* If there's no room left, wait for some to write EOT */
+	if (!vacant) {
+		rc = sbefifo_wait(sbefifo, true, &status, timeout);
+		if (rc)
+			return rc;
+	}
+
+	/* Send an EOT */
+	rc = sbefifo_regw(sbefifo, SBEFIFO_UP | SBEFIFO_EOT_RAISE, 0);
+	if (rc)
+		dev_err(dev, "FSI error %d writing EOT\n", rc);
+	return rc;
+}
+
+static int sbefifo_read_response(struct sbefifo *sbefifo, struct iov_iter *response)
+{
+	struct device *dev = &sbefifo->fsi_dev->dev;
+	u32 data, status, eot_set;
+	unsigned long timeout;
+	bool overflow = false;
+	size_t len;
+	int rc;
+
+	dev_vdbg(dev, "reading response, buflen = %zd\n", iov_iter_count(response));
+
+	timeout = msecs_to_jiffies(SBEFIFO_TIMEOUT_START_RSP);
+	for (;;) {
+		/* Grab FIFO status (this will handle parity errors) */
+		rc = sbefifo_wait(sbefifo, false, &status, timeout);
+		if (rc < 0)
+			return rc;
+		timeout = msecs_to_jiffies(SBEFIFO_TIMEOUT_IN_RSP);
+
+		/* Decode status */
+		len = sbefifo_populated(status);
+		eot_set = sbefifo_eot_set(status);
+
+		dev_vdbg(dev, "  chunk size %zd eot_set=0x%x\n", len, eot_set);
+
+		/* Go through the chunk */
+		while(len--) {
+			/* Read the data */
+			rc = sbefifo_down_read(sbefifo, &data);
+			if (rc < 0)
+				return rc;
+
+			/* Was it an EOT ? */
+			if (eot_set & 0x80) {
+				/*
+				 * There should be nothing else in the FIFO,
+				 * if there is, mark broken, this will force
+				 * a reset on next use, but don't fail the
+				 * command.
+				 */
+				if (len) {
+					dev_warn(dev, "FIFO read hit"
+						 " EOT with still %zd data\n",
+						 len);
+					sbefifo->broken = true;
+				}
+
+				/* We are done */
+				rc = sbefifo_regw(sbefifo,
+						  SBEFIFO_DOWN | SBEFIFO_EOT_ACK, 0);
+
+				/*
+				 * If that write fail, still complete the request but mark
+				 * the fifo as broken for subsequent reset (not much else
+				 * we can do here).
+				 */
+				if (rc) {
+					dev_err(dev, "FSI error %d ack'ing EOT\n", rc);
+					sbefifo->broken = true;
+				}
+
+				/* Tell whether we overflowed */
+				return overflow ? -EOVERFLOW : 0;
+			}
+
+			/* Store it if there is room */
+			if (iov_iter_count(response) >= sizeof(__be32)) {
+				if (copy_to_iter(&data, sizeof(__be32), response) < sizeof(__be32))
+					return -EFAULT;
+			} else {
+				dev_vdbg(dev, "Response overflowed !\n");
+
+				overflow = true;
+			}
+
+			/* Next EOT bit */
+			eot_set <<= 1;
+		}
+	}
+	/* Shouldn't happen */
+	return -EIO;
+}
+
+static int sbefifo_do_command(struct sbefifo *sbefifo,
+			      const __be32 *command, size_t cmd_len,
+			      struct iov_iter *response)
+{
+	/* Try sending the command */
+	int rc = sbefifo_send_command(sbefifo, command, cmd_len);
+	if (rc)
+		return rc;
+
+	/* Now, get the response */
+	return sbefifo_read_response(sbefifo, response);
+}
+
+static void sbefifo_collect_async_ffdc(struct sbefifo *sbefifo)
+{
+	struct device *dev = &sbefifo->fsi_dev->dev;
+        struct iov_iter ffdc_iter;
+        struct kvec ffdc_iov;
+	__be32 *ffdc;
+	size_t ffdc_sz;
+	u32 cmd[2];
+	int rc;
+
+	sbefifo->async_ffdc = false;
+	ffdc = vmalloc(SBEFIFO_MAX_FFDC_SIZE);
+	if (!ffdc) {
+		dev_err(dev, "Failed to allocate SBE FFDC buffer\n");
+		return;
+	}
+        ffdc_iov.iov_base = ffdc;
+	ffdc_iov.iov_len = SBEFIFO_MAX_FFDC_SIZE;;
+        iov_iter_kvec(&ffdc_iter, WRITE | ITER_KVEC, &ffdc_iov, 1, SBEFIFO_MAX_FFDC_SIZE);
+	cmd[0] = cpu_to_be32(2);
+	cmd[1] = cpu_to_be32(SBEFIFO_CMD_GET_SBE_FFDC);
+	rc = sbefifo_do_command(sbefifo, cmd, 2, &ffdc_iter);
+	if (rc != 0) {
+		dev_err(dev, "Error %d retrieving SBE FFDC\n", rc);
+		goto bail;
+	}
+	ffdc_sz = SBEFIFO_MAX_FFDC_SIZE - iov_iter_count(&ffdc_iter);
+	ffdc_sz /= sizeof(__be32);
+	rc = sbefifo_parse_status(dev, SBEFIFO_CMD_GET_SBE_FFDC, ffdc,
+				  ffdc_sz, &ffdc_sz);
+	if (rc != 0) {
+		dev_err(dev, "Error %d decoding SBE FFDC\n", rc);
+		goto bail;
+	}
+	if (ffdc_sz > 0)
+		sbefifo_dump_ffdc(dev, ffdc, ffdc_sz, true);
+ bail:
+	vfree(ffdc);
+
+}
+
+static int __sbefifo_submit(struct sbefifo *sbefifo,
+			    const __be32 *command, size_t cmd_len,
+			    struct iov_iter *response)
+{
+	struct device *dev = &sbefifo->fsi_dev->dev;
+	int rc;
+
+	if (cmd_len < 2 || be32_to_cpu(command[0]) != cmd_len) {
+		dev_vdbg(dev, "Invalid command len %zd (header: %d)\n",
+			 cmd_len, be32_to_cpu(command[0]));
+		return -EINVAL;
+	}
+
+	/* First ensure the HW is in a clean state */
+	rc = sbefifo_cleanup_hw(sbefifo);
+	if (rc)
+		return rc;
+
+	/* Look for async FFDC first if any */
+	if (sbefifo->async_ffdc)
+		sbefifo_collect_async_ffdc(sbefifo);
+
+	rc = sbefifo_do_command(sbefifo, command, cmd_len, response);
+	if (rc != 0 && rc != -EOVERFLOW)
+		goto fail;
+	return rc;
+ fail:
+	/*
+	 * On failure, attempt a reset. Ignore the result, it will mark
+	 * the fifo broken if the reset fails
+	 */
+        sbefifo_request_reset(sbefifo);
+
+	/* Return original error */
+	return rc;
+}
+
+/**
+ * sbefifo_submit() - Submit and SBE fifo command and receive response
+ * @dev: The sbefifo device
+ * @command: The raw command data
+ * @cmd_len: The command size (in 32-bit words)
+ * @response: The output response buffer
+ * @resp_len: In: Response buffer size, Out: Response size
+ *
+ * This will perform the entire operation. If the reponse buffer
+ * overflows, returns -EOVERFLOW
+ */
+int sbefifo_submit(struct device *dev, const __be32 *command, size_t cmd_len,
+		   __be32 *response, size_t *resp_len)
+{
+	struct sbefifo *sbefifo = dev_get_drvdata(dev);
+        struct iov_iter resp_iter;
+        struct kvec resp_iov;
+	size_t rbytes;
+	int rc;
+
+	if (!dev || !sbefifo)
+		return -ENODEV;
+	if (WARN_ON_ONCE(sbefifo->magic != SBEFIFO_MAGIC))
+		return -ENODEV;
+	if (!resp_len || !command || !response || cmd_len > SBEFIFO_MAX_CMD_LEN)
+		return -EINVAL;
+
+	/* Prepare iov iterator */
+	rbytes = (*resp_len) * sizeof(__be32);
+	resp_iov.iov_base = response;
+	resp_iov.iov_len = rbytes;
+        iov_iter_kvec(&resp_iter, WRITE | ITER_KVEC, &resp_iov, 1, rbytes);
+
+	/* Perform the command */
+	mutex_lock(&sbefifo->lock);
+	rc = __sbefifo_submit(sbefifo, command, cmd_len, &resp_iter);
+	mutex_unlock(&sbefifo->lock);
+
+	/* Extract the response length */
+	rbytes -= iov_iter_count(&resp_iter);
+	*resp_len = rbytes / sizeof(__be32);
+
+	return rc;
+}
+EXPORT_SYMBOL_GPL(sbefifo_submit);
+
+/*
+ * Char device interface
+ */
+static int sbefifo_user_open(struct inode *inode, struct file *file)
+{
+	struct sbefifo *sbefifo = container_of(file->private_data,
+					       struct sbefifo, mdev);
+	struct sbefifo_user *user;
+
+	user = kzalloc(sizeof(struct sbefifo_user), GFP_KERNEL);
+	if (!user)
+		return -ENOMEM;
+
+	file->private_data = user;
+	user->sbefifo = sbefifo;
+	user->pending_cmd = (void *)__get_free_page(GFP_KERNEL);
+	if (!user->pending_cmd) {
+		kfree(user);
+		return -ENOMEM;
+	}
+	mutex_init(&user->file_lock);
+
+	return 0;
+}
+
+static ssize_t sbefifo_user_read(struct file *file, char __user *buf,
+				 size_t len, loff_t *offset)
+{
+	struct sbefifo_user *user = file->private_data;
+	struct sbefifo *sbefifo;
+	struct iov_iter resp_iter;
+        struct iovec resp_iov;
+	size_t cmd_len;
+	int rc;
+
+	if (!user)
+		return -EINVAL;
+	sbefifo = user->sbefifo;
+	if (len & 3)
+		return -EINVAL;
+
+	mutex_lock(&user->file_lock);
+
+	/* Cronus relies on -EAGAIN after a short read */
+	if (user->pending_len == 0) {
+		rc = -EAGAIN;
+		goto bail;
+	}
+	if (user->pending_len < 8) {
+		rc = -EINVAL;
+		goto bail;
+	}
+	cmd_len = user->pending_len >> 2;
+
+	/* Prepare iov iterator */
+	resp_iov.iov_base = buf;
+	resp_iov.iov_len = len;
+	iov_iter_init(&resp_iter, WRITE, &resp_iov, 1, len);
+
+	/* Perform the command */
+	mutex_lock(&sbefifo->lock);
+	rc = __sbefifo_submit(sbefifo, user->pending_cmd, cmd_len, &resp_iter);
+	mutex_unlock(&sbefifo->lock);
+	if (rc < 0)
+		goto bail;
+
+	/* Extract the response length */
+	rc = len - iov_iter_count(&resp_iter);
+ bail:
+	user->pending_len = 0;
+	mutex_unlock(&user->file_lock);
+	return rc;
+}
+
+static ssize_t sbefifo_user_write(struct file *file, const char __user *buf,
+				  size_t len, loff_t *offset)
+{
+	struct sbefifo_user *user = file->private_data;
+	struct sbefifo *sbefifo;
+	int rc = len;
+
+	if (!user)
+		return -EINVAL;
+	sbefifo = user->sbefifo;
+	if (len > SBEFIFO_MAX_CMD_LEN)
+		return -EINVAL;
+	if (len & 3)
+		return -EINVAL;
+
+	mutex_lock(&user->file_lock);
+
+	/* Copy the command into the staging buffer */
+	if (copy_from_user(user->pending_cmd, buf, len)) {
+		rc = -EFAULT;
+		goto bail;
+	}
+
+	/* Check for the magic reset command */
+	if (len == 4 && be32_to_cpu(*(__be32 *)user->pending_cmd) ==
+	    SBEFIFO_RESET_MAGIC)  {
+
+		/* Clear out any pending command */
+		user->pending_len = 0;
+
+		/* Trigger reset request */
+		mutex_lock(&sbefifo->lock);
+		rc = sbefifo_request_reset(user->sbefifo);
+		mutex_unlock(&sbefifo->lock);
+		if (rc == 0)
+			rc = 4;
+		goto bail;
+	}
+
+	/* Update the staging buffer size */
+	user->pending_len = len;
+ bail:
+	mutex_unlock(&user->file_lock);
+
+	/* And that's it, we'll issue the command on a read */
+	return rc;
+}
+
+static int sbefifo_user_release(struct inode *inode, struct file *file)
+{
+	struct sbefifo_user *user = file->private_data;
+
+	if (!user)
+		return -EINVAL;
+
+	free_page((unsigned long)user->pending_cmd);
+	kfree(user);
+
+	return 0;
+}
+
+static const struct file_operations sbefifo_fops = {
+	.owner		= THIS_MODULE,
+	.open		= sbefifo_user_open,
+	.read		= sbefifo_user_read,
+	.write		= sbefifo_user_write,
+	.release	= sbefifo_user_release,
+};
+
+/*
+ * Probe/remove
+ */
+
+static int sbefifo_probe(struct device *dev)
+{
+	struct fsi_device *fsi_dev = to_fsi_dev(dev);
+	struct sbefifo *sbefifo;
+	struct device_node *np;
+	struct platform_device *child;
+	char child_name[32];
+	int rc, child_idx = 0;
+
+	dev_dbg(dev, "Found sbefifo device\n");
+
+	sbefifo = devm_kzalloc(dev, sizeof(*sbefifo), GFP_KERNEL);
+	if (!sbefifo)
+		return -ENOMEM;
+	sbefifo->magic = SBEFIFO_MAGIC;
+	sbefifo->fsi_dev = fsi_dev;
+	mutex_init(&sbefifo->lock);
+
+	/*
+	 * Try cleaning up the FIFO. If this fails, we still register the
+	 * driver and will try cleaning things up again on the next access.
+	 */
+	rc = sbefifo_cleanup_hw(sbefifo);
+	if (rc && rc != -ESHUTDOWN)
+		dev_err(dev, "Initial HW cleanup failed, will retry later\n");
+
+	sbefifo->idx = ida_simple_get(&sbefifo_ida, 1, INT_MAX, GFP_KERNEL);
+	snprintf(sbefifo->name, sizeof(sbefifo->name), "sbefifo%d",
+		 sbefifo->idx);
+
+	dev_set_drvdata(dev, sbefifo);
+
+	/* Create misc chardev for userspace access */
+	sbefifo->mdev.minor = MISC_DYNAMIC_MINOR;
+	sbefifo->mdev.fops = &sbefifo_fops;
+	sbefifo->mdev.name = sbefifo->name;
+	sbefifo->mdev.parent = dev;
+	rc = misc_register(&sbefifo->mdev);
+	if (rc) {
+		dev_err(dev, "Failed to register miscdevice: %d\n", rc);
+		ida_simple_remove(&sbefifo_ida, sbefifo->idx);
+		return rc;
+	}
+
+	/* Create platform devs for dts child nodes (occ, etc) */
+	for_each_available_child_of_node(dev->of_node, np) {
+		snprintf(child_name, sizeof(child_name), "%s-dev%d",
+			 sbefifo->name, child_idx++);
+		child = of_platform_device_create(np, child_name, dev);
+		if (!child)
+			dev_warn(dev, "failed to create child %s dev\n",
+				 child_name);
+	}
+
+	return 0;
+}
+
+static int sbefifo_unregister_child(struct device *dev, void *data)
+{
+	struct platform_device *child = to_platform_device(dev);
+
+	of_device_unregister(child);
+	if (dev->of_node)
+		of_node_clear_flag(dev->of_node, OF_POPULATED);
+
+	return 0;
+}
+
+static int sbefifo_remove(struct device *dev)
+{
+	struct sbefifo *sbefifo = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "Removing sbefifo device...\n");
+
+	misc_deregister(&sbefifo->mdev);
+	device_for_each_child(dev, NULL, sbefifo_unregister_child);
+
+	ida_simple_remove(&sbefifo_ida, sbefifo->idx);
+
+	return 0;
+}
+
+static struct fsi_device_id sbefifo_ids[] = {
+	{
+		.engine_type = FSI_ENGID_SBE,
+		.version = FSI_VERSION_ANY,
+	},
+	{ 0 }
+};
+
+static struct fsi_driver sbefifo_drv = {
+	.id_table = sbefifo_ids,
+	.drv = {
+		.name = DEVICE_NAME,
+		.bus = &fsi_bus_type,
+		.probe = sbefifo_probe,
+		.remove = sbefifo_remove,
+	}
+};
+
+static int sbefifo_init(void)
+{
+	return fsi_driver_register(&sbefifo_drv);
+}
+
+static void sbefifo_exit(void)
+{
+	fsi_driver_unregister(&sbefifo_drv);
+
+	ida_destroy(&sbefifo_ida);
+}
+
+module_init(sbefifo_init);
+module_exit(sbefifo_exit);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Brad Bishop <bradleyb@fuzziesquirrel.com>");
+MODULE_AUTHOR("Eddie James <eajames@linux.vnet.ibm.com>");
+MODULE_AUTHOR("Andrew Jeffery <andrew@aj.id.au>");
+MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
+MODULE_DESCRIPTION("Linux device interface to the POWER Self Boot Engine");
diff --git a/drivers/fsi/fsi-scom.c b/drivers/fsi/fsi-scom.c
index e13353a2fd7c..c8eb5e5b94a7 100644
--- a/drivers/fsi/fsi-scom.c
+++ b/drivers/fsi/fsi-scom.c
@@ -26,15 +26,11 @@
 
 #define FSI_ENGID_SCOM		0x5
 
-#define SCOM_FSI2PIB_DELAY	50
-
 /* SCOM engine register set */
 #define SCOM_DATA0_REG		0x00
 #define SCOM_DATA1_REG		0x04
 #define SCOM_CMD_REG		0x08
-#define SCOM_RESET_REG		0x1C
 
-#define SCOM_RESET_CMD		0x80000000
 #define SCOM_WRITE_CMD		0x80000000
 
 struct scom_device {
@@ -180,7 +176,6 @@ static const struct file_operations scom_fops = {
 
 static int scom_probe(struct device *dev)
 {
-	uint32_t data;
 	struct fsi_device *fsi_dev = to_fsi_dev(dev);
 	struct scom_device *scom;
 
@@ -197,9 +192,6 @@ static int scom_probe(struct device *dev)
 	scom->mdev.parent = dev;
 	list_add(&scom->link, &scom_devices);
 
-	data = cpu_to_be32(SCOM_RESET_CMD);
-	fsi_device_write(fsi_dev, SCOM_RESET_REG, &data, sizeof(uint32_t));
-
 	return misc_register(&scom->mdev);
 }
 
diff --git a/include/linux/fsi-sbefifo.h b/include/linux/fsi-sbefifo.h
new file mode 100644
index 000000000000..13f9ebeaa25e
--- /dev/null
+++ b/include/linux/fsi-sbefifo.h
@@ -0,0 +1,33 @@
+/*
+ * SBEFIFO FSI Client device driver
+ *
+ * Copyright (C) IBM Corporation 2017
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERGCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef LINUX_FSI_SBEFIFO_H
+#define LINUX_FSI_SBEFIFO_H
+
+#define SBEFIFO_CMD_PUT_OCC_SRAM	0xa404
+#define SBEFIFO_CMD_GET_OCC_SRAM	0xa403
+#define SBEFIFO_CMD_GET_SBE_FFDC	0xa801
+
+#define SBEFIFO_MAX_FFDC_SIZE		0x2000
+
+struct device;
+
+int sbefifo_submit(struct device *dev, const __be32 *command, size_t cmd_len,
+		   __be32 *response, size_t *resp_len);
+
+int sbefifo_parse_status(struct device *dev, u16 cmd, __be32 *response,
+			 size_t resp_len, size_t *data_len);
+
+#endif /* LINUX_FSI_SBEFIFO_H */
diff --git a/include/trace/events/fsi_master_gpio.h b/include/trace/events/fsi_master_gpio.h
index f95cf3264bf9..389082132433 100644
--- a/include/trace/events/fsi_master_gpio.h
+++ b/include/trace/events/fsi_master_gpio.h
@@ -64,6 +64,49 @@ TRACE_EVENT(fsi_master_gpio_break,
 	)
 );
 
+TRACE_EVENT(fsi_master_gpio_crc_cmd_error,
+	TP_PROTO(const struct fsi_master_gpio *master),
+	TP_ARGS(master),
+	TP_STRUCT__entry(
+		__field(int,	master_idx)
+	),
+	TP_fast_assign(
+		__entry->master_idx = master->master.idx;
+	),
+	TP_printk("fsi-gpio%d ----CRC command retry---",
+		__entry->master_idx
+	)
+);
+
+TRACE_EVENT(fsi_master_gpio_crc_rsp_error,
+	TP_PROTO(const struct fsi_master_gpio *master),
+	TP_ARGS(master),
+	TP_STRUCT__entry(
+		__field(int,	master_idx)
+	),
+	TP_fast_assign(
+		__entry->master_idx = master->master.idx;
+	),
+	TP_printk("fsi-gpio%d ----CRC response---",
+		__entry->master_idx
+	)
+);
+
+TRACE_EVENT(fsi_master_gpio_poll_response_busy,
+	TP_PROTO(const struct fsi_master_gpio *master, int busy),
+	TP_ARGS(master, busy),
+	TP_STRUCT__entry(
+		__field(int,	master_idx)
+		__field(int,	busy)
+	),
+	TP_fast_assign(
+		__entry->master_idx = master->master.idx;
+		__entry->busy = busy;
+	),
+	TP_printk("fsi-gpio%d: device reported busy %d times",
+		__entry->master_idx, __entry->busy)
+);
+
 #endif /* _TRACE_FSI_MASTER_GPIO_H */
 
 #include <trace/define_trace.h>