[PATCH 3/3] hwmon: (pmbus/core): Implement IRQ support

[Naresh Solanki]

From: Patrick Rudolph <patrick.rudolph@xxxxxxxxxxxxx>

Implement IRQ support to monitor PMBUS regulator events.

Signed-off-by: Patrick Rudolph <patrick.rudolph@xxxxxxxxxxxxx>
Signed-off-by: Naresh Solanki <Naresh.Solanki@xxxxxxxxxxxxx>
---
 drivers/hwmon/pmbus/pmbus.h      |   2 +-
 drivers/hwmon/pmbus/pmbus_core.c | 155 ++++++++++++++++++++++++++++---
 2 files changed, 144 insertions(+), 13 deletions(-)

diff --git a/drivers/hwmon/pmbus/pmbus.h b/drivers/hwmon/pmbus/pmbus.h
index 10fb17879f8e..6b2e6cf93b19 100644
--- a/drivers/hwmon/pmbus/pmbus.h
+++ b/drivers/hwmon/pmbus/pmbus.h
@@ -26,7 +26,7 @@ enum pmbus_regs {
 
 	PMBUS_CAPABILITY		= 0x19,
 	PMBUS_QUERY			= 0x1A,
-
+	PMBUS_SMBALERT_MASK		= 0x1B,
 	PMBUS_VOUT_MODE			= 0x20,
 	PMBUS_VOUT_COMMAND		= 0x21,
 	PMBUS_VOUT_TRIM			= 0x22,
diff --git a/drivers/hwmon/pmbus/pmbus_core.c b/drivers/hwmon/pmbus/pmbus_core.c
index 7d7d10039987..4234297813bb 100644
--- a/drivers/hwmon/pmbus/pmbus_core.c
+++ b/drivers/hwmon/pmbus/pmbus_core.c
@@ -3033,13 +3033,110 @@ const struct regulator_ops pmbus_regulator_ops = {
 };
 EXPORT_SYMBOL_NS_GPL(pmbus_regulator_ops, PMBUS);
 
-static int pmbus_regulator_register(struct pmbus_data *data)
+static int pmbus_write_smbalert_mask(struct i2c_client *client, u8 page, u8 reg, u8 val)
 {
-	struct device *dev = data->dev;
+	return pmbus_write_word_data(client, page, PMBUS_SMBALERT_MASK, reg | (val << 8));
+}
+
+static int pmbus_irq_subhandler(struct i2c_client *client, struct regulator_err_state *stat,
+			       unsigned long *dev_mask)
+{
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	u8 page = rdev_get_id(stat->rdev);
+	const struct pmbus_regulator_status_category *cat;
+	const struct pmbus_regulator_status_assoc *bit;
+	int status, i;
+	int func = data->info->func[page];
+
+
+	stat->notifs = 0;
+	stat->errors = 0;
+
+	for (i = 0; i < ARRAY_SIZE(pmbus_regulator_flag_map); i++) {
+		cat = &pmbus_regulator_flag_map[i];
+		if (!(func & cat->func))
+			continue;
+
+		status = _pmbus_read_byte_data(client, page, cat->reg);
+		if (status < 0) {
+			mutex_unlock(&data->update_lock);
+			return status;
+		}
+
+		for (bit = cat->bits; bit->pflag; bit++) {
+			if (status & bit->pflag) {
+				stat->notifs |= bit->eflags;
+				stat->errors |= bit->rflag;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int pmbus_fault_handler(int irq, struct regulator_irq_data *rid, unsigned long *dev_mask)
+{
+	struct regulator_err_state *stat;
+	struct pmbus_data *data;
+	struct device *dev;
+	struct i2c_client *client;
+	int status;
+	u8 page;
+	int i;
+	int ret = REGULATOR_ERROR_CLEARED;
+
+	*dev_mask = 0;
+	rid->opaque = 0;
+
+	for (i = 0; i < rid->num_states; i++) {
+		stat  = &rid->states[i];
+		dev = rdev_get_dev(stat->rdev);
+		client = to_i2c_client(dev->parent);
+		data = i2c_get_clientdata(client);
+		page = rdev_get_id(stat->rdev);
+
+		mutex_lock(&data->update_lock);
+		status = pmbus_irq_subhandler(client, stat, dev_mask);
+		if (status < 0) {
+			ret = REGULATOR_FAILED_RETRY;
+			goto unlock;
+		}
+
+		status = pmbus_read_status_byte(client, page);
+		if (status < 0) {
+			ret = REGULATOR_FAILED_RETRY;
+			goto unlock;
+		}
+
+		if (status & ~(PB_STATUS_OFF | PB_STATUS_BUSY))
+			pmbus_clear_fault_page(client, page);
+
+	}
+unlock:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static int pmbus_regulator_register(struct i2c_client *client, struct pmbus_data *data)
+{
+	struct device *dev = &client->dev;
 	const struct pmbus_driver_info *info = data->info;
 	const struct pmbus_platform_data *pdata = dev_get_platdata(dev);
-	struct regulator_dev *rdev;
-	int i;
+	const struct pmbus_regulator_status_category *cat;
+	const struct pmbus_regulator_status_assoc *bit;
+	int func;
+	struct regulator_dev **rdevs;
+	struct regulator_irq_desc pmbus_notif = {
+		.name = "pmbus-irq",
+		.map_event = pmbus_fault_handler,
+	};
+	void *irq_helper;
+	int i, j, err, errs;
+	u8 mask;
+
+	rdevs = devm_kzalloc(dev, sizeof(*rdevs) * info->num_regulators, GFP_KERNEL);
+	if (!rdevs)
+		return -ENOMEM;
 
 	for (i = 0; i < info->num_regulators; i++) {
 		struct regulator_config config = { };
@@ -3050,18 +3147,52 @@ static int pmbus_regulator_register(struct pmbus_data *data)
 		if (pdata && pdata->reg_init_data)
 			config.init_data = &pdata->reg_init_data[i];
 
-		rdev = devm_regulator_register(dev, &info->reg_desc[i],
-					       &config);
-		if (IS_ERR(rdev))
-			return dev_err_probe(dev, PTR_ERR(rdev),
-					     "Failed to register %s regulator\n",
-					     info->reg_desc[i].name);
+		rdevs[i] = devm_regulator_register(dev, &info->reg_desc[i], &config);
+		if (IS_ERR(rdevs[i])) {
+			dev_err(dev, "Failed to register %s regulator\n",
+				info->reg_desc[i].name);
+			return PTR_ERR(rdevs[i]);
+		}
 	}
 
+	if (client->irq > 0) {
+		pmbus_notif.data = rdevs;
+		errs = 0;
+		for (i = 0; i < data->info->pages; i++) {
+			func = data->info->func[i];
+
+			for (j = 0; j < ARRAY_SIZE(pmbus_regulator_flag_map); j++) {
+				cat = &pmbus_regulator_flag_map[i];
+				if (!(func & cat->func))
+					continue;
+				mask = 0;
+				for (bit = cat->bits; bit->pflag; bit++) {
+					errs |= bit->rflag;
+					mask |= bit->pflag;
+				}
+				err = pmbus_write_smbalert_mask(client, i, cat->reg, ~mask);
+				if (err)
+					dev_err(dev, "Failed to set smbalert for reg 0x%02x\n",
+						cat->reg);
+			}
+
+		}
+
+		/* Register notifiers - can fail if IRQ is not given */
+		irq_helper = devm_regulator_irq_helper(dev, &pmbus_notif, client->irq, 0, errs,
+						      NULL, &rdevs[0], info->num_regulators);
+		if (IS_ERR(irq_helper)) {
+			if (PTR_ERR(irq_helper) == -EPROBE_DEFER) {
+				return -EPROBE_DEFER;
+			}
+			dev_warn(dev, "IRQ disabled %pe\n", irq_helper);
+		}
+	}
+	
 	return 0;
 }
 #else
-static int pmbus_regulator_register(struct pmbus_data *data)
+static int pmbus_regulator_register(struct i2c_client *client, struct pmbus_data *data)
 {
 	return 0;
 }
@@ -3425,7 +3556,7 @@ int pmbus_do_probe(struct i2c_client *client, struct pmbus_driver_info *info)
 		return PTR_ERR(data->hwmon_dev);
 	}
 
-	ret = pmbus_regulator_register(data);
+	ret = pmbus_regulator_register(client, data);
 	if (ret)
 		return ret;
 
-- 
2.37.3