dooly: refactor tcs3400_translate_to_xyz()

Refactor tcs3400_translate_to_xyz(): simplify calculations, reduce code
duplication, avoid integer overflow, and guard against division by zero.

Previously there were several places where integer values could
overflow, such as converting 16-bit unsigned integers to fp_t values and
multiplications with results exceeding the range of fp_t.  In practice
operations would overflow when holding a phone flashlight in front of
the light sensor.

BUG=b:179960346
BRANCH=none
TEST=Flashed EC and tested sensor with a phone flashlight.
     Logging output values from old/new implementations showed no
     changes, except when the old implementation overflowed.

Signed-off-by: Nikolai Artemiev <nartemiev@google.com>
Change-Id: I52c37414b5205beaefa2111b1737b0eb22b7235a
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3007377
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Commit-Queue: Gwendal Grignou <gwendal@chromium.org>

1 files changed, 63 insertions, 57 deletions

diff --git a/board/dooly/board.c b/board/dooly/board.c
index bbefcdfe85..c3de9483cd 100644
--- a/board/dooly/board.c
+++ b/board/dooly/board.c
@@ -184,71 +184,77 @@ BUILD_ASSERT(ARRAY_SIZE(motion_als_sensors) == ALS_COUNT);
 static void power_monitor(void);
 DECLARE_DEFERRED(power_monitor);
 
