driver: kionix: Set ODR between 12.5Hz and 1600Hz

Limit the ODR range to write a more compact function.
Given EC typically needs accels running at 10Hz for lid angle
calculation and ODR is limited at ~200Hz by the EC collection speed,
limiting the range is acceptable.

BUG=b:27849483
TEST=tested on Cyan, save 64 bytes on minnie.
BRANCH=veyron,cyan

Change-Id: I284c6fe90fd76b4b60547d4e6f53343df6083b33
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/378720

2 files changed, 25 insertions, 79 deletions

diff --git a/driver/accel_kionix.c b/driver/accel_kionix.c
index 26fa7ff2f9..e2ab1b3e56 100644
--- a/driver/accel_kionix.c
+++ b/driver/accel_kionix.c
@@ -33,75 +33,6 @@
 #else
 #define V(s_) ((s_)->chip == MOTIONSENSE_CHIP_KXCJ9)
 #endif
-/* Index for which table to use. */
-#if !defined(CONFIG_ACCEL_KXCJ9) || !defined(CONFIG_ACCEL_KX022)
-#define T(s_) 0
-#else
-#define T(s_) V(s_)
-#endif /* !defined(CONFIG_ACCEL_KXCJ9) || !defined(CONFIG_ACCEL_KX022) */
-
-/* List of ODR values in mHz and their associated register values. */
-static const struct accel_param_pair datarates[][13] = {
-#ifdef CONFIG_ACCEL_KX022
-	/* One duplicate because table sizes must match. */
-	{ {781,     KX022_OSA_0_781HZ},
-	  {781,     KX022_OSA_0_781HZ},
-	  {1563,    KX022_OSA_1_563HZ},
-	  {3125,    KX022_OSA_3_125HZ},
-	  {6250,    KX022_OSA_6_250HZ},
-	  {12500,   KX022_OSA_12_50HZ},
-	  {25000,   KX022_OSA_25_00HZ},
-	  {50000,   KX022_OSA_50_00HZ},
-	  {100000,  KX022_OSA_100_0HZ},
-	  {200000,  KX022_OSA_200_0HZ},
-	  {400000,  KX022_OSA_400_0HZ},
-	  {800000,  KX022_OSA_800_0HZ},
-	  {1600000, KX022_OSA_1600HZ} },
-#endif /* defined(CONFIG_ACCEL_KX022) */
-#ifdef CONFIG_ACCEL_KXCJ9
-	{ {0,       KXCJ9_OSA_0_000HZ},
-	  {781,     KXCJ9_OSA_0_781HZ},
-	  {1563,    KXCJ9_OSA_1_563HZ},
-	  {3125,    KXCJ9_OSA_3_125HZ},
-	  {6250,    KXCJ9_OSA_6_250HZ},
-	  {12500,   KXCJ9_OSA_12_50HZ},
-	  {25000,   KXCJ9_OSA_25_00HZ},
-	  {50000,   KXCJ9_OSA_50_00HZ},
-	  {100000,  KXCJ9_OSA_100_0HZ},
-	  {200000,  KXCJ9_OSA_200_0HZ},
-	  {400000,  KXCJ9_OSA_400_0HZ},
-	  {800000,  KXCJ9_OSA_800_0HZ},
-	  {1600000, KXCJ9_OSA_1600_HZ} },
-#endif /* defined(CONFIG_ACCEL_KXCJ9) */
-};
-
-/**
- * Find index into a accel_param_pair that matches the given engineering value
- * passed in. The round_up flag is used to specify whether to round up or down.
- * Note, this function always returns a valid index. If the request is
- * outside the range of values, it returns the closest valid index.
- */
-static int find_param_index(const int eng_val, const int round_up,
-			    const struct accel_param_pair *pairs,
-			    const int size)
-{
-	int i;
-
-	/* Linear search for index to match. */
-	for (i = 0; i < size - 1; i++) {
-		if (eng_val <= pairs[i].val)
-			return i;
-
-		if (eng_val < pairs[i+1].val) {
-			if (round_up)
-				return i + 1;
-			else
-				return i;
-		}
-	}
-
-	return i;
-}
 
 /**
  * Read register from accelerometer.
@@ -150,7 +81,7 @@ static int disable_sensor(const struct motion_sensor_t *s, int *reg_val)
 
 		ret = raw_write8(s->addr, reg, *reg_val);
 		if (ret == EC_SUCCESS)
-			return EC_SUCCESS;
+			break;
 	}
 	return ret;
 }
@@ -252,7 +183,6 @@ static int set_resolution(const struct motion_sensor_t *s, int res, int rnd)
 		res_val = KX022_RES_16BIT;
 	res_field = KIONIX_RES_FIELD(V(s));
 	reg = KIONIX_CTRL1_REG(V(s));
-
 	return set_value(s, reg, res_val, res_field);
 }
 
@@ -266,26 +196,37 @@ static int get_resolution(const struct motion_sensor_t *s)
 
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
-	int ret, index, reg, odr_field, odr_val;
+	int ret, reg, odr_field, odr_val, real_rate;
 	struct kionix_accel_data *data = s->drv_data;
 
-	/* Find index for interface pair matching the specified rate. */
-	index = find_param_index(rate, rnd, datarates[T(s)],
-				 ARRAY_SIZE(datarates[T(s)]));
-	odr_val = datarates[T(s)][index].reg;
+	/* Find ODR from rate between 12.5Hz and 1600Hz */
+	if (rate <= 12500) {
+		/*
+		 * On minnie, EC is polling at 10Hz, so that the lowest we
+		 * will got
+		 */
+		odr_val = KX_OSA_12_50HZ;
+	} else {
+		odr_val = 31 - __builtin_clz(rate / 12500);
+	}
+	real_rate = 12500 << odr_val;
+	if (odr_val < KX_OSA_1600HZ && real_rate < rate && rnd) {
+		odr_val++;
+		real_rate *= 2;
+	}
+
 	reg = KIONIX_ODR_REG(V(s));
 	odr_field = KIONIX_ODR_FIELD(V(s));
-
 	ret = set_value(s, reg, odr_val, odr_field);
 	if (ret == EC_SUCCESS)
-		data->sensor_datarate = index;
+		data->sensor_datarate = real_rate;
 	return ret;
 }
 
 static int get_data_rate(const struct motion_sensor_t *s)
 {
 	struct kionix_accel_data *data = s->drv_data;
-	return datarates[T(s)][data->sensor_datarate].val;
+	return data->sensor_datarate;
 }
 
 static int set_offset(const struct motion_sensor_t *s, const int16_t *offset,
diff --git a/driver/accel_kionix.h b/driver/accel_kionix.h
index 2e8f84a82a..449f364a0c 100644
--- a/driver/accel_kionix.h
+++ b/driver/accel_kionix.h
@@ -51,4 +51,9 @@ extern const struct accelgyro_drv kionix_accel_drv;
 #define KIONIX_XOUT_L(v) (KX022_XOUT_L +	\
 			  (v) * (KXCJ9_XOUT_L - KX022_XOUT_L))
 
+/* Common ODR setting: */
+#define KX_OSA_12_50HZ	0
+#define KX_OSA_1600HZ	7
+
+
 #endif /* __CROS_EC_ACCEL_KIONIX_H */