Refactor TMP006 module

1) Use floating-point more freely, since it's on all the time now, and
the old fixed-point code no longer compiled.

2) Sensitivity and Bn values are now in a RAM-based struct in
preparation for setting them at runtime.  No changes from current
values.

3) If a sensor fails to read good data, is initialized, or loses
power, its die temperature history will be set to the next good
temperature, rather than persisting an arbitrary start value or old
state.  This fixes reading wildly inaccurate object temperatures for
the first few seconds following boot/resume.

4) If a sensor loses power, wait for the sensor to report data-ready
before reading temperature/voltage.  Otherwise, those read as 0, which
again throws off the first few seconds of data.

BUG=chrome-os-partner:14955
BRANCH=link

TEST=Boot system and set at login screen for a minute to reach thermal
equilibrium.  Then reboot system, type 'temps' repeatedly.  Data from
TMP006's should initially be Error; after a second or so it should be
good, and shouldn't change more than a few degrees.

Original-Change-Id: Id0b42b9b18e94978ba7d3a1ee33194e44b1904bc
Signed-off-by: Randall Spangler <rspangler@chromium.org>
Reviewed-on: https://gerrit.chromium.org/gerrit/35188
(cherry picked from commit 0b6b6b7754359546d4fd6684156bc7b120328721)

Change-Id: I3b6e5d21a5452c5bebcdd1c06217759fd753ca10
Reviewed-on: https://gerrit.chromium.org/gerrit/35666
Reviewed-by: Rong Chang <rongchang@chromium.org>
Tested-by: Rong Chang <rongchang@chromium.org>

3 files changed, 187 insertions, 171 deletions

