Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-03 Thread Vinod Koul 
On Sun, Dec 03, 2017 at 09:07:29PM -0600, Pierre-Louis Bossart wrote:
> On 12/3/17 11:08 AM, Vinod Koul wrote:
> >On Fri, Dec 01, 2017 at 05:36:47PM -0600, Pierre-Louis Bossart wrote:
> >
> >>>+/* called with bus_lock held */
> >>>+static int sdw_get_device_num(struct sdw_slave *slave)
> >>>+{
> >>>+  int bit;
> >>>+
> >>>+  bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
> >>>+  if (bit == SDW_MAX_DEVICES) {
> >>>+  bit = -ENODEV;
> >>>+  goto err;
> >>
> >>My brain is starting to fry but is this correct? Bit11 seems like a valid
> >>value. Should it be bit > 15 (assuming bit 12,13,14 are set to avoid using
> >>groups and master)?
> >
> >this is correct. You are confusing SDW concept and API return types!
> >That should be hint for you to start weekend if you didn't do so :D
> >
> >This API returns max value it was provided (last arg) if it doesn't
> >find free bit. That's an indication to caller that we ran out of devices
> >hence ENODEV error!
> 
> Can you just make sure bit11 is included?

yes it is, refer to the masks we set for bit, only 0 and 15 and now 12,13
and 14 will be masked out. So we can get from 1 to 11 both inclusive.

-- 
~Vinod

Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-03 Thread Vinod Koul 
On Sun, Dec 03, 2017 at 09:07:29PM -0600, Pierre-Louis Bossart wrote:
> On 12/3/17 11:08 AM, Vinod Koul wrote:
> >On Fri, Dec 01, 2017 at 05:36:47PM -0600, Pierre-Louis Bossart wrote:
> >
> >>>+/* called with bus_lock held */
> >>>+static int sdw_get_device_num(struct sdw_slave *slave)
> >>>+{
> >>>+  int bit;
> >>>+
> >>>+  bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
> >>>+  if (bit == SDW_MAX_DEVICES) {
> >>>+  bit = -ENODEV;
> >>>+  goto err;
> >>
> >>My brain is starting to fry but is this correct? Bit11 seems like a valid
> >>value. Should it be bit > 15 (assuming bit 12,13,14 are set to avoid using
> >>groups and master)?
> >
> >this is correct. You are confusing SDW concept and API return types!
> >That should be hint for you to start weekend if you didn't do so :D
> >
> >This API returns max value it was provided (last arg) if it doesn't
> >find free bit. That's an indication to caller that we ran out of devices
> >hence ENODEV error!
> 
> Can you just make sure bit11 is included?

yes it is, refer to the masks we set for bit, only 0 and 15 and now 12,13
and 14 will be masked out. So we can get from 1 to 11 both inclusive.

-- 
~Vinod

Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-03 Thread Pierre-Louis Bossart 

On 12/3/17 11:08 AM, Vinod Koul wrote:

On Fri, Dec 01, 2017 at 05:36:47PM -0600, Pierre-Louis Bossart wrote:


+/* called with bus_lock held */
+static int sdw_get_device_num(struct sdw_slave *slave)
+{
+   int bit;
+
+   bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
+   if (bit == SDW_MAX_DEVICES) {
+   bit = -ENODEV;
+   goto err;


My brain is starting to fry but is this correct? Bit11 seems like a valid
value. Should it be bit > 15 (assuming bit 12,13,14 are set to avoid using
groups and master)?


this is correct. You are confusing SDW concept and API return types!
That should be hint for you to start weekend if you didn't do so :D

This API returns max value it was provided (last arg) if it doesn't
find free bit. That's an indication to caller that we ran out of devices
hence ENODEV error!


Can you just make sure bit11 is included?




+static int sdw_program_device_num(struct sdw_bus *bus)
+{
+   u8 buf[SDW_NUM_DEV_ID_REGISTERS] = {0};
+   struct sdw_slave *slave, *_s;
+   struct sdw_slave_id id;
+   struct sdw_msg msg;
+   bool found = false;
+   int count = 0, ret;
+   u64 addr;
+
+   /* No Slave, so use raw xfer api */
+   ret = sdw_fill_msg(, NULL, SDW_SCP_DEVID_0,
+   SDW_NUM_DEV_ID_REGISTERS, 0, SDW_MSG_FLAG_READ, buf);
+   if (ret < 0)
+   return ret;
+
+   do {
+   ret = sdw_transfer(bus, NULL, );
+   if (ret == -ENODATA) { /* end of device id reads */
+   ret = 0;
+   break;
+   }
+   if (ret < 0) {
+   dev_err(bus->dev, "DEVID read fail:%d\n", ret);
+   break;
+   }
+
+   /*
+* Construct the addr and extract. Cast the higher shift
+* bits to avoid truncation due to size limit.
+*/
+   addr = buf[5] | (buf[4] << 8) | (buf[3] << 16) |
+   (buf[2] << 24) | ((unsigned long long)buf[1] << 32) |
+   ((unsigned long long)buf[0] << 40);
+
+   sdw_extract_slave_id(bus, addr, );
+
+   /* Now compare with entries */
+   list_for_each_entry_safe(slave, _s, >slaves, node) {
+   if (sdw_compare_devid(slave, id) == 0) {
+   found = true;
+
+   /*
+* Assign a new dev_num to this Slave and
+* not mark it present. It will be marked
+* present after it reports ATTACHED on new
+* dev_num
+*/
+   ret = sdw_assign_device_num(slave);
+   if (ret) {
+   dev_err(slave->bus->dev,
+   "Assign dev_num failed:%d",
+   ret);
+   return ret;
+   }
+
+   break;
+   }
+   }
+
+   if (found == false) {
+   /* TODO: Park this device in Group 13 */
+   dev_err(bus->dev, "Slave Entry not found");
+   }
+
+   count++;
+
+   } while (ret == 0 && count < (SDW_MAX_DEVICES * 2));


explain that the last condition is intentional - this is not a bug -, some
devices can drop off during enumeration and rejoin so might be counted
twice.


ok will add

Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-03 Thread Pierre-Louis Bossart 

On 12/3/17 11:08 AM, Vinod Koul wrote:

On Fri, Dec 01, 2017 at 05:36:47PM -0600, Pierre-Louis Bossart wrote:


+/* called with bus_lock held */
+static int sdw_get_device_num(struct sdw_slave *slave)
+{
+   int bit;
+
+   bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
+   if (bit == SDW_MAX_DEVICES) {
+   bit = -ENODEV;
+   goto err;


My brain is starting to fry but is this correct? Bit11 seems like a valid
value. Should it be bit > 15 (assuming bit 12,13,14 are set to avoid using
groups and master)?


this is correct. You are confusing SDW concept and API return types!
That should be hint for you to start weekend if you didn't do so :D

This API returns max value it was provided (last arg) if it doesn't
find free bit. That's an indication to caller that we ran out of devices
hence ENODEV error!


Can you just make sure bit11 is included?




+static int sdw_program_device_num(struct sdw_bus *bus)
+{
+   u8 buf[SDW_NUM_DEV_ID_REGISTERS] = {0};
+   struct sdw_slave *slave, *_s;
+   struct sdw_slave_id id;
+   struct sdw_msg msg;
+   bool found = false;
+   int count = 0, ret;
+   u64 addr;
+
+   /* No Slave, so use raw xfer api */
+   ret = sdw_fill_msg(, NULL, SDW_SCP_DEVID_0,
+   SDW_NUM_DEV_ID_REGISTERS, 0, SDW_MSG_FLAG_READ, buf);
+   if (ret < 0)
+   return ret;
+
+   do {
+   ret = sdw_transfer(bus, NULL, );
+   if (ret == -ENODATA) { /* end of device id reads */
+   ret = 0;
+   break;
+   }
+   if (ret < 0) {
+   dev_err(bus->dev, "DEVID read fail:%d\n", ret);
+   break;
+   }
+
+   /*
+* Construct the addr and extract. Cast the higher shift
+* bits to avoid truncation due to size limit.
+*/
+   addr = buf[5] | (buf[4] << 8) | (buf[3] << 16) |
+   (buf[2] << 24) | ((unsigned long long)buf[1] << 32) |
+   ((unsigned long long)buf[0] << 40);
+
+   sdw_extract_slave_id(bus, addr, );
+
+   /* Now compare with entries */
+   list_for_each_entry_safe(slave, _s, >slaves, node) {
+   if (sdw_compare_devid(slave, id) == 0) {
+   found = true;
+
+   /*
+* Assign a new dev_num to this Slave and
+* not mark it present. It will be marked
+* present after it reports ATTACHED on new
+* dev_num
+*/
+   ret = sdw_assign_device_num(slave);
+   if (ret) {
+   dev_err(slave->bus->dev,
+   "Assign dev_num failed:%d",
+   ret);
+   return ret;
+   }
+
+   break;
+   }
+   }
+
+   if (found == false) {
+   /* TODO: Park this device in Group 13 */
+   dev_err(bus->dev, "Slave Entry not found");
+   }
+
+   count++;
+
+   } while (ret == 0 && count < (SDW_MAX_DEVICES * 2));


explain that the last condition is intentional - this is not a bug -, some
devices can drop off during enumeration and rejoin so might be counted
twice.


ok will add

Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-03 Thread Vinod Koul 
On Fri, Dec 01, 2017 at 05:36:47PM -0600, Pierre-Louis Bossart wrote:

> >+/* called with bus_lock held */
> >+static int sdw_get_device_num(struct sdw_slave *slave)
> >+{
> >+int bit;
> >+
> >+bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
> >+if (bit == SDW_MAX_DEVICES) {
> >+bit = -ENODEV;
> >+goto err;
> 
> My brain is starting to fry but is this correct? Bit11 seems like a valid
> value. Should it be bit > 15 (assuming bit 12,13,14 are set to avoid using
> groups and master)?

this is correct. You are confusing SDW concept and API return types!
That should be hint for you to start weekend if you didn't do so :D

This API returns max value it was provided (last arg) if it doesn't
find free bit. That's an indication to caller that we ran out of devices
hence ENODEV error!

> >+static int sdw_program_device_num(struct sdw_bus *bus)
> >+{
> >+u8 buf[SDW_NUM_DEV_ID_REGISTERS] = {0};
> >+struct sdw_slave *slave, *_s;
> >+struct sdw_slave_id id;
> >+struct sdw_msg msg;
> >+bool found = false;
> >+int count = 0, ret;
> >+u64 addr;
> >+
> >+/* No Slave, so use raw xfer api */
> >+ret = sdw_fill_msg(, NULL, SDW_SCP_DEVID_0,
> >+SDW_NUM_DEV_ID_REGISTERS, 0, SDW_MSG_FLAG_READ, buf);
> >+if (ret < 0)
> >+return ret;
> >+
> >+do {
> >+ret = sdw_transfer(bus, NULL, );
> >+if (ret == -ENODATA) { /* end of device id reads */
> >+ret = 0;
> >+break;
> >+}
> >+if (ret < 0) {
> >+dev_err(bus->dev, "DEVID read fail:%d\n", ret);
> >+break;
> >+}
> >+
> >+/*
> >+ * Construct the addr and extract. Cast the higher shift
> >+ * bits to avoid truncation due to size limit.
> >+ */
> >+addr = buf[5] | (buf[4] << 8) | (buf[3] << 16) |
> >+(buf[2] << 24) | ((unsigned long long)buf[1] << 32) |
> >+((unsigned long long)buf[0] << 40);
> >+
> >+sdw_extract_slave_id(bus, addr, );
> >+
> >+/* Now compare with entries */
> >+list_for_each_entry_safe(slave, _s, >slaves, node) {
> >+if (sdw_compare_devid(slave, id) == 0) {
> >+found = true;
> >+
> >+/*
> >+ * Assign a new dev_num to this Slave and
> >+ * not mark it present. It will be marked
> >+ * present after it reports ATTACHED on new
> >+ * dev_num
> >+ */
> >+ret = sdw_assign_device_num(slave);
> >+if (ret) {
> >+dev_err(slave->bus->dev,
> >+"Assign dev_num failed:%d",
> >+ret);
> >+return ret;
> >+}
> >+
> >+break;
> >+}
> >+}
> >+
> >+if (found == false) {
> >+/* TODO: Park this device in Group 13 */
> >+dev_err(bus->dev, "Slave Entry not found");
> >+}
> >+
> >+count++;
> >+
> >+} while (ret == 0 && count < (SDW_MAX_DEVICES * 2));
> 
> explain that the last condition is intentional - this is not a bug -, some
> devices can drop off during enumeration and rejoin so might be counted
> twice.

ok will add

-- 
~Vinod

Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-03 Thread Vinod Koul 
On Fri, Dec 01, 2017 at 05:36:47PM -0600, Pierre-Louis Bossart wrote:

> >+/* called with bus_lock held */
> >+static int sdw_get_device_num(struct sdw_slave *slave)
> >+{
> >+int bit;
> >+
> >+bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
> >+if (bit == SDW_MAX_DEVICES) {
> >+bit = -ENODEV;
> >+goto err;
> 
> My brain is starting to fry but is this correct? Bit11 seems like a valid
> value. Should it be bit > 15 (assuming bit 12,13,14 are set to avoid using
> groups and master)?

this is correct. You are confusing SDW concept and API return types!
That should be hint for you to start weekend if you didn't do so :D

This API returns max value it was provided (last arg) if it doesn't
find free bit. That's an indication to caller that we ran out of devices
hence ENODEV error!

> >+static int sdw_program_device_num(struct sdw_bus *bus)
> >+{
> >+u8 buf[SDW_NUM_DEV_ID_REGISTERS] = {0};
> >+struct sdw_slave *slave, *_s;
> >+struct sdw_slave_id id;
> >+struct sdw_msg msg;
> >+bool found = false;
> >+int count = 0, ret;
> >+u64 addr;
> >+
> >+/* No Slave, so use raw xfer api */
> >+ret = sdw_fill_msg(, NULL, SDW_SCP_DEVID_0,
> >+SDW_NUM_DEV_ID_REGISTERS, 0, SDW_MSG_FLAG_READ, buf);
> >+if (ret < 0)
> >+return ret;
> >+
> >+do {
> >+ret = sdw_transfer(bus, NULL, );
> >+if (ret == -ENODATA) { /* end of device id reads */
> >+ret = 0;
> >+break;
> >+}
> >+if (ret < 0) {
> >+dev_err(bus->dev, "DEVID read fail:%d\n", ret);
> >+break;
> >+}
> >+
> >+/*
> >+ * Construct the addr and extract. Cast the higher shift
> >+ * bits to avoid truncation due to size limit.
> >+ */
> >+addr = buf[5] | (buf[4] << 8) | (buf[3] << 16) |
> >+(buf[2] << 24) | ((unsigned long long)buf[1] << 32) |
> >+((unsigned long long)buf[0] << 40);
> >+
> >+sdw_extract_slave_id(bus, addr, );
> >+
> >+/* Now compare with entries */
> >+list_for_each_entry_safe(slave, _s, >slaves, node) {
> >+if (sdw_compare_devid(slave, id) == 0) {
> >+found = true;
> >+
> >+/*
> >+ * Assign a new dev_num to this Slave and
> >+ * not mark it present. It will be marked
> >+ * present after it reports ATTACHED on new
> >+ * dev_num
> >+ */
> >+ret = sdw_assign_device_num(slave);
> >+if (ret) {
> >+dev_err(slave->bus->dev,
> >+"Assign dev_num failed:%d",
> >+ret);
> >+return ret;
> >+}
> >+
> >+break;
> >+}
> >+}
> >+
> >+if (found == false) {
> >+/* TODO: Park this device in Group 13 */
> >+dev_err(bus->dev, "Slave Entry not found");
> >+}
> >+
> >+count++;
> >+
> >+} while (ret == 0 && count < (SDW_MAX_DEVICES * 2));
> 
> explain that the last condition is intentional - this is not a bug -, some
> devices can drop off during enumeration and rejoin so might be counted
> twice.

ok will add

-- 
~Vinod

Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-01 Thread Pierre-Louis Bossart 

On 12/1/17 3:56 AM, Vinod Koul wrote:

From: Sanyog Kale 

SoundWire Slaves report status to bus. Add helpers to handle
the status changes.

Signed-off-by: Hardik T Shah 
Signed-off-by: Sanyog Kale 
Signed-off-by: Vinod Koul 
---
  drivers/soundwire/bus.c   | 202 ++
  drivers/soundwire/bus.h   |  14 +++
  include/linux/soundwire/sdw.h |   1 +
  3 files changed, 217 insertions(+)

diff --git a/drivers/soundwire/bus.c b/drivers/soundwire/bus.c
index 2f108f162905..09126ddd3cdd 100644
--- a/drivers/soundwire/bus.c
+++ b/drivers/soundwire/bus.c
@@ -372,6 +372,93 @@ int sdw_write(struct sdw_slave *slave, u32 addr, u8 value)
  }
  EXPORT_SYMBOL(sdw_write);
  
+/*

+ * SDW alert handling
+ */
+
+/* called with bus_lock held */
+static struct sdw_slave *sdw_get_slave(struct sdw_bus *bus, int i)
+{
+   struct sdw_slave *slave = NULL;
+
+   list_for_each_entry(slave, >slaves, node) {
+   if (slave->dev_num == i)
+   return slave;
+   }
+
+   return NULL;
+}
+
+static int sdw_compare_devid(struct sdw_slave *slave, struct sdw_slave_id id)
+{
+
+   if ((slave->id.unique_id != id.unique_id) ||
+   (slave->id.mfg_id != id.mfg_id) ||
+   (slave->id.part_id != id.part_id) ||
+   (slave->id.class_id != id.class_id))
+   return -ENODEV;
+
+   return 0;
+}
+
+/* called with bus_lock held */
+static int sdw_get_device_num(struct sdw_slave *slave)
+{
+   int bit;
+
+   bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
+   if (bit == SDW_MAX_DEVICES) {
+   bit = -ENODEV;
+   goto err;


My brain is starting to fry but is this correct? Bit11 seems like a 
valid value. Should it be bit > 15 (assuming bit 12,13,14 are set to 
avoid using groups and master)?




+   }
+
+   /*
+* Do not update dev_num in Slave data structure here,
+* Update once program dev_num is successful
+*/
+   set_bit(bit, slave->bus->assigned);
+
+err:
+   return bit;
+}
+
+static int sdw_assign_device_num(struct sdw_slave *slave)
+{
+   int ret, dev_num;
+
+   /* check first if device number is assigned, if so reuse that */
+   if (!slave->dev_num) {
+   mutex_lock(>bus->bus_lock);
+   dev_num = sdw_get_device_num(slave);
+   mutex_unlock(>bus->bus_lock);
+   if (dev_num < 0) {
+   dev_err(slave->bus->dev, "Get dev_num failed: %d",
+   dev_num);
+   return dev_num;
+   }
+   } else {
+   dev_info(slave->bus->dev,
+   "Slave already registered dev_num:%d",
+   slave->dev_num);
+
+   /* Clear the slave->dev_num to transfer message on device 0 */
+   dev_num = slave->dev_num;
+   slave->dev_num = 0;
+
+   }
+
+   ret = sdw_write(slave, SDW_SCP_DEVNUMBER, dev_num);
+   if (ret < 0) {
+   dev_err(>dev, "Program device_num failed: %d", ret);
+   return ret;
+   }
+
+   /* After xfer of msg, restore dev_num */
+   slave->dev_num = dev_num;
+
+   return 0;
+}
+
  void sdw_extract_slave_id(struct sdw_bus *bus,
u64 addr, struct sdw_slave_id *id)
  {
@@ -400,3 +487,118 @@ void sdw_extract_slave_id(struct sdw_bus *bus,
id->unique_id, id->sdw_version);
  
  }

+
+static int sdw_program_device_num(struct sdw_bus *bus)
+{
+   u8 buf[SDW_NUM_DEV_ID_REGISTERS] = {0};
+   struct sdw_slave *slave, *_s;
+   struct sdw_slave_id id;
+   struct sdw_msg msg;
+   bool found = false;
+   int count = 0, ret;
+   u64 addr;
+
+   /* No Slave, so use raw xfer api */
+   ret = sdw_fill_msg(, NULL, SDW_SCP_DEVID_0,
+   SDW_NUM_DEV_ID_REGISTERS, 0, SDW_MSG_FLAG_READ, buf);
+   if (ret < 0)
+   return ret;
+
+   do {
+   ret = sdw_transfer(bus, NULL, );
+   if (ret == -ENODATA) { /* end of device id reads */
+   ret = 0;
+   break;
+   }
+   if (ret < 0) {
+   dev_err(bus->dev, "DEVID read fail:%d\n", ret);
+   break;
+   }
+
+   /*
+* Construct the addr and extract. Cast the higher shift
+* bits to avoid truncation due to size limit.
+*/
+   addr = buf[5] | (buf[4] << 8) | (buf[3] << 16) |
+   (buf[2] << 24) | ((unsigned long long)buf[1] << 32) |
+   ((unsigned long long)buf[0] << 40);
+
+   sdw_extract_slave_id(bus, addr, );
+
+   /* Now

Re: [alsa-devel] [PATCH v4 08/15] soundwire: Add Slave status handling helpers

2017-12-01 Thread Pierre-Louis Bossart 

On 12/1/17 3:56 AM, Vinod Koul wrote:

From: Sanyog Kale 

SoundWire Slaves report status to bus. Add helpers to handle
the status changes.

Signed-off-by: Hardik T Shah 
Signed-off-by: Sanyog Kale 
Signed-off-by: Vinod Koul 
---
  drivers/soundwire/bus.c   | 202 ++
  drivers/soundwire/bus.h   |  14 +++
  include/linux/soundwire/sdw.h |   1 +
  3 files changed, 217 insertions(+)

diff --git a/drivers/soundwire/bus.c b/drivers/soundwire/bus.c
index 2f108f162905..09126ddd3cdd 100644
--- a/drivers/soundwire/bus.c
+++ b/drivers/soundwire/bus.c
@@ -372,6 +372,93 @@ int sdw_write(struct sdw_slave *slave, u32 addr, u8 value)
  }
  EXPORT_SYMBOL(sdw_write);
  
+/*

+ * SDW alert handling
+ */
+
+/* called with bus_lock held */
+static struct sdw_slave *sdw_get_slave(struct sdw_bus *bus, int i)
+{
+   struct sdw_slave *slave = NULL;
+
+   list_for_each_entry(slave, >slaves, node) {
+   if (slave->dev_num == i)
+   return slave;
+   }
+
+   return NULL;
+}
+
+static int sdw_compare_devid(struct sdw_slave *slave, struct sdw_slave_id id)
+{
+
+   if ((slave->id.unique_id != id.unique_id) ||
+   (slave->id.mfg_id != id.mfg_id) ||
+   (slave->id.part_id != id.part_id) ||
+   (slave->id.class_id != id.class_id))
+   return -ENODEV;
+
+   return 0;
+}
+
+/* called with bus_lock held */
+static int sdw_get_device_num(struct sdw_slave *slave)
+{
+   int bit;
+
+   bit = find_first_zero_bit(slave->bus->assigned, SDW_MAX_DEVICES);
+   if (bit == SDW_MAX_DEVICES) {
+   bit = -ENODEV;
+   goto err;


My brain is starting to fry but is this correct? Bit11 seems like a 
valid value. Should it be bit > 15 (assuming bit 12,13,14 are set to 
avoid using groups and master)?




+   }
+
+   /*
+* Do not update dev_num in Slave data structure here,
+* Update once program dev_num is successful
+*/
+   set_bit(bit, slave->bus->assigned);
+
+err:
+   return bit;
+}
+
+static int sdw_assign_device_num(struct sdw_slave *slave)
+{
+   int ret, dev_num;
+
+   /* check first if device number is assigned, if so reuse that */
+   if (!slave->dev_num) {
+   mutex_lock(>bus->bus_lock);
+   dev_num = sdw_get_device_num(slave);
+   mutex_unlock(>bus->bus_lock);
+   if (dev_num < 0) {
+   dev_err(slave->bus->dev, "Get dev_num failed: %d",
+   dev_num);
+   return dev_num;
+   }
+   } else {
+   dev_info(slave->bus->dev,
+   "Slave already registered dev_num:%d",
+   slave->dev_num);
+
+   /* Clear the slave->dev_num to transfer message on device 0 */
+   dev_num = slave->dev_num;
+   slave->dev_num = 0;
+
+   }
+
+   ret = sdw_write(slave, SDW_SCP_DEVNUMBER, dev_num);
+   if (ret < 0) {
+   dev_err(>dev, "Program device_num failed: %d", ret);
+   return ret;
+   }
+
+   /* After xfer of msg, restore dev_num */
+   slave->dev_num = dev_num;
+
+   return 0;
+}
+
  void sdw_extract_slave_id(struct sdw_bus *bus,
u64 addr, struct sdw_slave_id *id)
  {
@@ -400,3 +487,118 @@ void sdw_extract_slave_id(struct sdw_bus *bus,
id->unique_id, id->sdw_version);
  
  }

+
+static int sdw_program_device_num(struct sdw_bus *bus)
+{
+   u8 buf[SDW_NUM_DEV_ID_REGISTERS] = {0};
+   struct sdw_slave *slave, *_s;
+   struct sdw_slave_id id;
+   struct sdw_msg msg;
+   bool found = false;
+   int count = 0, ret;
+   u64 addr;
+
+   /* No Slave, so use raw xfer api */
+   ret = sdw_fill_msg(, NULL, SDW_SCP_DEVID_0,
+   SDW_NUM_DEV_ID_REGISTERS, 0, SDW_MSG_FLAG_READ, buf);
+   if (ret < 0)
+   return ret;
+
+   do {
+   ret = sdw_transfer(bus, NULL, );
+   if (ret == -ENODATA) { /* end of device id reads */
+   ret = 0;
+   break;
+   }
+   if (ret < 0) {
+   dev_err(bus->dev, "DEVID read fail:%d\n", ret);
+   break;
+   }
+
+   /*
+* Construct the addr and extract. Cast the higher shift
+* bits to avoid truncation due to size limit.
+*/
+   addr = buf[5] | (buf[4] << 8) | (buf[3] << 16) |
+   (buf[2] << 24) | ((unsigned long long)buf[1] << 32) |
+   ((unsigned long long)buf[0] << 40);
+
+   sdw_extract_slave_id(bus, addr, );
+
+   /* Now compare with entries */
+   list_for_each_entry_safe(slave, _s, >slaves, node) {
+