[PATCH linux dev-4.7] drivers/fsi: Adjust slave ID based on address in command send

[Christopher Bostic]

In order to access slave address ranges > 0x1FFFFF the slave ID
must be adjusted accordingly in the command encoding:

ID 0: 0x00000000 - 0x001FFFFF
ID 1: 0x00200000 - 0x003FFFFF
ID 2: 0x00400000 - 0x005FFFFF
ID 3: 0x00600000 - 0x007FFFFF

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

diff --git a/drivers/fsi/fsi-master-gpio.c b/drivers/fsi/fsi-master-gpio.c
index 73d9985..428f3db 100644
--- a/drivers/fsi/fsi-master-gpio.c
+++ b/drivers/fsi/fsi-master-gpio.c
@@ -30,7 +30,9 @@
 #define	FSI_GPIO_CMD_READ	0x0400000000000000ULL
 #define	FSI_GPIO_CMD_SLAVE_MASK	0xC000000000000000ULL
 #define	FSI_GPIO_CMD_ADDR_SHIFT	37
-#define	FSI_GPIO_CMD_ADDR_MASK	0x001FFFFF
+#define	FSI_GPIO_CMD_ADR8_MASK	0x001FFFFF
+#define	FSI_GPIO_CMD_ADR16_MASK	0x001FFFFE
+#define	FSI_GPIO_CMD_ADR32_MASK	0x001FFFFC
 #define	FSI_GPIO_CMD_SLV_SHIFT	62
 #define	FSI_GPIO_CMD_SIZE_16	0x0000001000000000ULL
 #define	FSI_GPIO_CMD_SIZE_32	0x0000003000000000ULL
@@ -40,6 +42,9 @@
 #define	FSI_GPIO_CMD_DFLT_LEN	28
 #define	FSI_GPIO_CMD_CRC_SHIFT	60
 
+#define	FSI_SLAVE_SHIFT		21
+#define	FSI_SLAVE_MASK		0x3
+
 /* Bus errors */
 #define	FSI_GPIO_ERR_BUSY	1	/* Slave stuck in busy state */
 #define	FSI_GPIO_RESP_ERRA	2	/* Any (misc) Error */
@@ -301,15 +306,17 @@ static void build_abs_ar_command(struct fsi_gpio_msg *cmd, uint64_t mode,
 		const void *data)
 {
 	uint8_t crc;
+	uint32_t addr_mask;
 
 	cmd->bits = FSI_GPIO_CMD_DFLT_LEN;
 	cmd->msg = FSI_GPIO_CMD_DEFAULT;
 	cmd->msg |= mode;
-	cmd->msg &= ~FSI_GPIO_CMD_SLAVE_MASK;
+	if (!slave)
+		slave = (addr >> FSI_SLAVE_SHIFT) & FSI_SLAVE_MASK;
 	cmd->msg |= (((uint64_t)slave) << FSI_GPIO_CMD_SLV_SHIFT);
-	addr &= FSI_GPIO_CMD_ADDR_MASK;
-	cmd->msg |= (((uint64_t)addr) << FSI_GPIO_CMD_ADDR_SHIFT);
+
 	if (size == sizeof(uint8_t)) {
+		addr_mask = FSI_GPIO_CMD_ADR8_MASK;
 		if (data) {
 			uint8_t cmd_data = *((uint8_t *)data);
 
@@ -317,6 +324,7 @@ static void build_abs_ar_command(struct fsi_gpio_msg *cmd, uint64_t mode,
 				((uint64_t)cmd_data) << FSI_GPIO_CMD_DT8_SHIFT;
 		}
 	} else if (size == sizeof(uint16_t)) {
+		addr_mask = FSI_GPIO_CMD_ADR16_MASK;
 		cmd->msg |= FSI_GPIO_CMD_SIZE_16;
 		if (data) {
 			uint16_t cmd_data;
@@ -326,6 +334,7 @@ static void build_abs_ar_command(struct fsi_gpio_msg *cmd, uint64_t mode,
 				((uint64_t)cmd_data) << FSI_GPIO_CMD_DT16_SHIFT;
 		}
 	} else {
+		addr_mask = FSI_GPIO_CMD_ADR32_MASK;
 		cmd->msg |= FSI_GPIO_CMD_SIZE_32;
 		if (data) {
 			uint32_t cmd_data;
@@ -336,6 +345,9 @@ static void build_abs_ar_command(struct fsi_gpio_msg *cmd, uint64_t mode,
 		}
 	}
 
+	addr &= addr_mask;
+	cmd->msg |= (((uint64_t)addr) << FSI_GPIO_CMD_ADDR_SHIFT);
+
 	if (mode == FSI_GPIO_CMD_WRITE)
 		cmd->bits += (8 * size);
 
-- 
1.8.2.2