diff --git a/board/link/board_temp_sensor.c b/board/link/board_temp_sensor.c
index 14f6f16043..c5b317b204 100644
--- a/board/link/board_temp_sensor.c
+++ b/board/link/board_temp_sensor.c
@@ -55,8 +55,8 @@ const struct temp_sensor_t temp_sensors[TEMP_SENSOR_COUNT] = {
 
 const struct tmp006_t tmp006_sensors[TMP006_COUNT] = {
 	/* TODO: Calibrate sensitivity factors. See crosbug.com/p/9599 */
-	{"USB C", TEMP_USB_ADDR, 3648},
-	{"PCH D", TEMP_PCH_ADDR, 9301},
-	{"Hinge C", TEMP_HINGE_ADDR, -11000},
-	{"Charger D", TEMP_CHARGER_ADDR, 10426},
+	{"USB C", TEMP_USB_ADDR, 3.648 * 1e-14f},
+	{"PCH D", TEMP_PCH_ADDR, 9.301 * 1e-14f},
+	{"Hinge C", TEMP_HINGE_ADDR, -11.000 * 1e-14f},
+	{"Charger D", TEMP_CHARGER_ADDR, 10.426 * 1e-14f},
 };
diff --git a/common/tmp006.c b/common/tmp006.c
index b69c348ccb..3aa305b475 100644
--- a/common/tmp006.c
+++ b/common/tmp006.c
@@ -17,188 +17,250 @@
 #include "tmp006.h"
 #include "util.h"
 
+/* Console output macros */
+#define CPUTS(outstr) cputs(CC_THERMAL, outstr)
+#define CPRINTF(format, args...) cprintf(CC_THERMAL, format, ## args)
+
+/* Constants for calculating target object temperatures */
+const float A1 = 1.75e-3f;
+const float A2 = -1.678e-5f;
+const float B0 = -2.94e-5f;
+const float B1 = -5.7e-7f;
+const float B2 = 4.63e-9f;
+const float C2 = 13.4f;
+
 /* Defined in board_temp_sensor.c. */
 extern const struct tmp006_t tmp006_sensors[TMP006_COUNT];
 
+/* Flags for tdata->fail */
+#define FAIL_INIT        (1 << 0)  /* Just initialized */
+#define FAIL_POWER       (1 << 1)  /* Sensor not powered */
+#define FAIL_I2C         (1 << 2)  /* I2C communication error */
+#define FAIL_NOT_READY   (1 << 3)  /* Data not ready */
+
 struct tmp006_data_t {
-	/* Object voltage */
-	int v;
-	/* The last four die temperature value. Used as a circular buffer. */
-	int t[4];
-	/* The index of the current value in the dir temperature array. */
-	int tidx;
-	/* Fail bit: 1 if last read fail. 0 if ok. */
-	int fail;
+	int v;     /* Object voltage */
+	int t[4];  /* Circular buffer of last four die temperatures */
+	int tidx;  /* Index of the current value in t[] */
+	int fail;  /* Fail flags; non-zero if last read failed */
+	float S0;  /* Sensitivity factor */
+	float B0, B1, B2;  /* Coefficients for self-heating correction */
 };
 
 static struct tmp006_data_t tmp006_data[TMP006_COUNT];
 
-static int tmp006_read_die_temp(int idx)
+/**
+ * Check if sensor has power
+ *
+ * @param idx		Sensor index
+ *
+ * @return non-zero if sensor has power.
+ */
+static int tmp006_has_power(int idx)
 {
-	int pidx = (tmp006_data[idx].tidx - 1) & 0x3;
-	if (tmp006_data[idx].fail == 1)
+	/* All TMP006 sensors are powered by VS. */
+	return gpio_get_level(GPIO_PGOOD_1_8VS);
+}
+
+static int tmp006_read_die_temp(const struct tmp006_data_t *tdata)
+{
+	if (tdata->fail)
 		return -1;
-	return tmp006_data[idx].t[pidx] / 100;
+
+	/* Return previous die temperature */
+	return tdata->t[(tdata->tidx - 1) & 0x3] / 100;
 }
 
-/* Calculate the remote object temperature.
- * Parameters:
- *     Tdie: Die temperature in 1/100 K.
- *     Vobj: Voltage read from register 0. In nV.
- *     S0:   Sensitivity factor in 1e-17.
- * Return:
- *     Object temperature in 1/100 K.
+/**
+ * Calculate the remote object temperature.
+ *
+ * @param Tdie_i	Die temperature in 1/100 K.
+ * @param Vobj_i	Voltage read from register 0. In nV.
+ * @param tdata		TMP006 data for this sensor.
+ *
+ * @return Object temperature in 1/100 K.
  */
-static int tmp006_calculate_object_temp(int Tdie_i, int Vobj_i, int S0_i)
+static int tmp006_calculate_object_temp(int Tdie_i, int Vobj_i,
+					const struct tmp006_data_t *tdata)
 {
-#ifdef CONFIG_FPU
-	float Tdie, Vobj, S0;
+	float Tdie, Vobj;
 	float Tx, S, Vos, Vx, fv, Tobj, T4;
 	int Tobj_i;
 
 	Tdie = (float)Tdie_i * 1e-2f;
 	Vobj = (float)Vobj_i * 1e-9f;
-	S0 = (float)S0_i * 1e-17f;
 
 	/* Calculate according to TMP006 users guide. */
 	Tx = Tdie - 298.15f;
 	/* S is the sensitivity */
-	S = S0 * (1.0f + 1.75e-3f * Tx - 1.678e-5f * Tx * Tx);
+	S = tdata->S0 * (1.0f + A1 * Tx + A2 * Tx * Tx);
 	/* Vos is the offset voltage */
-	Vos = -2.94e-5f - 5.7e-7f * Tx + 4.63e-9f * Tx * Tx;
+	Vos = tdata->B0 + tdata->B1 * Tx + tdata->B2 * Tx * Tx;
 	Vx = Vobj - Vos;
 	/* fv is Seebeck coefficient f(Vobj) */
-	fv = Vx + 13.4f * Vx * Vx;
+	fv = Vx + C2 * Vx * Vx;
 
 	T4 = Tdie * Tdie * Tdie * Tdie + fv / S;
 	Tobj = sqrtf(sqrtf(T4));
 	Tobj_i = (int32_t)(Tobj * 100.0f);
 
 	return Tobj_i;
-#else
-	/* This is the fixed-point version of object temperature calculation.
-	 * Should be accurate but it is hard to prevent and debug
-	 * overflow/underflow problem. Only use this version if there is no
-	 * FPU support.
-	 * Division is delayed when possible to preserve precision, but should
-	 * not cause overflow.
-	 * Assuming Tdie is between 200K and 400K, and S0 between 3e-14 and
-	 * 9e-14, the maximum value during the calculation should be less than
-	 * (1 << 30), which fits in int32_t.
-	 */
-	int32_t Tx, S19, Vos, Vx, fv9, ub, lb;
-
-	Tx = Tdie - 29815;
-	/* S19 is the sensitivity multipled by 1e19 */
-	S19 = S0 * (100000 + 175 * Tx / 100 -
-			1678 * Tx / 100 * Tx / 100000) / 1000;
-	/* Vos is the offset voltage in nV */
-	Vos = -29400 - 570 * Tx / 100 + 463 * Tx / 100 * Tx / 10000;
-	Vx = Vobj - Vos;
-	/* fv9 is Seebeck coefficient f(Vobj) multipled by 1e9 */
-	fv9 = Vx + 134 * Vx / 100000 * Vx / 100000;
-
-	/* The last step in the calculation involves square root, so we use
-	 * binary search.
-	 * Assuming the object temperature is between 200K and 400K, the search
-	 * should take at most 14 iterations.
-	 */
-	ub = 40000;
-	lb = 20000;
-	while (lb != ub) {
-		int32_t t, rhs, lhs;
-
-		t = (ub + lb) / 2;
-		lhs = t / 100 * t / 10000 * t / 10000 * (S19/100) / 1000 * t;
-		rhs = Tdie / 100 * Tdie / 10000 * Tdie / 10000 * (S19/100) /
-			1000 * Tdie + fv9 * 1000;
-		if (lhs > rhs)
-			ub = t;
-		else
-			lb = t + 1;
-	}
-
-	return ub;
-#endif /* CONFIG_FPU */
 }
 
-/* Temporal Correction
- * Parameters:
- *     T1-T4: Four die temperature readings separated by 1s in 1/100K.
- *     v:     Voltage read from register 0. In nV.
- * Return:
- *     Corrected object voltage in 1/100K.
+/**
+ * Apply TMP006 temporal correction.
+ *
+ * @param T1-T4		Four die temperature readings separated by 1s in 1/100K.
+ * @param Vobj		Voltage read from register 0, in nV.
+ *
+ * @return Corrected object voltage in 1/100K.
  */
-static int tmp006_correct_object_voltage(int T1,
-					      int T2,
-					      int T3,
-					      int T4,
-					      int Vobj)
+static int tmp006_correct_object_voltage(int T1, int T2, int T3, int T4,
+					 int Vobj)
 {
 	int Tslope = 3 * T1 + T2 - T3 - 3 * T4;
 	return Vobj + 296 * Tslope;
 }
 
-static int tmp006_read_object_temp(int idx)
+static int tmp006_read_object_temp(const struct tmp006_data_t *tdata)
 {
-	int pidx = (tmp006_data[idx].tidx - 1) & 0x3;
-	int t = tmp006_data[idx].t[pidx];
-	int v = tmp006_data[idx].v;
+	int pidx = (tdata->tidx - 1) & 0x3;
+	int t = tdata->t[pidx];
+	int v = tdata->v;
 
-	if (tmp006_data[idx].fail)
+	if (tdata->fail)
 		return -1;
 
 	v = tmp006_correct_object_voltage(
 		t,
-		tmp006_data[idx].t[(pidx + 3) & 3],
-		tmp006_data[idx].t[(pidx + 2) & 3],
-		tmp006_data[idx].t[(pidx + 1) & 3],
+		tdata->t[(pidx + 3) & 3],
+		tdata->t[(pidx + 2) & 3],
+		tdata->t[(pidx + 1) & 3],
 		v);
 
-	/* TODO: Calibrate the sensitivity factor. */
-	return tmp006_calculate_object_temp(t, v,
-			tmp006_sensors[idx].sens) / 100;
+	return tmp006_calculate_object_temp(t, v, tdata) / 100;
 }
 
 static int tmp006_poll_sensor(int sensor_id)
 {
+	struct tmp006_data_t *tdata = tmp006_data + sensor_id;
 	int traw, t;
 	int vraw, v;
 	int rv;
 	int addr = tmp006_sensors[sensor_id].addr;
 	int idx;
 
-	/* TODO: For now, all TMP006 sensors are powered by VS. Modify this
-	 *       if we have different design.
-	 */
-	if (gpio_get_level(GPIO_PGOOD_1_8VS) == 0) {
-		tmp006_data[sensor_id].fail = 1;
+	if (!tmp006_has_power(sensor_id)) {
+		tdata->fail |= FAIL_POWER;
 		return EC_ERROR_UNKNOWN;
 	}
 
+	/*
+	 * If sensor has just initialized and/or has lost power, wait for
+	 * data ready; otherwise, we read garbage data.
+	 */
+	if (tdata->fail && (FAIL_POWER | FAIL_INIT)) {
+		rv = i2c_read16(TMP006_PORT(addr), TMP006_REG(addr), 0x02, &v);
+		if (!(v & 0x80)) {
+			tdata->fail |= FAIL_NOT_READY;
+			return EC_ERROR_UNKNOWN;
+		}
+	}
+
 	rv = i2c_read16(TMP006_PORT(addr), TMP006_REG(addr), 0x01, &traw);
 	if (rv) {
-		tmp006_data[sensor_id].fail = 1;
+		tdata->fail |= FAIL_I2C;
 		return EC_ERROR_UNKNOWN;
 	}
+
+	/* Convert temperature from raw to 1/100 K */
 	t = ((int)(int16_t)traw * 100) / 128 + 27300;
 
 	rv = i2c_read16(TMP006_PORT(addr), TMP006_REG(addr), 0x00, &vraw);
 	if (rv) {
-		tmp006_data[sensor_id].fail = 1;
+		tdata->fail |= FAIL_I2C;
 		return EC_ERROR_UNKNOWN;
 	}
+
+	/* Convert voltage from raw to nV */
 	v = ((int)(int16_t)vraw * 15625) / 100;
 
-	idx = tmp006_data[sensor_id].tidx;
-	tmp006_data[sensor_id].t[idx] = t;
-	tmp006_data[sensor_id].v = v;
-	tmp006_data[sensor_id].tidx = (idx + 1) & 3;
-	tmp006_data[sensor_id].fail = 0;
+	/*
+	 * If last read failed, set the entire temperature history to the
+	 * current temperature.  This keeps us from making inaccurate temporal
+	 * corrections based on stale data.
+	 */
+	if (tdata->fail) {
+		for (idx = 0; idx < 4; idx++)
+			tdata->t[idx] = t;
+	} else {
+		idx = tdata->tidx;
+		tdata->t[idx] = t;
+		tdata->tidx = (idx + 1) & 3;
+	}
+
+	tdata->v = v;
+	tdata->fail = 0;
 
 	return EC_SUCCESS;
 }
 
+int tmp006_get_val(int idx)
+{
+	/*
+	 * Note: idx is a thermal sensor index, where the top N-1 bits are the
+	 * TMP006 index and the bottom bit is (0=die, 1=remote).
+	 */
+	const struct tmp006_data_t *tdata = tmp006_data + (idx >> 1);
+
+	/* Check the low bit to determine which temperature to read. */
+	if ((idx & 0x1) == 0)
+		return tmp006_read_die_temp(tdata);
+	else
+		return tmp006_read_object_temp(tdata);
+}
+
+int tmp006_poll(void)
+{
+	int i;
+	int rv;
+	int rv1 = EC_SUCCESS;
+
+	for (i = 0; i < TMP006_COUNT; ++i) {
+		rv = tmp006_poll_sensor(i);
+		if (rv != EC_SUCCESS)
+			rv1 = rv;
+	}
+
+	return rv1;
+}
+
+static int tmp006_init(void)
+{
+	int i;
+
+	for (i = 0; i < TMP006_COUNT; ++i) {
+		struct tmp006_data_t *tdata = tmp006_data + i;
+
+		/* Report error until we actually read the sensor */
+		tdata->fail = FAIL_INIT;
+
+		/*
+		 * TODO: remove default calibration data; sensor should fail
+		 * until calibrated by host or console command.
+		 */
+		tdata->S0 = tmp006_sensors[i].S0;
+		tdata->B0 = B0;
+		tdata->B1 = B1;
+		tdata->B2 = B2;
+	}
+
+	return EC_SUCCESS;
+}
+DECLARE_HOOK(HOOK_INIT, tmp006_init, HOOK_PRIO_DEFAULT);
+
+/*****************************************************************************/
+/* Console commands */
 
 /* Print temperature info for a sensor; used by console command. */
 static int tmp006_print(int idx)
@@ -212,10 +274,7 @@ static int tmp006_print(int idx)
 
 	ccprintf("Debug data from %s:\n", tmp006_sensors[idx].name);
 
-	/* TODO: For now, all TMP006 sensors are powered by VS. Modify this
-	 *       if we have different design.
-	 */
-	if (gpio_get_level(GPIO_PGOOD_1_8VS) == 0) {
+	if (!tmp006_has_power(idx)) {
 		ccputs("Sensor powered off.\n");
 		return EC_ERROR_UNKNOWN;
 	}
@@ -243,55 +302,6 @@ static int tmp006_print(int idx)
 	return EC_SUCCESS;
 }
 
-
-int tmp006_get_val(int idx)
-{
-	/* Check the low bit to determine which temperature to read. */
-	if ((idx & 0x1) == 0)
-		return tmp006_read_die_temp(idx >> 1);
-	else
-		return tmp006_read_object_temp(idx >> 1);
-}
-
-
-int tmp006_poll(void)
-{
-	int i;
-	int rv;
-	int rv1 = EC_SUCCESS;
-
-	for (i = 0; i < TMP006_COUNT; ++i) {
-		rv = tmp006_poll_sensor(i);
-		if (rv != EC_SUCCESS)
-			rv1 = rv;
-	}
-
-	return rv1;
-}
-
-static int tmp006_init(void)
-{
-	int i, j;
-
-	/*
-	 * Set temperature value to 27 C and we will update it later when
-	 * polled by temperature sensor module.
-	 */
-	for (i = 0; i < TMP006_COUNT; ++i) {
-		for (j = 0; j < 4; ++j)
-			tmp006_data[i].t[j] = 30000; /* 27 C */
-		tmp006_data[i].tidx = 0;
-		/* TODO(victoryang): Default value for V? */
-	}
-
-	return EC_SUCCESS;
-}
-DECLARE_HOOK(HOOK_INIT, tmp006_init, HOOK_PRIO_DEFAULT);
-
-
-/*****************************************************************************/
-/* Console commands */
-
 static int command_sensor_info(int argc, char **argv)
 {
 	int i;
diff --git a/include/tmp006.h b/include/tmp006.h
index 6d8b26857d..c1c4b21c24 100644
--- a/include/tmp006.h
+++ b/include/tmp006.h
@@ -14,19 +14,25 @@
 
 struct tmp006_t {
 	const char* name;
-	/* I2C address formed by TMP006_ADDR macro. */
-	int addr;
-	/* Sensitivity factor, in 10^11. */
-	int sens;
+	int addr;          /* I2C address formed by TMP006_ADDR macro. */
+	float S0;          /* Sensitivity factor */
 };
 
-/* Poll all TMP006 sensors. Return 0 on success. */
+/**
+ * Poll all TMP006 sensors.
+ *
+ * @return 0 if successful, non-zero if error. */
 int tmp006_poll(void);
 
-/* Get the last polled value of a sensor. Return temperature in K.
- * The low bit in idx indicate whether to read die temperature or
- * object temperature. The other bits serve as internal index to tmp006
- * module. */
+/**
+ * Get the last polled value of a sensor.
+ *
+ * @param idx		Index to read.  The low bit in idx indicates whether
+ *			to read die temperature or object temperature.  The
+ *			other bits serve as internal index to tmp006 module.
+ *
+ * @return Temperature in K.
+ */
 int tmp006_get_val(int idx);
 
 #endif  /* __CROS_EC_TMP006_H */