From 93d77ada6ca328044496d4d42364bc6f70b468d5 Mon Sep 17 00:00:00 2001
From: Vic Yang <victoryang@chromium.org>
Date: Wed, 15 Feb 2012 15:36:40 -0800
Subject: [PATCH] Change TMP006 temperature calculation to use FP.

The temperature calculation currently uses fixed point operations.
Change it to use floating point for better readability and maintenance.
Also changes disable_fpu() to accept parameter which serves as
optimization barrier to prevent floating point operations after
disabling FPU.

BUG=chrome-os-partner:7801
TEST=In console, tempremote "tempremote 29715 -105000 6390" gives 28506.

Change-Id: Ib766904b8feb9a78eac9f7cd53afeca85091c5a5
Signed-off-by: Vic Yang <victoryang@chromium.org>
---
 common/temp_sensor.c | 64 +++++++++++++++++++-------------------------
 core/cortex-m/fpu.c  | 21 ++++++++-------
 include/fpu.h        |  6 +++--
 3 files changed, 43 insertions(+), 48 deletions(-)

diff --git a/common/temp_sensor.c b/common/temp_sensor.c
index bc794874ab..80b3166e32 100644
--- a/common/temp_sensor.c
+++ b/common/temp_sensor.c
@@ -11,6 +11,9 @@
 #include "util.h"
 #include "console.h"
 #include "board.h"
+#include "task.h"
+#include "fpu.h"
+#include "math.h"
 
 /* Defined in board_temp_sensor.c. Must be in the same order as
  * in enum temp_sensor_id.
@@ -44,52 +47,39 @@ int temp_sensor_tmp006_read_die_temp(const struct temp_sensor_t* sensor)
  * Parameters:
  *     Tdie: Die temperature in 1/100 K.
  *     Vobj: Voltage read from register 0. In nV.
- *     S0:   Sensitivity factor in 1/1000.
+ *     S0:   Sensitivity factor in 1e-17.
  * Return:
  *     Object temperature in 1/100 K.
  */
-int temp_sensor_tmp006_calculate_object_temp(int Tdie, int Vobj, int S0)
+int temp_sensor_tmp006_calculate_object_temp(int Tdie_i, int Vobj_i, int S0_i)
 {
-	int32_t Tx, S19, Vos, Vx, fv9, ub, lb;
+	float Tdie, Vobj, S0;
+	float Tx, S, Vos, Vx, fv, Tobj, T4;
+	int Tobj_i;
 
-	/* Calculate according to TMP006 users guide.
-	 * 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.
-	 */
-	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;
+	enable_fpu();
+
+	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);
+	/* Vos is the offset voltage */
+	Vos = -2.94e-5f - 5.7e-7f * Tx + 4.63e-9f * Tx * Tx;
 	Vx = Vobj - Vos;
-	/* fv9 is Seebeck coefficient f(Vobj) multipled by 1e9 */
-	fv9 = Vx + 134 * Vx / 100000 * Vx / 100000;
+	/* fv is Seebeck coefficient f(Vobj) */
+	fv = Vx + 13.4f * Vx * Vx;
 
-	/* 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;
+	T4 = Tdie * Tdie * Tdie * Tdie + fv / S;
+	Tobj = sqrtf(sqrtf(T4));
+	Tobj_i = (int32_t)(Tobj * 100.0f);
 
-		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;
-	}
+	disable_fpu(Tobj_i);
 
-	return ub;
+	return Tobj_i;
 }
 
 int temp_sensor_tmp006_read_object_temp(const struct temp_sensor_t* sensor)
diff --git a/core/cortex-m/fpu.c b/core/cortex-m/fpu.c
index a4e1636cbf..806b05438e 100644
--- a/core/cortex-m/fpu.c
+++ b/core/cortex-m/fpu.c
@@ -10,17 +10,20 @@
 void enable_fpu(void)
 {
 	interrupt_disable();
-	asm("mrs r0, control\n"
-	    "orr r0, r0, #(1 << 2)\n"
-	    "msr control, r0\n"
-	    "isb");
+	asm volatile("mrs r0, control;"
+		     "orr r0, r0, #(1 << 2);"
+		     "msr control, r0;"
+		     "isb;");
 }
 
-void disable_fpu(void)
+void disable_fpu(int32_t v)
 {
-	asm("mrs r0, control\n"
-	    "bic r0, r0, #(1 << 2)\n"
-	    "msr control, r0\n"
-	    "isb");
+	/* Optimization barrier to force compiler generate floating point
+	 * calculation code for 'v' before disabling FPU. */
+	asm volatile("" : : "r" (v) : "memory");
+	asm volatile("mrs r0, control;"
+		     "bic r0, r0, #(1 << 2);"
+		     "msr control, r0;"
+		     "isb;");
 	interrupt_enable();
 }
diff --git a/include/fpu.h b/include/fpu.h
index 1e398c7a67..a21992290a 100644
--- a/include/fpu.h
+++ b/include/fpu.h
@@ -11,7 +11,9 @@
 /* Enables FPU. Note that this function also disables interrupt. */
 void enable_fpu(void);
 
-/* Disables FPU. This function also enables interrupt. */
-void disable_fpu(void);
+/* Disables FPU. This function also enables interrupt.
+ * The value passed in serves as optimization barrier. This value would be
+ * calculated before FPU is disabled regardless of compiler optimization. */
+void disable_fpu(int32_t);
 
 #endif  /* __EC_FPU_H */