drivers/fsi: Set up links for slave communication

Enable each link and send a break command, and try to detect a slave by
reading from the SMODE register.
Signed-off-by: NChristopher Bostic <cbostic@linux.vnet.ibm.com>
Signed-off-by: NJoel Stanley <joel@jms.id.au>
Signed-off-by: NGreg Kroah-Hartman <gregkh@linuxfoundation.org>

drivers/fsi/fsi-core.c

@@ -133,12 +133,45 @@ static int fsi_master_write(struct fsi_master *master, int link,
 	return master->write(master, link, slave_id, addr, val, size);
 }
 
+static int fsi_master_link_enable(struct fsi_master *master, int link)
+{
+	if (master->link_enable)
+		return master->link_enable(master, link);
+
+	return 0;
+}
+
+/*
+ * Issue a break command on this link
+ */
+static int fsi_master_break(struct fsi_master *master, int link)
+{
+	if (master->send_break)
+		return master->send_break(master, link);
+
+	return 0;
+}
+
 static int fsi_master_scan(struct fsi_master *master)
 {
-	int link;
+	int link, rc;
+
+	for (link = 0; link < master->n_links; link++) {
+		rc = fsi_master_link_enable(master, link);
+		if (rc) {
+			dev_dbg(&master->dev,
+				"enable link %d failed: %d\n", link, rc);
+			continue;
+		}
+		rc = fsi_master_break(master, link);
+		if (rc) {
+			dev_dbg(&master->dev,
+				"break to link %d failed: %d\n", link, rc);
+			continue;
+		}
 
-	for (link = 0; link < master->n_links; link++)
 		fsi_slave_init(master, link, 0);
+	}
 
 	return 0;
 }