diff --git a/drivers/fsi/Kconfig b/drivers/fsi/Kconfig
index 24f84a96b8b9..9c08f467a7bb 100644
--- a/drivers/fsi/Kconfig
+++ b/drivers/fsi/Kconfig
@@ -34,6 +34,7 @@ config FSI_SCOM
 
 config FSI_SBEFIFO
 	tristate "SBEFIFO FSI client device driver"
+	depends on OF_ADDRESS
 	---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
diff --git a/drivers/fsi/fsi-core.c b/drivers/fsi/fsi-core.c
index 4c03d6933646..e9f8813b75e6 100644
--- a/drivers/fsi/fsi-core.c
+++ b/drivers/fsi/fsi-core.c
@@ -81,6 +81,8 @@ struct fsi_slave {
 	int			id;
 	int			link;
 	uint32_t		size;	/* size of slave address space */
+	u8			t_send_delay;
+	u8			t_echo_delay;
 };
 
 #define to_fsi_master(d) container_of(d, struct fsi_master, dev)
@@ -190,7 +192,7 @@ static int fsi_slave_calc_addr(struct fsi_slave *slave, uint32_t *addrp,
 static int fsi_slave_report_and_clear_errors(struct fsi_slave *slave)
 {
 	struct fsi_master *master = slave->master;
-	uint32_t irq, stat;
+	__be32 irq, stat;
 	int rc, link;
 	uint8_t id;
 
@@ -215,7 +217,53 @@ static int fsi_slave_report_and_clear_errors(struct fsi_slave *slave)
 			&irq, sizeof(irq));
 }
 
-static int fsi_slave_set_smode(struct fsi_master *master, int link, int id);
+/* Encode slave local bus echo delay */
+static inline uint32_t fsi_smode_echodly(int x)
+{
+	return (x & FSI_SMODE_ED_MASK) << FSI_SMODE_ED_SHIFT;
+}
+
+/* Encode slave local bus send delay */
+static inline uint32_t fsi_smode_senddly(int x)
+{
+	return (x & FSI_SMODE_SD_MASK) << FSI_SMODE_SD_SHIFT;
+}
+
+/* Encode slave local bus clock rate ratio */
+static inline uint32_t fsi_smode_lbcrr(int x)
+{
+	return (x & FSI_SMODE_LBCRR_MASK) << FSI_SMODE_LBCRR_SHIFT;
+}
+
+/* Encode slave ID */
+static inline uint32_t fsi_smode_sid(int x)
+{
+	return (x & FSI_SMODE_SID_MASK) << FSI_SMODE_SID_SHIFT;
+}
+
+static uint32_t fsi_slave_smode(int id, u8 t_senddly, u8 t_echodly)
+{
+	return FSI_SMODE_WSC | FSI_SMODE_ECRC
+		| fsi_smode_sid(id)
+		| fsi_smode_echodly(t_echodly - 1) | fsi_smode_senddly(t_senddly - 1)
+		| fsi_smode_lbcrr(0x8);
+}
+
+static int fsi_slave_set_smode(struct fsi_slave *slave)
+{
+	uint32_t smode;
+	__be32 data;
+
+	/* set our smode register with the slave ID field to 0; this enables
+	 * extended slave addressing
+	 */
+	smode = fsi_slave_smode(slave->id, slave->t_send_delay, slave->t_echo_delay);
+	data = cpu_to_be32(smode);
+
+	return fsi_master_write(slave->master, slave->link, slave->id,
+				FSI_SLAVE_BASE + FSI_SMODE,
+				&data, sizeof(data));
+}
 
 static int fsi_slave_handle_error(struct fsi_slave *slave, bool write,
 				  uint32_t addr, size_t size)
@@ -223,7 +271,7 @@ static int fsi_slave_handle_error(struct fsi_slave *slave, bool write,
 	struct fsi_master *master = slave->master;
 	int rc, link;
 	uint32_t reg;
-	uint8_t id;
+	uint8_t id, send_delay, echo_delay;
 
 	if (discard_errors)
 		return -1;
@@ -254,15 +302,26 @@ static int fsi_slave_handle_error(struct fsi_slave *slave, bool write,
 		}
 	}
 
+	send_delay = slave->t_send_delay;
+	echo_delay = slave->t_echo_delay;
+
 	/* getting serious, reset the slave via BREAK */
 	rc = fsi_master_break(master, link);
 	if (rc)
 		return rc;
 
-	rc = fsi_slave_set_smode(master, link, id);
+	slave->t_send_delay = send_delay;
+	slave->t_echo_delay = echo_delay;
+
+	rc = fsi_slave_set_smode(slave);
 	if (rc)
 		return rc;
 
+	if (master->link_config)
+		master->link_config(master, link,
+				    slave->t_send_delay,
+				    slave->t_echo_delay);
+
 	return fsi_slave_report_and_clear_errors(slave);
 }
 
