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

[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>

---

Changes in V2
    - Remove alignment masking as per recommendation of Alistair Popple
      <alistair at popple.id.au>
---
 drivers/fsi/fsi-master-gpio.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/drivers/fsi/fsi-master-gpio.c b/drivers/fsi/fsi-master-gpio.c
index 73d9985..91bdbf2 100644
--- a/drivers/fsi/fsi-master-gpio.c
+++ b/drivers/fsi/fsi-master-gpio.c
@@ -40,6 +40,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 */
@@ -305,7 +308,8 @@ static void build_abs_ar_command(struct fsi_gpio_msg *cmd, uint64_t mode,
 	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);
-- 
1.8.2.2