+/* On ECs without an FPU, the fp_t type is backed by a 32-bit fixed precision
+ * representation that can only store values in the range [-32K, +32K]. Some
+ * intermediary values produced in tcs3400_translate_to_xyz() do not fit in
+ * that range, so we define and use a 64-bit fixed representation instead.
+ */
+typedef int64_t fp64_t;
+#define INT_TO_FP64(x)   ((int64_t)(x) << 32)
+#define FP64_TO_INT(x)   ((x) >> 32)
+#define FLOAT_TO_FP64(x) ((int64_t)((x) * (float)(1LL << 32)))
+
 __override void tcs3400_translate_to_xyz(struct motion_sensor_t *s,
-				     int32_t *crgb_data, int32_t *xyz_data)
+					 int32_t *crgb_data, int32_t *xyz_data)
 {
-	int n, cur_gain;
-	fp_t n_interval, rgbc_sum;
-	int integration_time_us;
 	struct tcs_saturation_t *sat_p =
-				&(TCS3400_RGB_DRV_DATA(s+1)->saturation);
+		&(TCS3400_RGB_DRV_DATA(s+1)->saturation);
 
-	cur_gain = (1 << (2 * sat_p->again));
-
-	integration_time_us =
+	int32_t cur_gain = (1 << (2 * sat_p->again));
+	int32_t integration_time_us =
 		tcs3400_get_integration_time(sat_p->atime);
 
-	/* n_interval = (G+B)/C, to use different coefficient*/
-	if (crgb_data[0] != 0)
-		n_interval = INT_TO_FP(crgb_data[2]+crgb_data[3])/crgb_data[0];
-	else
-		n_interval = FLOAT_TO_FP(0.692); /* set default n = 2 */
-
-	if (n_interval < FLOAT_TO_FP(0.692))
-		n = 1;
-	else if (n_interval >= FLOAT_TO_FP(0.692) &&
-			n_interval < FLOAT_TO_FP(1.012))
-		n = 2;
-	else
-		n = 3;
-
-	switch (n) {
-	case 1:
-		rgbc_sum =
-			fp_mul(INT_TO_FP(crgb_data[0]), FLOAT_TO_FP(0.009)) +
-			fp_mul(INT_TO_FP(crgb_data[1]), FLOAT_TO_FP(0.056)) +
-			fp_mul(INT_TO_FP(crgb_data[2]), FLOAT_TO_FP(2.735)) +
-			fp_mul(INT_TO_FP(crgb_data[3]), FLOAT_TO_FP(-1.903));
-
-		xyz_data[1] = FP_TO_INT(fp_mul(FLOAT_TO_FP(799.797), rgbc_sum
-			/ (int)(integration_time_us * cur_gain / 1000ULL)));
-	break;
-	case 2:
-		rgbc_sum =
-			fp_mul(INT_TO_FP(crgb_data[0]), FLOAT_TO_FP(0.202)) +
-			fp_mul(INT_TO_FP(crgb_data[1]), FLOAT_TO_FP(-1.1)) +
-			fp_mul(INT_TO_FP(crgb_data[2]), FLOAT_TO_FP(8.692)) +
-			fp_mul(INT_TO_FP(crgb_data[3]), FLOAT_TO_FP(-7.068));
-
-		xyz_data[1] = FP_TO_INT(fp_mul(FLOAT_TO_FP(801.347), rgbc_sum
-			/ (int)(integration_time_us * cur_gain / 1000ULL)));
-	break;
-	case 3:
-		rgbc_sum =
-			fp_mul(INT_TO_FP(crgb_data[0]), FLOAT_TO_FP(-0.661)) +
-			fp_mul(INT_TO_FP(crgb_data[1]), FLOAT_TO_FP(1.334)) +
-			fp_mul(INT_TO_FP(crgb_data[2]), FLOAT_TO_FP(1.095)) +
-			fp_mul(INT_TO_FP(crgb_data[3]), FLOAT_TO_FP(-1.821));
-
-		xyz_data[1] = FP_TO_INT(fp_mul(FLOAT_TO_FP(795.574), rgbc_sum
-			/ (int)(integration_time_us * cur_gain / 1000ULL)));
-	break;
-	default:
-	break;
+	fp64_t c_coeff, r_coeff, g_coeff, b_coeff;
+	fp64_t result;
+
+	/* Use different coefficients based on n_interval = (G+B)/C */
+	fp64_t gb_sum = INT_TO_FP64(crgb_data[2]) +
+			INT_TO_FP64(crgb_data[3]);
+	fp64_t n_interval = gb_sum / MAX(crgb_data[0], 1);
+
+	if (n_interval < FLOAT_TO_FP64(0.692)) {
+		const float scale = 799.797;
+
+		c_coeff = FLOAT_TO_FP64(0.009  * scale);
+		r_coeff = FLOAT_TO_FP64(0.056  * scale);
+		g_coeff = FLOAT_TO_FP64(2.735  * scale);
+		b_coeff = FLOAT_TO_FP64(-1.903 * scale);
+	} else if (n_interval < FLOAT_TO_FP64(1.012)) {
+		const float scale = 801.347;
+
+		c_coeff = FLOAT_TO_FP64(0.202  * scale);
+		r_coeff = FLOAT_TO_FP64(-1.1   * scale);
+		g_coeff = FLOAT_TO_FP64(8.692  * scale);
+		b_coeff = FLOAT_TO_FP64(-7.068 * scale);
+	} else {
+		const float scale = 795.574;
+
+		c_coeff = FLOAT_TO_FP64(-0.661 * scale);
+		r_coeff = FLOAT_TO_FP64(1.334  * scale);
+		g_coeff = FLOAT_TO_FP64(1.095  * scale);
+		b_coeff = FLOAT_TO_FP64(-1.821 * scale);
 	}
 
-	if (xyz_data[1] < 0)
-		xyz_data[1] = 0;
+	/* Multiply each channel by the coefficient and compute the sum.
+	 * Note: int * fp64_t = fp64_t and fp64_t + fp64_t = fp64_t.
+	 */
+	result = crgb_data[0] * c_coeff +
+		 crgb_data[1] * r_coeff +
+		 crgb_data[2] * g_coeff +
+		 crgb_data[3] * b_coeff;
+
+	/* Adjust for exposure time and sensor gain.
+	 * Note: fp64_t / int = fp64_t.
+	 */
+	result /= MAX(integration_time_us * cur_gain / 1000, 1);
+
+	/* Some C/R/G/B coefficients are negative, so the result could also be
+	 * negative and must be clamped at zero.
+	 *
+	 * The value of xyz_data[1] is stored in a 16 bit integer later on, so
+	 * it must be clamped at INT16_MAX.
+	 */
+	xyz_data[1] = MIN(MAX(FP64_TO_INT(result), 0), INT16_MAX);
 }
 
 static void ppc_interrupt(enum gpio_signal signal)