[PATCH linux dev-4.7 3/5] drivers/fsi: Define hub master read write

[Christopher Bostic]

Define the master read, write, link enable, and break methods.
Register the hub master with the core.

Signed-off-by: Christopher Bostic <cbostic at linux.vnet.ibm.com>
---
 drivers/fsi/fsi-master-hub.c | 30 ++++++++++++++++++++++++++----
 1 file changed, 26 insertions(+), 4 deletions(-)

diff --git a/drivers/fsi/fsi-master-hub.c b/drivers/fsi/fsi-master-hub.c
index e8c0726..ea6db17 100644
--- a/drivers/fsi/fsi-master-hub.c
+++ b/drivers/fsi/fsi-master-hub.c
@@ -34,23 +34,38 @@
 int hub_master_read(struct fsi_master *master, int linkno, uint8_t slave,
 			uint32_t addr, void *val, size_t size)
 {
-	return 0;
+	addr += (linkno * FSI_MASTER_HUB_LINK_SIZE);
+	return fsi_device_read(master->link, addr, val, size);
 }
 
 int hub_master_write(struct fsi_master *master, int linkno, uint8_t slave,
 			uint32_t addr, const void *val, size_t size)
 {
-	return 0;
+	addr += (linkno * FSI_MASTER_HUB_LINK_SIZE);
+	return fsi_device_write(master->link, addr, val, size);
 }
 
 int hub_master_break(struct fsi_master *master, int linkno)
 {
-	return 0;
+	uint32_t command;
+	uint32_t break_offset = 0x4; /* hw workaround */
+
+	command = FSI_BREAK;
+	return fsi_device_write(master->link, break_offset, &command,
+				sizeof(command));
 }
 
 int hub_master_link_enable(struct fsi_master *master, int link)
 {
-	return 0;
+	uint32_t menp = L_MSB_MASK(link);
+	int rc;
+
+	rc = fsi_device_write(master->engine, FSI_MSENP0, &menp,
+				sizeof(menp));
+	/* Wait for hw to finish enable */
+	mdelay(10);
+
+	return rc;
 }
 
 static int hub_master_probe(struct device *dev)
@@ -178,6 +193,13 @@ static int hub_master_probe(struct device *dev)
 			master->link = fsi_dev;
 		}
 		master->n_links++;
+
+		/* Each hub links is listed twice in CFAM config space */
+		if (master->n_links == (FSI_MASTER_HUB_MAX_LINKS * 2)) {
+			/* Only need hub link 1 for now */
+			master->n_links = 2;
+			return fsi_master_register(master);
+		}
 	}
 
 	return 0;
-- 
1.8.2.2