@@ -390,7 +449,6 @@ static struct device_node *fsi_device_find_of_node(struct fsi_device *dev)
 static int fsi_slave_scan(struct fsi_slave *slave)
 {
 	uint32_t engine_addr;
-	uint32_t conf;
 	int rc, i;
 
 	/*
@@ -404,15 +462,17 @@ static int fsi_slave_scan(struct fsi_slave *slave)
 	for (i = 2; i < engine_page_size / sizeof(uint32_t); i++) {
 		uint8_t slots, version, type, crc;
 		struct fsi_device *dev;
+		uint32_t conf;
+		__be32 data;
 
-		rc = fsi_slave_read(slave, (i + 1) * sizeof(conf),
-				&conf, sizeof(conf));
+		rc = fsi_slave_read(slave, (i + 1) * sizeof(data),
+				&data, sizeof(data));
 		if (rc) {
 			dev_warn(&slave->dev,
 				"error reading slave registers\n");
 			return -1;
 		}
-		conf = be32_to_cpu(conf);
+		conf = be32_to_cpu(data);
 
 		crc = crc4(0, conf, 32);
 		if (crc) {
@@ -562,52 +622,6 @@ static const struct bin_attribute fsi_slave_term_attr = {
 	.write = fsi_slave_sysfs_term_write,
 };
 
-/* Encode slave local bus echo delay */
-static inline uint32_t fsi_smode_echodly(int x)
-{
-	return (x & FSI_SMODE_ED_MASK) << FSI_SMODE_ED_SHIFT;
-}
-
-/* Encode slave local bus send delay */
-static inline uint32_t fsi_smode_senddly(int x)
-{
-	return (x & FSI_SMODE_SD_MASK) << FSI_SMODE_SD_SHIFT;
-}
-
-/* Encode slave local bus clock rate ratio */
-static inline uint32_t fsi_smode_lbcrr(int x)
-{
-	return (x & FSI_SMODE_LBCRR_MASK) << FSI_SMODE_LBCRR_SHIFT;
-}
-
-/* Encode slave ID */
-static inline uint32_t fsi_smode_sid(int x)
-{
-	return (x & FSI_SMODE_SID_MASK) << FSI_SMODE_SID_SHIFT;
-}
-
-static uint32_t fsi_slave_smode(int id)
-{
-	return FSI_SMODE_WSC | FSI_SMODE_ECRC
-		| fsi_smode_sid(id)
-		| fsi_smode_echodly(0xf) | fsi_smode_senddly(0xf)
-		| fsi_smode_lbcrr(0x8);
-}
-
-static int fsi_slave_set_smode(struct fsi_master *master, int link, int id)
-{
-	uint32_t smode;
-
-	/* set our smode register with the slave ID field to 0; this enables
-	 * extended slave addressing
-	 */
-	smode = fsi_slave_smode(id);
-	smode = cpu_to_be32(smode);
-
-	return fsi_master_write(master, link, id, FSI_SLAVE_BASE + FSI_SMODE,
-			&smode, sizeof(smode));
-}
-
 static void fsi_slave_release(struct device *dev)
 {
 	struct fsi_slave *slave = to_fsi_slave(dev);
@@ -659,11 +673,56 @@ static struct device_node *fsi_slave_find_of_node(struct fsi_master *master,
 	return NULL;
 }
 
+static ssize_t slave_send_echo_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct fsi_slave *slave = to_fsi_slave(dev);
+
+	return sprintf(buf, "%u\n", slave->t_send_delay);
+}
+
+static ssize_t slave_send_echo_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fsi_slave *slave = to_fsi_slave(dev);
+	struct fsi_master *master = slave->master;
+	unsigned long val;
+	int rc;
+
+	if (kstrtoul(buf, 0, &val) < 0)
+		return -EINVAL;
+
+	if (val < 1 || val > 16)
+		return -EINVAL;
+
+	if (!master->link_config)
+		return -ENXIO;
+
+	/* Current HW mandates that send and echo delay are identical */
+	slave->t_send_delay = val;
+	slave->t_echo_delay = val;
+
+	rc = fsi_slave_set_smode(slave);
+	if (rc < 0)
+		return rc;
+	if (master->link_config)
+		master->link_config(master, slave->link,
+				    slave->t_send_delay,
+				    slave->t_echo_delay);
+
+	return count;
+}
+
+static DEVICE_ATTR(send_echo_delays, 0600,
+		   slave_send_echo_show, slave_send_echo_store);
+
 static int fsi_slave_init(struct fsi_master *master, int link, uint8_t id)
 {
-	uint32_t chip_id, llmode;
+	uint32_t chip_id;
 	struct fsi_slave *slave;
 	uint8_t crc;
+	__be32 data, llmode;
 	int rc;
 
 	/* Currently, we only support single slaves on a link, and use the
@@ -672,13 +731,13 @@ static int fsi_slave_init(struct fsi_master *master, int link, uint8_t id)
 	if (id != 0)
 		return -EINVAL;
 
-	rc = fsi_master_read(master, link, id, 0, &chip_id, sizeof(chip_id));
+	rc = fsi_master_read(master, link, id, 0, &data, sizeof(data));
 	if (rc) {
 		dev_dbg(&master->dev, "can't read slave %02x:%02x %d\n",
 				link, id, rc);
 		return -ENODEV;
 	}
-	chip_id = be32_to_cpu(chip_id);
+	chip_id = be32_to_cpu(data);
 
 	crc = crc4(0, chip_id, 32);
 	if (crc) {
@@ -690,14 +749,6 @@ static int fsi_slave_init(struct fsi_master *master, int link, uint8_t id)
 	dev_dbg(&master->dev, "fsi: found chip %08x at %02x:%02x:%02x\n",
 			chip_id, master->idx, link, id);
 
-	rc = fsi_slave_set_smode(master, link, id);
-	if (rc) {
-		dev_warn(&master->dev,
-				"can't set smode on slave:%02x:%02x %d\n",
-				link, id, rc);
-		return -ENODEV;
-	}
-
 	/* If we're behind a master that doesn't provide a self-running bus
 	 * clock, put the slave into async mode
 	 */
@@ -726,6 +777,21 @@ static int fsi_slave_init(struct fsi_master *master, int link, uint8_t id)
 	slave->link = link;
 	slave->id = id;
 	slave->size = FSI_SLAVE_SIZE_23b;
+	slave->t_send_delay = 16;
+	slave->t_echo_delay = 16;
+
+	rc = fsi_slave_set_smode(slave);
+	if (rc) {
+		dev_warn(&master->dev,
+				"can't set smode on slave:%02x:%02x %d\n",
+				link, id, rc);
+		kfree(slave);
+		return -ENODEV;
+	}
+	if (master->link_config)
+		master->link_config(master, link,
+				    slave->t_send_delay,
+				    slave->t_echo_delay);
 
 	dev_set_name(&slave->dev, "slave@%02x:%02x", link, id);
 	rc = device_register(&slave->dev);
@@ -744,6 +810,10 @@ static int fsi_slave_init(struct fsi_master *master, int link, uint8_t id)
 	if (rc)
 		dev_warn(&slave->dev, "failed to create term attr: %d\n", rc);
 
+	rc = device_create_file(&slave->dev, &dev_attr_send_echo_delays);
+	if (rc)
+		dev_warn(&slave->dev, "failed to create delay attr: %d\n", rc);
+
 	rc = fsi_slave_scan(slave);
 	if (rc)
 		dev_dbg(&master->dev, "failed during slave scan with: %d\n",
@@ -814,12 +884,16 @@ static int fsi_master_link_enable(struct fsi_master *master, int link)
  */
 static int fsi_master_break(struct fsi_master *master, int link)
 {
+	int rc = 0;
+
 	trace_fsi_master_break(master, link);
 
 	if (master->send_break)
-		return master->send_break(master, link);
+		rc = master->send_break(master, link);
+	if (master->link_config)
+		master->link_config(master, link, 16, 16);
 
-	return 0;
+	return rc;
 }
 
 static int fsi_master_scan(struct fsi_master *master)
@@ -903,9 +977,6 @@ int fsi_master_register(struct fsi_master *master)
 	int rc;
 	struct device_node *np;
 
-	if (!master)
-		return -EINVAL;
-
 	master->idx = ida_simple_get(&master_ida, 0, INT_MAX, GFP_KERNEL);
 	dev_set_name(&master->dev, "fsi%d", master->idx);
 
@@ -917,14 +988,14 @@ int fsi_master_register(struct fsi_master *master)
 
 	rc = device_create_file(&master->dev, &dev_attr_rescan);
 	if (rc) {
-		device_unregister(&master->dev);
+		device_del(&master->dev);
 		ida_simple_remove(&master_ida, master->idx);
 		return rc;
 	}
 
 	rc = device_create_file(&master->dev, &dev_attr_break);
 	if (rc) {
-		device_unregister(&master->dev);
+		device_del(&master->dev);
 		ida_simple_remove(&master_ida, master->idx);
 		return rc;
 	}
diff --git a/drivers/fsi/fsi-master-gpio.c b/drivers/fsi/fsi-master-gpio.c
index 084e9da8d151..4eb3a766fd4a 100644
--- a/drivers/fsi/fsi-master-gpio.c
+++ b/drivers/fsi/fsi-master-gpio.c
@@ -17,39 +17,6 @@
 #include "fsi-master.h"
 
 #define	FSI_GPIO_STD_DLY	1	/* Standard pin delay in nS */
-#define	FSI_ECHO_DELAY_CLOCKS	16	/* Number clocks for echo delay */
-#define	FSI_PRE_BREAK_CLOCKS	50	/* Number clocks to prep for break */
-#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... */
-
-/* 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_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	20
-
 #define LAST_ADDR_INVALID		0x1
 
 struct fsi_master_gpio {
@@ -64,6 +31,8 @@ struct fsi_master_gpio {
 	bool			external_mode;
 	bool			no_delays;
 	uint32_t		last_addr;
+	uint8_t			t_send_delay;
+	uint8_t			t_echo_delay;
 };
 
 #define CREATE_TRACE_POINTS
@@ -128,10 +97,17 @@ static void set_sda_output(struct fsi_master_gpio *master, int value)
 
 static void clock_zeros(struct fsi_master_gpio *master, int count)
 {
+	trace_fsi_master_gpio_clock_zeros(master, count);
 	set_sda_output(master, 1);
 	clock_toggle(master, count);
 }
 
+static void echo_delay(struct fsi_master_gpio *master)
+{
+	clock_zeros(master, master->t_echo_delay);
+}
+
+
 static void serial_in(struct fsi_master_gpio *master, struct fsi_gpio_msg *msg,
 			uint8_t num_bits)
 {
@@ -276,17 +252,20 @@ static void build_ar_command(struct fsi_master_gpio *master,
 		/* we still address the byte offset within the word */
 		addr_bits = 2;
 		opcode_bits = 2;
-		opcode = FSI_GPIO_CMD_SAME_AR;
+		opcode = FSI_CMD_SAME_AR;
+		trace_fsi_master_gpio_cmd_same_addr(master);
 
 	} 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;
+		opcode = FSI_CMD_REL_AR;
+		trace_fsi_master_gpio_cmd_rel_addr(master, rel_addr);
 
 	} else {
 		addr_bits = 21;
-		opcode = FSI_GPIO_CMD_ABS_AR;
+		opcode = FSI_CMD_ABS_AR;
+		trace_fsi_master_gpio_cmd_abs_addr(master, addr);
 	}
 
 	/*
@@ -321,7 +300,7 @@ static void build_dpoll_command(struct fsi_gpio_msg *cmd, uint8_t slave_id)
 	cmd->msg = 0;
 
 	msg_push_bits(cmd, slave_id, 2);
-	msg_push_bits(cmd, FSI_GPIO_CMD_DPOLL, 3);
+	msg_push_bits(cmd, FSI_CMD_DPOLL, 3);
 	msg_push_crc(cmd);
 }
 
@@ -331,23 +310,17 @@ static void build_epoll_command(struct fsi_gpio_msg *cmd, uint8_t slave_id)
 	cmd->msg = 0;
 
 	msg_push_bits(cmd, slave_id, 2);
-	msg_push_bits(cmd, FSI_GPIO_CMD_EPOLL, 3);
+	msg_push_bits(cmd, FSI_CMD_EPOLL, 3);
 	msg_push_crc(cmd);
 }
 
-static void echo_delay(struct fsi_master_gpio *master)
-{
-	set_sda_output(master, 1);
-	clock_toggle(master, FSI_ECHO_DELAY_CLOCKS);
-}
-
 static void build_term_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_TERM, 6);
+	msg_push_bits(cmd, FSI_CMD_TERM, 6);
 	msg_push_crc(cmd);
 }
 
@@ -369,14 +342,14 @@ static int read_one_response(struct fsi_master_gpio *master,
 	local_irq_save(flags);
 
 	/* wait for the start bit */
