[PATCH 3/3] thermal/drivers/tsens: Extract and shift-in optional MSB

[Bryan O'Donoghue]

In msm8939 some of the sensor calibration data traverses byte boundaries.
Two examples of this are thermal sensor 2 point 1 and sensor 9 point 2.

For sensor 2 point 1 we can get away with a simple read traversing byte
boundaries as the calibration most significant bits are adjacent to the
least significant across the byte boundary.

In this case a read starting at the end of the first byte for nine bits
will deliver up the data we want.

In the case of sensor 9 point 2 however, the most significant bits are not
adjacent and so therefore we need to perform two reads and or the bits
together.

If reg.p1_shift or reg.p2_shift is set then automatically search for
pX_sY_msb in the dts applying pX_shift as a right shift or into the pX_sY
value.

Signed-off-by: Bryan O'Donoghue <bryan.odonoghue@xxxxxxxxxx>
---
 drivers/thermal/qcom/tsens.c | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/drivers/thermal/qcom/tsens.c b/drivers/thermal/qcom/tsens.c
index a260f563b4889..eff2c8671c343 100644
--- a/drivers/thermal/qcom/tsens.c
+++ b/drivers/thermal/qcom/tsens.c
@@ -74,6 +74,7 @@ int tsens_read_calibration(struct tsens_priv *priv, int shift, u32 *p1, u32 *p2,
 {
 	u32 mode;
 	u32 base1, base2;
+	u32 msb;
 	char name[] = "sXX_pY_backup"; /* s10_p1_backup */
 	int i, ret;
 
@@ -122,6 +123,22 @@ int tsens_read_calibration(struct tsens_priv *priv, int shift, u32 *p1, u32 *p2,
 
 		dev_dbg(priv->dev, "%s 0x%x\n", name, p1[i]);
 
+		if (priv->reg && priv->reg[i].p1_shift) {
+			ret = snprintf(name, sizeof(name), "s%d_p1_msb",
+				       priv->sensor[i].hw_id);
+			if (ret < 0)
+				return ret;
+
+			ret = nvmem_cell_read_variable_le_u32(priv->dev, name, &msb);
+			if (ret) {
+				dev_err(priv->dev, "Failed to read %s\n", name);
+				return ret;
+			}
+
+			dev_dbg(priv->dev, "%s 0x%x\n", name, msb);
+			p1[i] |= msb >> priv->reg[i].p1_shift;
+		}
+
 		ret = snprintf(name, sizeof(name), "s%d_p2%s", priv->sensor[i].hw_id,
 			       backup ? "_backup" : "");
 		if (ret < 0)
@@ -134,6 +151,22 @@ int tsens_read_calibration(struct tsens_priv *priv, int shift, u32 *p1, u32 *p2,
 		}
 
 		dev_dbg(priv->dev, "%s 0x%x\n", name, p2[i]);
+
+		if (priv->reg && priv->reg[i].p2_shift) {
+			ret = snprintf(name, sizeof(name), "s%d_p2_msb",
+				       priv->sensor[i].hw_id);
+			if (ret < 0)
+				return ret;
+
+			ret = nvmem_cell_read_variable_le_u32(priv->dev, name, &msb);
+			if (ret) {
+				dev_err(priv->dev, "Failed to read %s\n", name);
+				return ret;
+			}
+
+			dev_dbg(priv->dev, "%s 0x%x\n", name, msb);
+			p2[i] |= msb >> priv->reg[i].p2_shift;
+		}
 	}
 
 	switch (mode) {
-- 
2.39.2