[PATCH platform-next 09/13] platform/mellanox: mlxreg-hotplug: Allow more flexible hotplug events configuration

[Vadim Pasternak <vadimp@xxxxxxxxxx>]

Currently hotplug configuration in logic device assumes that all items
are provided with no holes.
Thus, any group of hotplug events, associated with the specific
status/event/mask registers is configured in those registers
successively from bit zero to bit #n (#n < 8).

This logic is changed int order to allow non-successive definition to
support configuration with the skipped bits – for example bits 3, 5, 7
in status/event/mask registers can be associated with hotplug events,
while others can be skipped.

Signed-off-by: Vadim Pasternak <vadimp@xxxxxxxxxx>
Reviewed-by: Michael Shych <michaelsh@xxxxxxxxxx>
---
 drivers/platform/mellanox/mlxreg-hotplug.c | 28 ++++++++++++++++++----
 1 file changed, 23 insertions(+), 5 deletions(-)

diff --git a/drivers/platform/mellanox/mlxreg-hotplug.c b/drivers/platform/mellanox/mlxreg-hotplug.c
index 117bc3f395fd..b7dcc64cd238 100644
--- a/drivers/platform/mellanox/mlxreg-hotplug.c
+++ b/drivers/platform/mellanox/mlxreg-hotplug.c
@@ -239,6 +239,17 @@ static ssize_t mlxreg_hotplug_attr_show(struct device *dev,
 #define PRIV_ATTR(i) priv->mlxreg_hotplug_attr[i]
 #define PRIV_DEV_ATTR(i) priv->mlxreg_hotplug_dev_attr[i]
 
+static int mlxreg_hotplug_item_label_index_get(u32 mask, u32 bit)
+{
+	int i, j;
+
+	for (i = 0, j = -1; i <= bit; i++) {
+		if (mask & BIT(i))
+			j++;
+	}
+	return j;
+}
+
 static int mlxreg_hotplug_attr_init(struct mlxreg_hotplug_priv_data *priv)
 {
 	struct mlxreg_core_hotplug_platform_data *pdata;
@@ -246,7 +257,7 @@ static int mlxreg_hotplug_attr_init(struct mlxreg_hotplug_priv_data *priv)
 	struct mlxreg_core_data *data;
 	unsigned long mask;
 	u32 regval;
-	int num_attrs = 0, id = 0, i, j, k, ret;
+	int num_attrs = 0, id = 0, i, j, k, count, ret;
 
 	pdata = dev_get_platdata(&priv->pdev->dev);
 	item = pdata->items;
@@ -272,7 +283,8 @@ static int mlxreg_hotplug_attr_init(struct mlxreg_hotplug_priv_data *priv)
 		/* Go over all unmasked units within item. */
 		mask = item->mask;
 		k = 0;
-		for_each_set_bit(j, &mask, item->count) {
+		count = item->ind ? item->ind : item->count;
+		for_each_set_bit(j, &mask, count) {
 			if (data->capability) {
 				/*
 				 * Read capability register and skip non
@@ -282,16 +294,17 @@ static int mlxreg_hotplug_attr_init(struct mlxreg_hotplug_priv_data *priv)
 						  data->capability, &regval);
 				if (ret)
 					return ret;
+
 				if (!(regval & data->bit)) {
 					data++;
 					continue;
 				}
 			}
+
 			PRIV_ATTR(id) = &PRIV_DEV_ATTR(id).dev_attr.attr;
 			PRIV_ATTR(id)->name = devm_kasprintf(&priv->pdev->dev,
 							     GFP_KERNEL,
 							     data->label);
-
 			if (!PRIV_ATTR(id)->name) {
 				dev_err(priv->dev, "Memory allocation failed for attr %d.\n",
 					id);
@@ -365,9 +378,14 @@ mlxreg_hotplug_work_helper(struct mlxreg_hotplug_priv_data *priv,
 	regval &= item->mask;
 	asserted = item->cache ^ regval;
 	item->cache = regval;
-
 	for_each_set_bit(bit, &asserted, 8) {
-		data = item->data + bit;
+		int pos;
+
+		pos = mlxreg_hotplug_item_label_index_get(item->mask, bit);
+		if (pos < 0)
+			goto out;
+
+		data = item->data + pos;
 		if (regval & BIT(bit)) {
 			if (item->inversed)
 				mlxreg_hotplug_device_destroy(priv, data, item->kind);
-- 
2.20.1