-	for (i = 0; i < FSI_GPIO_MTOE_COUNT; i++) {
+	for (i = 0; i < FSI_MASTER_MTOE_COUNT; i++) {
 		msg.bits = 0;
 		msg.msg = 0;
 		serial_in(master, &msg, 1);
 		if (msg.msg)
 			break;
 	}
-	if (i == FSI_GPIO_MTOE_COUNT) {
+	if (i == FSI_MASTER_MTOE_COUNT) {
 		dev_dbg(master->dev,
 			"Master time out waiting for response\n");
 		local_irq_restore(flags);
@@ -392,11 +365,11 @@ static int read_one_response(struct fsi_master_gpio *master,
 	tag = msg.msg & 0x3;
 
 	/* If we have an ACK and we're expecting data, clock the data in too */
-	if (tag == FSI_GPIO_RESP_ACK && data_size)
+	if (tag == FSI_RESP_ACK && data_size)
 		serial_in(master, &msg, data_size * 8);
 
 	/* read CRC */
-	serial_in(master, &msg, FSI_GPIO_CRC_SIZE);
+	serial_in(master, &msg, FSI_CRC_SIZE);
 
 	local_irq_restore(flags);
 
@@ -439,7 +412,7 @@ static int issue_term(struct fsi_master_gpio *master, uint8_t slave)
 		dev_err(master->dev,
 				"TERM failed; lost communication with slave\n");
 		return -EIO;
-	} else if (tag != FSI_GPIO_RESP_ACK) {
+	} else if (tag != FSI_RESP_ACK) {
 		dev_err(master->dev, "TERM failed; response %d\n", tag);
 		return -EIO;
 	}
@@ -475,7 +448,7 @@ static int poll_for_response(struct fsi_master_gpio *master,
 		trace_fsi_master_gpio_crc_rsp_error(master);
 		build_epoll_command(&cmd, slave);
 		local_irq_save(flags);
-		clock_zeros(master, FSI_GPIO_EPOLL_CLOCKS);
+		clock_zeros(master, FSI_MASTER_EPOLL_CLOCKS);
 		serial_out(master, &cmd);
 		echo_delay(master);
 		local_irq_restore(flags);
@@ -484,7 +457,7 @@ static int poll_for_response(struct fsi_master_gpio *master,
 		goto fail;
 
 	switch (tag) {
-	case FSI_GPIO_RESP_ACK:
+	case FSI_RESP_ACK:
 		if (size && data) {
 			uint64_t val = response.msg;
 			/* clear crc & mask */
@@ -497,16 +470,16 @@ static int poll_for_response(struct fsi_master_gpio *master,
 			}
 		}
 		break;
-	case FSI_GPIO_RESP_BUSY:
+	case FSI_RESP_BUSY:
 		/*
 		 * Its necessary to clock slave before issuing
 		 * d-poll, not indicated in the hardware protocol
 		 * spec. < 20 clocks causes slave to hang, 21 ok.
 		 */
-		if (busy_count++ < FSI_GPIO_MAX_BUSY) {
+		if (busy_count++ < FSI_MASTER_MAX_BUSY) {
 			build_dpoll_command(&cmd, slave);
 			local_irq_save(flags);
-			clock_zeros(master, FSI_GPIO_DPOLL_CLOCKS);
+			clock_zeros(master, FSI_MASTER_DPOLL_CLOCKS);
 			serial_out(master, &cmd);
 			echo_delay(master);
 			local_irq_restore(flags);
@@ -515,17 +488,17 @@ static int poll_for_response(struct fsi_master_gpio *master,
 		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);
+		clock_zeros(master, FSI_MASTER_DPOLL_CLOCKS);
 		local_irq_restore(flags);
 		issue_term(master, slave);
 		rc = -EIO;
 		break;
 
-	case FSI_GPIO_RESP_ERRA:
+	case FSI_RESP_ERRA:
 		dev_dbg(master->dev, "ERRA received: 0x%x\n", (int)response.msg);
 		rc = -EIO;
 		break;
-	case FSI_GPIO_RESP_ERRC:
+	case FSI_RESP_ERRC:
 		dev_dbg(master->dev, "ERRC received: 0x%x\n", (int)response.msg);
 		trace_fsi_master_gpio_crc_cmd_error(master);
 		rc = -EAGAIN;
@@ -535,9 +508,12 @@ static int poll_for_response(struct fsi_master_gpio *master,
 	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 */
+	/*
+	 * tSendDelay clocks, avoids signal reflections when switching
+	 * from receive of response back to send of data.
+	 */
 	local_irq_save(flags);
-	clock_zeros(master, FSI_GPIO_PRIME_SLAVE_CLOCKS);
+	clock_zeros(master, master->t_send_delay);
 	local_irq_restore(flags);
 
 	return rc;
@@ -719,6 +695,22 @@ static int fsi_master_gpio_link_enable(struct fsi_master *_master, int link)
 	return rc;
 }
 
+static int fsi_master_gpio_link_config(struct fsi_master *_master, int link,
+				       u8 t_send_delay, u8 t_echo_delay)
+{
+	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
+
+	if (link != 0)
+		return -ENODEV;
+
+	mutex_lock(&master->cmd_lock);
+	master->t_send_delay = t_send_delay;
+	master->t_echo_delay = t_echo_delay;
+	mutex_unlock(&master->cmd_lock);
+
+	return 0;
+}
+
 static ssize_t external_mode_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -765,32 +757,44 @@ static ssize_t external_mode_store(struct device *dev,
 static DEVICE_ATTR(external_mode, 0664,
 		external_mode_show, external_mode_store);
 
+static void fsi_master_gpio_release(struct device *dev)
+{
+	struct fsi_master_gpio *master = to_fsi_master_gpio(dev_to_fsi_master(dev));
+
+	of_node_put(dev_of_node(master->dev));
+
+	kfree(master);
+}
+
 static int fsi_master_gpio_probe(struct platform_device *pdev)
 {
 	struct fsi_master_gpio *master;
 	struct gpio_desc *gpio;
 	int rc;
 
-	master = devm_kzalloc(&pdev->dev, sizeof(*master), GFP_KERNEL);
+	master = kzalloc(sizeof(*master), GFP_KERNEL);
 	if (!master)
 		return -ENOMEM;
 
 	master->dev = &pdev->dev;
 	master->master.dev.parent = master->dev;
 	master->master.dev.of_node = of_node_get(dev_of_node(master->dev));
+	master->master.dev.release = fsi_master_gpio_release;
 	master->last_addr = LAST_ADDR_INVALID;
 
 	gpio = devm_gpiod_get(&pdev->dev, "clock", 0);
 	if (IS_ERR(gpio)) {
 		dev_err(&pdev->dev, "failed to get clock gpio\n");
-		return PTR_ERR(gpio);
+		rc = PTR_ERR(gpio);
+		goto err_free;
 	}
 	master->gpio_clk = gpio;
 
 	gpio = devm_gpiod_get(&pdev->dev, "data", 0);
 	if (IS_ERR(gpio)) {
 		dev_err(&pdev->dev, "failed to get data gpio\n");
-		return PTR_ERR(gpio);
+		rc = PTR_ERR(gpio);
+		goto err_free;
 	}
 	master->gpio_data = gpio;
 
@@ -798,21 +802,24 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 	gpio = devm_gpiod_get_optional(&pdev->dev, "trans", 0);
 	if (IS_ERR(gpio)) {
 		dev_err(&pdev->dev, "failed to get trans gpio\n");
-		return PTR_ERR(gpio);
+		rc = PTR_ERR(gpio);
+		goto err_free;
 	}
 	master->gpio_trans = gpio;
 
 	gpio = devm_gpiod_get_optional(&pdev->dev, "enable", 0);
 	if (IS_ERR(gpio)) {
 		dev_err(&pdev->dev, "failed to get enable gpio\n");
-		return PTR_ERR(gpio);
+		rc = PTR_ERR(gpio);
+		goto err_free;
 	}
 	master->gpio_enable = gpio;
 
 	gpio = devm_gpiod_get_optional(&pdev->dev, "mux", 0);
 	if (IS_ERR(gpio)) {
 		dev_err(&pdev->dev, "failed to get mux gpio\n");
-		return PTR_ERR(gpio);
+		rc = PTR_ERR(gpio);
+		goto err_free;
 	}
 	master->gpio_mux = gpio;
 
@@ -823,6 +830,10 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 	 */
 	master->no_delays = device_property_present(&pdev->dev, "no-gpio-delays");
 
+	/* Default FSI command delays */
+	master->t_send_delay = FSI_SEND_DELAY_CLOCKS;
+	master->t_echo_delay = FSI_ECHO_DELAY_CLOCKS;
+
 	master->master.n_links = 1;
 	master->master.flags = FSI_MASTER_FLAG_SWCLOCK;
 	master->master.read = fsi_master_gpio_read;
@@ -830,6 +841,7 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 	master->master.term = fsi_master_gpio_term;
 	master->master.send_break = fsi_master_gpio_break;
 	master->master.link_enable = fsi_master_gpio_link_enable;
+	master->master.link_config = fsi_master_gpio_link_config;
 	platform_set_drvdata(pdev, master);
 	mutex_init(&master->cmd_lock);
 
@@ -837,27 +849,29 @@ static int fsi_master_gpio_probe(struct platform_device *pdev)
 
 	rc = device_create_file(&pdev->dev, &dev_attr_external_mode);
 	if (rc)
-		return rc;
+		goto err_free;
 
-	return fsi_master_register(&master->master);
+	rc = fsi_master_register(&master->master);
+	if (rc) {
+		device_remove_file(&pdev->dev, &dev_attr_external_mode);
+		put_device(&master->master.dev);
+		return rc;
+	}
+	return 0;
+ err_free:
+	kfree(master);
+	return rc;
 }
 
 
+
 static int fsi_master_gpio_remove(struct platform_device *pdev)
 {
 	struct fsi_master_gpio *master = platform_get_drvdata(pdev);
 
-	devm_gpiod_put(&pdev->dev, master->gpio_clk);
-	devm_gpiod_put(&pdev->dev, master->gpio_data);
-	if (master->gpio_trans)
-		devm_gpiod_put(&pdev->dev, master->gpio_trans);
-	if (master->gpio_enable)
-		devm_gpiod_put(&pdev->dev, master->gpio_enable);
-	if (master->gpio_mux)
-		devm_gpiod_put(&pdev->dev, master->gpio_mux);
-	fsi_master_unregister(&master->master);
+	device_remove_file(&pdev->dev, &dev_attr_external_mode);
 
-	of_node_put(master->master.dev.of_node);
+	fsi_master_unregister(&master->master);
 
 	return 0;
 }
diff --git a/drivers/fsi/fsi-master-hub.c b/drivers/fsi/fsi-master-hub.c
index 5885fc4a1ef0..b3c1e9debcf2 100644
--- a/drivers/fsi/fsi-master-hub.c
+++ b/drivers/fsi/fsi-master-hub.c
@@ -122,7 +122,8 @@ static int hub_master_write(struct fsi_master *master, int link,
 
 static int hub_master_break(struct fsi_master *master, int link)
 {
-	uint32_t addr, cmd;
+	uint32_t addr;
+	__be32 cmd;
 
 	addr = 0x4;
 	cmd = cpu_to_be32(0xc0de0000);
@@ -205,7 +206,7 @@ static int hub_master_init(struct fsi_master_hub *hub)
 	if (rc)
 		return rc;
 
-	reg = ~0;
+	reg = cpu_to_be32(~0);
 	rc = fsi_device_write(dev, FSI_MSENP0, &reg, sizeof(reg));
 	if (rc)
 		return rc;
diff --git a/drivers/fsi/fsi-master.h b/drivers/fsi/fsi-master.h
index ee0b46086026..f653f75da7be 100644
--- a/drivers/fsi/fsi-master.h
+++ b/drivers/fsi/fsi-master.h
@@ -19,6 +19,39 @@
 
 #include <linux/device.h>
 
+/* Various protocol delays */
+#define	FSI_ECHO_DELAY_CLOCKS	16	/* Number clocks for echo delay */
+#define	FSI_SEND_DELAY_CLOCKS	16	/* Number clocks for send delay */
+#define	FSI_PRE_BREAK_CLOCKS	50	/* Number clocks to prep for break */
+#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_MASTER_DPOLL_CLOCKS	50      /* < 21 will cause slave to hang */
+#define	FSI_MASTER_EPOLL_CLOCKS	50      /* Number of clocks for E_POLL retry */
+
+/* Various retry maximums */
+#define FSI_CRC_ERR_RETRIES	10
+#define	FSI_MASTER_MAX_BUSY	200
+#define	FSI_MASTER_MTOE_COUNT	1000
+
+/* Command encodings */
+#define	FSI_CMD_DPOLL		0x2
+#define	FSI_CMD_EPOLL		0x3
+#define	FSI_CMD_TERM		0x3f
+#define FSI_CMD_ABS_AR		0x4
+#define FSI_CMD_REL_AR		0x5
+#define FSI_CMD_SAME_AR		0x3	/* but only a 2-bit opcode... */
+
+/* Slave responses */
+#define	FSI_RESP_ACK		0	/* Success */
+#define	FSI_RESP_BUSY		1	/* Slave busy */
+#define	FSI_RESP_ERRA		2	/* Any (misc) Error */
+#define	FSI_RESP_ERRC		3	/* Slave reports master CRC error */
+
+/* Misc */
+#define	FSI_CRC_SIZE		4
+
+/* fsi-master definition and flags */
 #define FSI_MASTER_FLAG_SWCLOCK		0x1
 
 struct fsi_master {
@@ -33,6 +66,8 @@ struct fsi_master {
 	int		(*term)(struct fsi_master *, int link, uint8_t id);
 	int		(*send_break)(struct fsi_master *, int link);
 	int		(*link_enable)(struct fsi_master *, int link);
+	int		(*link_config)(struct fsi_master *, int link,
+				       u8 t_send_delay, u8 t_echo_delay);
 };
 
 #define dev_to_fsi_master(d) container_of(d, struct fsi_master, dev)
diff --git a/drivers/fsi/fsi-sbefifo.c b/drivers/fsi/fsi-sbefifo.c
index 9b8b6b346af6..6b31cc24fb0d 100644
--- a/drivers/fsi/fsi-sbefifo.c
+++ b/drivers/fsi/fsi-sbefifo.c
@@ -194,6 +194,7 @@ static void sbefifo_dump_ffdc(struct device *dev, const __be32 *ffdc,
 		}
 		dev_warn(dev, "+-------------------------------------------+\n");
 	}
+	mutex_unlock(&sbefifo_ffdc_mutex);
 }
 
 int sbefifo_parse_status(struct device *dev, u16 cmd, __be32 *response,
@@ -519,9 +520,10 @@ static int sbefifo_send_command(struct sbefifo *sbefifo,
 static int sbefifo_read_response(struct sbefifo *sbefifo, struct iov_iter *response)
 {
 	struct device *dev = &sbefifo->fsi_dev->dev;
-	u32 data, status, eot_set;
+	u32 status, eot_set;
 	unsigned long timeout;
 	bool overflow = false;
+	__be32 data;
 	size_t len;
 	int rc;
 
@@ -619,7 +621,7 @@ static void sbefifo_collect_async_ffdc(struct sbefifo *sbefifo)
         struct kvec ffdc_iov;
 	__be32 *ffdc;
 	size_t ffdc_sz;
-	u32 cmd[2];
+	__be32 cmd[2];
 	int rc;
 
 	sbefifo->async_ffdc = false;
@@ -629,7 +631,7 @@ static void sbefifo_collect_async_ffdc(struct sbefifo *sbefifo)
 		return;
 	}
         ffdc_iov.iov_base = ffdc;
-	ffdc_iov.iov_len = SBEFIFO_MAX_FFDC_SIZE;;
+	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);
@@ -704,13 +706,16 @@ static int __sbefifo_submit(struct sbefifo *sbefifo,
 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 sbefifo *sbefifo;
         struct iov_iter resp_iter;
         struct kvec resp_iov;
 	size_t rbytes;
 	int rc;
 
-	if (!dev || !sbefifo)
+	if (!dev)
+		return -ENODEV;
+	sbefifo = dev_get_drvdata(dev);
+	if (!sbefifo)
 		return -ENODEV;
 	if (WARN_ON_ONCE(sbefifo->magic != SBEFIFO_MAGIC))
 		return -ENODEV;
diff --git a/drivers/fsi/fsi-scom.c b/drivers/fsi/fsi-scom.c
index c8eb5e5b94a7..39c74351f1bf 100644
--- a/drivers/fsi/fsi-scom.c
+++ b/drivers/fsi/fsi-scom.c
@@ -24,21 +24,63 @@
 #include <linux/list.h>
 #include <linux/idr.h>
 
+#include <uapi/linux/fsi.h>
+
 #define FSI_ENGID_SCOM		0x5
 
 /* SCOM engine register set */
 #define SCOM_DATA0_REG		0x00
 #define SCOM_DATA1_REG		0x04
 #define SCOM_CMD_REG		0x08
+#define SCOM_FSI2PIB_RESET_REG	0x18
+#define SCOM_STATUS_REG		0x1C /* Read */
+#define SCOM_PIB_RESET_REG	0x1C /* Write */
 
+/* Command register */
 #define SCOM_WRITE_CMD		0x80000000
+#define SCOM_READ_CMD		0x00000000
+
+/* Status register bits */
+#define SCOM_STATUS_ERR_SUMMARY		0x80000000
+#define SCOM_STATUS_PROTECTION		0x01000000
+#define SCOM_STATUS_PARITY		0x04000000
+#define SCOM_STATUS_PIB_ABORT		0x00100000
+#define SCOM_STATUS_PIB_RESP_MASK	0x00007000
+#define SCOM_STATUS_PIB_RESP_SHIFT	12
+
+#define SCOM_STATUS_ANY_ERR		(SCOM_STATUS_ERR_SUMMARY | \
+					 SCOM_STATUS_PROTECTION | \
+					 SCOM_STATUS_PARITY |	  \
+					 SCOM_STATUS_PIB_ABORT | \
+					 SCOM_STATUS_PIB_RESP_MASK)
+/* SCOM address encodings */
+#define XSCOM_ADDR_IND_FLAG		BIT_ULL(63)
+#define XSCOM_ADDR_INF_FORM1		BIT_ULL(60)
+
+/* SCOM indirect stuff */
+#define XSCOM_ADDR_DIRECT_PART		0x7fffffffull
+#define XSCOM_ADDR_INDIRECT_PART	0x000fffff00000000ull
+#define XSCOM_DATA_IND_READ		BIT_ULL(63)
+#define XSCOM_DATA_IND_COMPLETE		BIT_ULL(31)
+#define XSCOM_DATA_IND_ERR_MASK		0x70000000ull
+#define XSCOM_DATA_IND_ERR_SHIFT	28
+#define XSCOM_DATA_IND_DATA		0x0000ffffull
+#define XSCOM_DATA_IND_FORM1_DATA	0x000fffffffffffffull
+#define XSCOM_ADDR_FORM1_LOW		0x000ffffffffull
+#define XSCOM_ADDR_FORM1_HI		0xfff00000000ull
+#define XSCOM_ADDR_FORM1_HI_SHIFT	20
+
+/* Retries */
+#define SCOM_MAX_RETRIES		100	/* Retries on busy */
+#define SCOM_MAX_IND_RETRIES		10	/* Retries indirect not ready */
 
 struct scom_device {
 	struct list_head link;
 	struct fsi_device *fsi_dev;
 	struct miscdevice mdev;
+	struct mutex lock;
 	char	name[32];
-	int idx;
+	int	idx;
 };
 
 #define to_scom_dev(x)		container_of((x), struct scom_device, mdev)
@@ -47,11 +89,11 @@ static struct list_head scom_devices;
 
 static DEFINE_IDA(scom_ida);
 
-static int put_scom(struct scom_device *scom_dev, uint64_t value,
-			uint32_t addr)
+static int __put_scom(struct scom_device *scom_dev, uint64_t value,
+		      uint32_t addr, uint32_t *status)
 {
+	__be32 data, raw_status;
 	int rc;
-	uint32_t data;
 
 	data = cpu_to_be32((value >> 32) & 0xffffffff);
 	rc = fsi_device_write(scom_dev->fsi_dev, SCOM_DATA0_REG, &data,
@@ -66,53 +108,285 @@ static int put_scom(struct scom_device *scom_dev, uint64_t value,
 		return rc;
 
 	data = cpu_to_be32(SCOM_WRITE_CMD | addr);
-	return fsi_device_write(scom_dev->fsi_dev, SCOM_CMD_REG, &data,
-				sizeof(uint32_t));
-}
-
-static int get_scom(struct scom_device *scom_dev, uint64_t *value,
-			uint32_t addr)
-{
-	uint32_t result, data;
-	int rc;
-
-	*value = 0ULL;
-	data = cpu_to_be32(addr);
 	rc = fsi_device_write(scom_dev->fsi_dev, SCOM_CMD_REG, &data,
 				sizeof(uint32_t));
 	if (rc)
 		return rc;
-
-	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_DATA0_REG, &result,
-				sizeof(uint32_t));
+	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_STATUS_REG, &raw_status,
+			     sizeof(uint32_t));
 	if (rc)
 		return rc;
-
-	*value |= (uint64_t)cpu_to_be32(result) << 32;
-	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_DATA1_REG, &result,
-				sizeof(uint32_t));
-	if (rc)
-		return rc;
-
-	*value |= cpu_to_be32(result);
+	*status = be32_to_cpu(raw_status);
 
 	return 0;
 }
 
-static ssize_t scom_read(struct file *filep, char __user *buf, size_t len,
-			loff_t *offset)
+static int __get_scom(struct scom_device *scom_dev, uint64_t *value,
+		      uint32_t addr, uint32_t *status)
 {
+	__be32 data, raw_status;
 	int rc;
+
+
+	*value = 0ULL;
+	data = cpu_to_be32(SCOM_READ_CMD | addr);
+	rc = fsi_device_write(scom_dev->fsi_dev, SCOM_CMD_REG, &data,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_STATUS_REG, &raw_status,
+			     sizeof(uint32_t));
+	if (rc)
+		return rc;
+
+	/*
+	 * Read the data registers even on error, so we don't have
+	 * to interpret the status register here.
+	 */
+	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_DATA0_REG, &data,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+	*value |= (uint64_t)be32_to_cpu(data) << 32;
+	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_DATA1_REG, &data,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+	*value |= be32_to_cpu(data);
+	*status = be32_to_cpu(raw_status);
+
+	return rc;
+}
+
+static int put_indirect_scom_form0(struct scom_device *scom, uint64_t value,
+				   uint64_t addr, uint32_t *status)
+{
+	uint64_t ind_data, ind_addr;
+	int rc, retries, err = 0;
+
+	if (value & ~XSCOM_DATA_IND_DATA)
+		return -EINVAL;
+
+	ind_addr = addr & XSCOM_ADDR_DIRECT_PART;
+	ind_data = (addr & XSCOM_ADDR_INDIRECT_PART) | value;
+	rc = __put_scom(scom, ind_data, ind_addr, status);
+	if (rc || (*status & SCOM_STATUS_ANY_ERR))
+		return rc;
+
+	for (retries = 0; retries < SCOM_MAX_IND_RETRIES; retries++) {
+		rc = __get_scom(scom, &ind_data, addr, status);
+		if (rc || (*status & SCOM_STATUS_ANY_ERR))
+			return rc;
+
+		err = (ind_data & XSCOM_DATA_IND_ERR_MASK) >> XSCOM_DATA_IND_ERR_SHIFT;
+		*status = err << SCOM_STATUS_PIB_RESP_SHIFT;
+		if ((ind_data & XSCOM_DATA_IND_COMPLETE) || (err != SCOM_PIB_BLOCKED))
+			return 0;
+
+		msleep(1);
+	}
+	return rc;
+}
+
+static int put_indirect_scom_form1(struct scom_device *scom, uint64_t value,
+				   uint64_t addr, uint32_t *status)
+{
+	uint64_t ind_data, ind_addr;
+
+	if (value & ~XSCOM_DATA_IND_FORM1_DATA)
+		return -EINVAL;
+
+	ind_addr = addr & XSCOM_ADDR_FORM1_LOW;
+	ind_data = value | (addr & XSCOM_ADDR_FORM1_HI) << XSCOM_ADDR_FORM1_HI_SHIFT;
+	return __put_scom(scom, ind_data, ind_addr, status);
+}
+
+static int get_indirect_scom_form0(struct scom_device *scom, uint64_t *value,
+				   uint64_t addr, uint32_t *status)
+{
+	uint64_t ind_data, ind_addr;
+	int rc, retries, err = 0;
+
+	ind_addr = addr & XSCOM_ADDR_DIRECT_PART;
+	ind_data = (addr & XSCOM_ADDR_INDIRECT_PART) | XSCOM_DATA_IND_READ;
+	rc = __put_scom(scom, ind_data, ind_addr, status);
+	if (rc || (*status & SCOM_STATUS_ANY_ERR))
+		return rc;
+
+	for (retries = 0; retries < SCOM_MAX_IND_RETRIES; retries++) {
+		rc = __get_scom(scom, &ind_data, addr, status);
+		if (rc || (*status & SCOM_STATUS_ANY_ERR))
+			return rc;
+
+		err = (ind_data & XSCOM_DATA_IND_ERR_MASK) >> XSCOM_DATA_IND_ERR_SHIFT;
+		*status = err << SCOM_STATUS_PIB_RESP_SHIFT;
+		*value = ind_data & XSCOM_DATA_IND_DATA;
+
+		if ((ind_data & XSCOM_DATA_IND_COMPLETE) || (err != SCOM_PIB_BLOCKED))
+			return 0;
+
+		msleep(1);
+	}
+	return rc;
+}
+
+static int raw_put_scom(struct scom_device *scom, uint64_t value,
+			uint64_t addr, uint32_t *status)
+{
+	if (addr & XSCOM_ADDR_IND_FLAG) {
+		if (addr & XSCOM_ADDR_INF_FORM1)
+			return put_indirect_scom_form1(scom, value, addr, status);
+		else
+			return put_indirect_scom_form0(scom, value, addr, status);
+	} else
+		return __put_scom(scom, value, addr, status);
+}
+
+static int raw_get_scom(struct scom_device *scom, uint64_t *value,
+			uint64_t addr, uint32_t *status)
+{
+	if (addr & XSCOM_ADDR_IND_FLAG) {
+		if (addr & XSCOM_ADDR_INF_FORM1)
+			return -ENXIO;
+		return get_indirect_scom_form0(scom, value, addr, status);
+	} else
+		return __get_scom(scom, value, addr, status);
+}
+
+static int handle_fsi2pib_status(struct scom_device *scom, uint32_t status)
+{
+	uint32_t dummy = -1;
+
+	if (status & SCOM_STATUS_PROTECTION)
+		return -EPERM;
+	if (status & SCOM_STATUS_PARITY) {
+		fsi_device_write(scom->fsi_dev, SCOM_FSI2PIB_RESET_REG, &dummy,
+				 sizeof(uint32_t));
+		return -EIO;
+	}
+	/* Return -EBUSY on PIB abort to force a retry */
+	if (status & SCOM_STATUS_PIB_ABORT)
+		return -EBUSY;
+	if (status & SCOM_STATUS_ERR_SUMMARY) {
+		fsi_device_write(scom->fsi_dev, SCOM_FSI2PIB_RESET_REG, &dummy,
+				 sizeof(uint32_t));
+		return -EIO;
+	}
+	return 0;
+}
+
+static int handle_pib_status(struct scom_device *scom, uint8_t status)
+{
+	uint32_t dummy = -1;
+
+	if (status == SCOM_PIB_SUCCESS)
+		return 0;
+	if (status == SCOM_PIB_BLOCKED)
+		return -EBUSY;
+
+	/* Reset the bridge */
+	fsi_device_write(scom->fsi_dev, SCOM_FSI2PIB_RESET_REG, &dummy,
+			 sizeof(uint32_t));
+
+	switch(status) {
+	case SCOM_PIB_OFFLINE:
+		return -ENODEV;
+	case SCOM_PIB_BAD_ADDR:
+		return -ENXIO;
+	case SCOM_PIB_TIMEOUT:
+		return -ETIMEDOUT;
+	case SCOM_PIB_PARTIAL:
+	case SCOM_PIB_CLK_ERR:
+	case SCOM_PIB_PARITY_ERR:
+	default:
+		return -EIO;
+	}
+}
+
+static int put_scom(struct scom_device *scom, uint64_t value,
+		    uint64_t addr)
+{
+	uint32_t status, dummy = -1;
+	int rc, retries;
+
+	for (retries = 0; retries < SCOM_MAX_RETRIES; retries++) {
+		rc = raw_put_scom(scom, value, addr, &status);
+		if (rc) {
+			/* Try resetting the bridge if FSI fails */
+			if (rc != -ENODEV && retries == 0) {
+				fsi_device_write(scom->fsi_dev, SCOM_FSI2PIB_RESET_REG,
+						 &dummy, sizeof(uint32_t));
+				rc = -EBUSY;
+			} else
+				return rc;
+		} else
+			rc = handle_fsi2pib_status(scom, status);
+		if (rc && rc != -EBUSY)
+			break;
+		if (rc == 0) {
+			rc = handle_pib_status(scom,
+					       (status & SCOM_STATUS_PIB_RESP_MASK)
+					       >> SCOM_STATUS_PIB_RESP_SHIFT);
+			if (rc && rc != -EBUSY)
+				break;
+		}
+		if (rc == 0)
+			break;
+		msleep(1);
+	}
+	return rc;
+}
+
+static int get_scom(struct scom_device *scom, uint64_t *value,
+		    uint64_t addr)
+{
+	uint32_t status, dummy = -1;
+	int rc, retries;
+
+	for (retries = 0; retries < SCOM_MAX_RETRIES; retries++) {
+		rc = raw_get_scom(scom, value, addr, &status);
+		if (rc) {
+			/* Try resetting the bridge if FSI fails */
+			if (rc != -ENODEV && retries == 0) {
+				fsi_device_write(scom->fsi_dev, SCOM_FSI2PIB_RESET_REG,
+						 &dummy, sizeof(uint32_t));
+				rc = -EBUSY;
+			} else
+				return rc;
+		} else
+			rc = handle_fsi2pib_status(scom, status);
+		if (rc && rc != -EBUSY)
+			break;
+		if (rc == 0) {
+			rc = handle_pib_status(scom,
+					       (status & SCOM_STATUS_PIB_RESP_MASK)
+					       >> SCOM_STATUS_PIB_RESP_SHIFT);
+			if (rc && rc != -EBUSY)
+				break;
+		}
+		if (rc == 0)
+			break;
+		msleep(1);
+	}
+	return rc;
+}
+
+static ssize_t scom_read(struct file *filep, char __user *buf, size_t len,
+			 loff_t *offset)
+{
 	struct miscdevice *mdev =
 				(struct miscdevice *)filep->private_data;
 	struct scom_device *scom = to_scom_dev(mdev);
 	struct device *dev = &scom->fsi_dev->dev;
 	uint64_t val;
+	int rc;
 
 	if (len != sizeof(uint64_t))
 		return -EINVAL;
 
+	mutex_lock(&scom->lock);
 	rc = get_scom(scom, &val, *offset);
+	mutex_unlock(&scom->lock);
 	if (rc) {
 		dev_dbg(dev, "get_scom fail:%d\n", rc);
 		return rc;
@@ -126,7 +400,7 @@ static ssize_t scom_read(struct file *filep, char __user *buf, size_t len,
 }
 
 static ssize_t scom_write(struct file *filep, const char __user *buf,
-			size_t len, loff_t *offset)
+			  size_t len, loff_t *offset)
 {
 	int rc;
 	struct miscdevice *mdev = filep->private_data;
@@ -143,7 +417,9 @@ static ssize_t scom_write(struct file *filep, const char __user *buf,
 		return -EINVAL;
 	}
 
+	mutex_lock(&scom->lock);
 	rc = put_scom(scom, val, *offset);
+	mutex_unlock(&scom->lock);
 	if (rc) {
 		dev_dbg(dev, "put_scom failed with:%d\n", rc);
 		return rc;
@@ -167,11 +443,125 @@ static loff_t scom_llseek(struct file *file, loff_t offset, int whence)
 	return offset;
 }
 
+static void raw_convert_status(struct scom_access *acc, uint32_t status)
+{
+	acc->pib_status = (status & SCOM_STATUS_PIB_RESP_MASK) >>
+		SCOM_STATUS_PIB_RESP_SHIFT;
+	acc->intf_errors = 0;
+
+	if (status & SCOM_STATUS_PROTECTION)
+		acc->intf_errors |= SCOM_INTF_ERR_PROTECTION;
+	else if (status & SCOM_STATUS_PARITY)
+		acc->intf_errors |= SCOM_INTF_ERR_PARITY;
+	else if (status & SCOM_STATUS_PIB_ABORT)
+		acc->intf_errors |= SCOM_INTF_ERR_ABORT;
+	else if (status & SCOM_STATUS_ERR_SUMMARY)
+		acc->intf_errors |= SCOM_INTF_ERR_UNKNOWN;
+}
+
+static int scom_raw_read(struct scom_device *scom, void __user *argp)
+{
+	struct scom_access acc;
+	uint32_t status;
+	int rc;
+
+	if (copy_from_user(&acc, argp, sizeof(struct scom_access)))
+		return -EFAULT;
+
+	rc = raw_get_scom(scom, &acc.data, acc.addr, &status);
+	if (rc)
+		return rc;
+	raw_convert_status(&acc, status);
+	if (copy_to_user(argp, &acc, sizeof(struct scom_access)))
+		return -EFAULT;
+	return 0;
+}
+
+static int scom_raw_write(struct scom_device *scom, void __user *argp)
+{
+	u64 prev_data, mask, data;
+	struct scom_access acc;
+	uint32_t status;
+	int rc;
+
+	if (copy_from_user(&acc, argp, sizeof(struct scom_access)))
+		return -EFAULT;
+
+	if (acc.mask) {
+		rc = raw_get_scom(scom, &prev_data, acc.addr, &status);
+		if (rc)
+			return rc;
+		if (status & SCOM_STATUS_ANY_ERR)
+			goto fail;
+		mask = acc.mask;
+	} else {
+		prev_data = mask = -1ull;
+	}
+	data = (prev_data & ~mask) | (acc.data & mask);
+	rc = raw_put_scom(scom, data, acc.addr, &status);
+	if (rc)
+		return rc;
+ fail:
+	raw_convert_status(&acc, status);
+	if (copy_to_user(argp, &acc, sizeof(struct scom_access)))
+		return -EFAULT;
+	return 0;
+}
+
+static int scom_reset(struct scom_device *scom, void __user *argp)
+{
+	uint32_t flags, dummy = -1;
+	int rc = 0;
+
+	if (get_user(flags, (__u32 __user *)argp))
+		return -EFAULT;
+	if (flags & SCOM_RESET_PIB)
+		rc = fsi_device_write(scom->fsi_dev, SCOM_PIB_RESET_REG, &dummy,
+				      sizeof(uint32_t));
+	if (!rc && (flags & (SCOM_RESET_PIB | SCOM_RESET_INTF)))
+		rc = fsi_device_write(scom->fsi_dev, SCOM_FSI2PIB_RESET_REG, &dummy,
+				      sizeof(uint32_t));
+	return rc;
+}
+
+static int scom_check(struct scom_device *scom, void __user *argp)
+{
+	/* Still need to find out how to get "protected" */
+	return put_user(SCOM_CHECK_SUPPORTED, (__u32 __user *)argp);
+}
+
+static long scom_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct miscdevice *mdev = file->private_data;
+	struct scom_device *scom = to_scom_dev(mdev);
+	void __user *argp = (void __user *)arg;
+	int rc = -ENOTTY;
+
+	mutex_lock(&scom->lock);
+	switch(cmd) {
+	case FSI_SCOM_CHECK:
+		rc = scom_check(scom, argp);
+		break;
+	case FSI_SCOM_READ:
+		rc = scom_raw_read(scom, argp);
+		break;
+	case FSI_SCOM_WRITE:
+		rc = scom_raw_write(scom, argp);
+		break;
+	case FSI_SCOM_RESET:
+		rc = scom_reset(scom, argp);
+		break;
+	}
+	mutex_unlock(&scom->lock);
+	return rc;
+}
+
 static const struct file_operations scom_fops = {
-	.owner	= THIS_MODULE,
-	.llseek	= scom_llseek,
-	.read	= scom_read,
-	.write	= scom_write,
+	.owner		= THIS_MODULE,
+	.llseek		= scom_llseek,
+	.read		= scom_read,
+	.write		= scom_write,
+	.unlocked_ioctl	= scom_ioctl,
 };
 
 static int scom_probe(struct device *dev)
@@ -183,6 +573,7 @@ static int scom_probe(struct device *dev)
 	if (!scom)
 		return -ENOMEM;
 
+	mutex_init(&scom->lock);
 	scom->idx = ida_simple_get(&scom_ida, 1, INT_MAX, GFP_KERNEL);
 	snprintf(scom->name, sizeof(scom->name), "scom%d", scom->idx);
 	scom->fsi_dev = fsi_dev;
diff --git a/include/trace/events/fsi_master_gpio.h b/include/trace/events/fsi_master_gpio.h
index 389082132433..70ef66e63e84 100644
--- a/include/trace/events/fsi_master_gpio.h
+++ b/include/trace/events/fsi_master_gpio.h
@@ -50,6 +50,22 @@ TRACE_EVENT(fsi_master_gpio_out,
 	)
 );
 
+TRACE_EVENT(fsi_master_gpio_clock_zeros,
+	TP_PROTO(const struct fsi_master_gpio *master, int clocks),
+	TP_ARGS(master, clocks),
+	TP_STRUCT__entry(
+		__field(int,	master_idx)
+		__field(int,	clocks)
+	),
+	TP_fast_assign(
+		__entry->master_idx = master->master.idx;
+		__entry->clocks = clocks;
+	),
+	TP_printk("fsi-gpio%d clock %d zeros",
+		  __entry->master_idx, __entry->clocks
+	)
+);
+
 TRACE_EVENT(fsi_master_gpio_break,
 	TP_PROTO(const struct fsi_master_gpio *master),
 	TP_ARGS(master),
@@ -107,6 +123,49 @@ TRACE_EVENT(fsi_master_gpio_poll_response_busy,
 		__entry->master_idx, __entry->busy)
 );
 
+TRACE_EVENT(fsi_master_gpio_cmd_abs_addr,
+	TP_PROTO(const struct fsi_master_gpio *master, u32 addr),
+	TP_ARGS(master, addr),
+	TP_STRUCT__entry(
+		__field(int,	master_idx)
+		__field(u32,	addr)
+	),
+	TP_fast_assign(
+		__entry->master_idx = master->master.idx;
+		__entry->addr = addr;
+	),
+	TP_printk("fsi-gpio%d: Sending ABS_ADR %06x",
+		__entry->master_idx, __entry->addr)
+);
+
+TRACE_EVENT(fsi_master_gpio_cmd_rel_addr,
+	TP_PROTO(const struct fsi_master_gpio *master, u32 rel_addr),
+	TP_ARGS(master, rel_addr),
+	TP_STRUCT__entry(
+		__field(int,	master_idx)
+		__field(u32,	rel_addr)
+	),
+	TP_fast_assign(
+		__entry->master_idx = master->master.idx;
+		__entry->rel_addr = rel_addr;
+	),
+	TP_printk("fsi-gpio%d: Sending REL_ADR %03x",
+		__entry->master_idx, __entry->rel_addr)
+);
+
+TRACE_EVENT(fsi_master_gpio_cmd_same_addr,
+	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: Sending SAME_ADR",
+		__entry->master_idx)
+);
+
 #endif /* _TRACE_FSI_MASTER_GPIO_H */
 
 #include <trace/define_trace.h>
diff --git a/include/uapi/linux/fsi.h b/include/uapi/linux/fsi.h
new file mode 100644
index 000000000000..da577ecd90e7
--- /dev/null
+++ b/include/uapi/linux/fsi.h
@@ -0,0 +1,58 @@
+/* SPDX-License-Identifier: GPL-2.0+ WITH Linux-syscall-note */
+#ifndef _UAPI_LINUX_FSI_H
+#define _UAPI_LINUX_FSI_H
+
+#include <linux/types.h>
+#include <linux/ioctl.h>
+
+/*
+ * /dev/scom "raw" ioctl interface
+ *
+ * The driver supports a high level "read/write" interface which
+ * handles retries and converts the status to Linux error codes,
+ * however low level tools an debugger need to access the "raw"
+ * HW status information and interpret it themselves, so this
+ * ioctl interface is also provided for their use case.
+ */
+
+/* Structure for SCOM read/write */
+struct scom_access {
+	__u64	addr;		/* SCOM address, supports indirect */
+	__u64	data;		/* SCOM data (in for write, out for read) */
+	__u64	mask;		/* Data mask for writes */
+	__u32	intf_errors;	/* Interface error flags */
+#define SCOM_INTF_ERR_PARITY		0x00000001 /* Parity error */
+#define SCOM_INTF_ERR_PROTECTION	0x00000002 /* Blocked by secure boot */
+#define SCOM_INTF_ERR_ABORT		0x00000004 /* PIB reset during access */
+#define SCOM_INTF_ERR_UNKNOWN		0x80000000 /* Unknown error */
+	/*
+	 * Note: Any other bit set in intf_errors need to be considered as an
+	 * error. Future implementations may define new error conditions. The
+	 * pib_status below is only valid if intf_errors is 0.
+	 */
+	__u8	pib_status;	/* 3-bit PIB status */
+#define SCOM_PIB_SUCCESS	0	/* Access successful */
+#define SCOM_PIB_BLOCKED	1	/* PIB blocked, pls retry */
+#define SCOM_PIB_OFFLINE	2	/* Chiplet offline */
+#define SCOM_PIB_PARTIAL	3	/* Partial good */
+#define SCOM_PIB_BAD_ADDR	4	/* Invalid address */
+#define SCOM_PIB_CLK_ERR	5	/* Clock error */
+#define SCOM_PIB_PARITY_ERR	6	/* Parity error on the PIB bus */
+#define SCOM_PIB_TIMEOUT	7	/* Bus timeout */
+	__u8	pad;
+};
+
+/* Flags for SCOM check */
+#define SCOM_CHECK_SUPPORTED	0x00000001	/* Interface supported */
+#define SCOM_CHECK_PROTECTED	0x00000002	/* Interface blocked by secure boot */
+
+/* Flags for SCOM reset */
+#define SCOM_RESET_INTF		0x00000001	/* Reset interface */
+#define SCOM_RESET_PIB		0x00000002	/* Reset PIB */
+
+#define FSI_SCOM_CHECK	_IOR('s', 0x00, __u32)
+#define FSI_SCOM_READ	_IOWR('s', 0x01, struct scom_access)
+#define FSI_SCOM_WRITE	_IOWR('s', 0x02, struct scom_access)
+#define FSI_SCOM_RESET	_IOW('s', 0x03, __u32)
+
+#endif /* _UAPI_LINUX_FSI_H */