[PATCH] [WAR] hwmon: (ina3221) Apply software WAR to offset shunt voltage

[Ninad Malwade]

This is used as a software WAR to offset shunt voltage reading
from INA3221 to increase its accuracy. This patch implements a
previous downstream feature by reading the offset information
from DT and apply it to current readings.

Signed-off-by: Ninad Malwade <nmalwade@xxxxxxxxxx>
---
 drivers/hwmon/ina3221.c | 141 ++++++++++++++++++++++++++++++++++++++--
 1 file changed, 137 insertions(+), 4 deletions(-)

diff --git a/drivers/hwmon/ina3221.c b/drivers/hwmon/ina3221.c
index e06186986444..726c8b99b8cd 100644
--- a/drivers/hwmon/ina3221.c
+++ b/drivers/hwmon/ina3221.c
@@ -94,13 +94,39 @@ enum ina3221_channels {
 	INA3221_NUM_CHANNELS
 };
 
+/**
+ * struct shuntv_offset_range - [WAR] shunt voltage offset sub-range
+ * @start: range start (uV)
+ * @end: range end (uV)
+ * @offset: offset for the current sub-range
+ */
+struct shuntv_offset_range {
+	s32 start;
+	s32 end;
+	s32 offset;
+};
+
+/**
+ * struct shuntv_offset - [WAR] shunt voltage offset information
+ * @offset: general offset
+ * @range: pointer to a sub-range of shunt voltage offset (uV)
+ * @num_range: number of sub-ranges of shunt voltage offset
+ */
+struct shuntv_offset {
+	s32 offset;
+	struct shuntv_offset_range *range;
+	s32 num_range;
+};
+
 /**
  * struct ina3221_input - channel input source specific information
+ * @shuntv_offset: [WAR] shunt voltage offset information
  * @label: label of channel input source
  * @shunt_resistor: shunt resistor value of channel input source
  * @disconnected: connection status of channel input source
  */
 struct ina3221_input {
+	struct shuntv_offset *shuntv_offset;
 	const char *label;
 	int shunt_resistor;
 	bool disconnected;
@@ -329,7 +355,7 @@ static int ina3221_read_curr(struct device *dev, u32 attr,
 	struct ina3221_data *ina = dev_get_drvdata(dev);
 	struct ina3221_input *input = ina->inputs;
 	u8 reg = ina3221_curr_reg[attr][channel];
-	int resistance_uo, voltage_nv;
+	int resistance_uo, voltage_uv;
 	int regval, ret;
 
 	if (channel > INA3221_CHANNEL3)
@@ -362,10 +388,34 @@ static int ina3221_read_curr(struct device *dev, u32 attr,
 		if (ret)
 			return ret;
 
-		/* Scale of shunt voltage: LSB is 40uV (40000nV) */
-		voltage_nv = regval * 40000;
+		/* Scale of shunt voltage: LSB is 40uV */
+		voltage_uv = regval * 40;
+
+		/* Apply software WAR to offset shunt voltage for accuracy */
+		if (input->shuntv_offset) {
+			struct shuntv_offset_range *range =
+						input->shuntv_offset->range;
+			int num_range = input->shuntv_offset->num_range;
+			int offset = input->shuntv_offset->offset;
+
+			while (num_range--) {
+				if (voltage_uv >= range->start &&
+				    voltage_uv <= range->end) {
+					/* Use range offset instead */
+					offset = range->offset;
+					break;
+				}
+				range++;
+			}
+
+			if (voltage_uv < 0)
+				voltage_uv += offset;
+			else
+				voltage_uv -= offset;
+		}
+
 		/* Return current in mA */
-		*val = DIV_ROUND_CLOSEST(voltage_nv, resistance_uo);
+		*val = DIV_ROUND_CLOSEST(voltage_uv * 1000, resistance_uo);
 		return 0;
 	case hwmon_curr_crit_alarm:
 	case hwmon_curr_max_alarm:
@@ -758,6 +808,84 @@ static const struct regmap_config ina3221_regmap_config = {
 	.volatile_table = &ina3221_volatile_table,
 };
 
+static struct shuntv_offset *
+ina3221_probe_shuntv_offset_from_dt(struct device *dev,
+				    struct device_node *child)
+{
+	struct device_node *np, *range_np;
+	struct shuntv_offset *shuntv_offset;
+	struct shuntv_offset_range *range;
+	s32 start, end, offset;
+	const __be32 *prop;
+	int ret, num_range;
+
+	prop = of_get_property(child, "shunt-volt-offset-uv", NULL);
+	/* Silently return for devices with no need of an offset WAR */
+	if (!prop)
+		return NULL;
+
+	np = of_find_node_by_phandle(be32_to_cpup(prop));
+	if (!np) {
+		dev_err(dev, "corrupted phandle for shunt-volt-offset-uv\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	ret = of_property_read_s32(np, "offset", &offset);
+	if (ret) {
+		dev_err(dev, "failed to read general shuntv offset\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	shuntv_offset = devm_kzalloc(dev, sizeof(*shuntv_offset), GFP_KERNEL);
+	if (!shuntv_offset)
+		return ERR_PTR(-ENOMEM);
+
+	shuntv_offset->offset = offset;
+
+	num_range = of_get_child_count(np);
+
+	/* Return upon no sub-range found */
+	if (!num_range)
+		return shuntv_offset;
+
+	range = devm_kzalloc(dev, sizeof(*range) * num_range, GFP_KERNEL);
+	if (!range)
+		return ERR_PTR(-ENOMEM);
+
+	shuntv_offset->range = range;
+	shuntv_offset->num_range = num_range;
+
+	for_each_child_of_node(np, range_np) {
+		ret = of_property_read_s32(range_np, "start", &start);
+		if (ret) {
+			dev_warn(dev, "missing start in range node\n");
+			range++;
+			continue;
+		}
+
+		ret = of_property_read_s32(range_np, "end", &end);
+		if (ret) {
+			dev_warn(dev, "missing end in range node\n");
+			range++;
+			continue;
+		}
+
+		ret = of_property_read_s32(range_np, "offset", &offset);
+		if (ret) {
+			dev_warn(dev, "missing offset in range node\n");
+			range++;
+			continue;
+		}
+
+		range->start = start;
+		range->end = end;
+		range->offset = offset;
+		range++;
+	}
+
+	return shuntv_offset;
+}
+
 static int ina3221_probe_child_from_dt(struct device *dev,
 				       struct device_node *child,
 				       struct ina3221_data *ina)
@@ -796,6 +924,11 @@ static int ina3221_probe_child_from_dt(struct device *dev,
 		input->shunt_resistor = val;
 	}
 
+	/* Apply software WAR to offset shunt voltage for accuracy */
+	input->shuntv_offset = ina3221_probe_shuntv_offset_from_dt(dev, child);
+	if (IS_ERR(input->shuntv_offset))
+		return PTR_ERR(input->shuntv_offset);
+
 	return 0;
 }
 
-- 
2.17.1