samus: added gyro support for lsm6ds0

Changed motion_sense task to assume sensors are unpowered in G3
and re-initialize sensors every time coming out of G3.

Added EC command line test utils as well.
Fixed some bug during unit tests.

BUG=chrome-os-partner:27313,27320
BRANCH=ToT
TEST=Verified on Samus.

Tested with accel EC CLIs
accelread, accelrange, accelrate, accelres

Tested accelcalib, a ACCEL calibration util, and it succeeded.

Tested sysfs interface:
cd /sys/bus/iio/devices/iio:device1
 cat in_accel_*_gyro_raw

Signed-off-by: Sheng-Liang Song <ssl@chromium.org>
Change-Id: I5752b00c03e1942c790ea4f28610fda83fa2dcbc
Reviewed-on: https://chromium-review.googlesource.com/211484
Reviewed-by: Alec Berg <alecaberg@chromium.org>

3 files changed, 79 insertions, 61 deletions

diff --git a/include/accelgyro.h b/include/accelgyro.h
index 2a4a458cfa..940a094d82 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -6,104 +6,91 @@
 #ifndef __CROS_EC_ACCELGYRO_H
 #define __CROS_EC_ACCELGYRO_H
 
+#include "motion_sense.h"
+
 /* Header file for accelerometer / gyro drivers. */
 
 /* Number of counts from accelerometer that represents 1G acceleration. */
 #define ACCEL_G  1024
 
-enum accelgyro_chip_t {
-	CHIP_TEST,
-	CHIP_KXCJ9,
-	CHIP_LSM6DS0,
-};
-
-enum accelgyro_sensor_t {
-	SENSOR_ACCELEROMETER,
-	SENSOR_GYRO,
-};
-
-struct accelgyro_info {
-	enum accelgyro_chip_t chip_type;
-	enum accelgyro_sensor_t sensor_type;
-
+struct accelgyro_drv {
 	/**
 	 * Initialize accelerometers.
-	 * @param drv_data Pointer to sensor data.
-	 * @i2c_addr i2c slave device address
+	 * @s Pointer to sensor data pointer. Sensor data will be
+	 * allocated on success.
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*init)(void *drv_data,
-		    int i2c_addr);
+	int (*init)(const struct motion_sensor_t *s);
 
 	/**
 	 * Read all three accelerations of an accelerometer. Note that all
 	 * three accelerations come back in counts, where ACCEL_G can be used
 	 * to convert counts to engineering units.
-	 * @param drv_data Pointer to sensor data.
-	 * @param x_acc Pointer to store X-axis acceleration (in counts).
-	 * @param y_acc Pointer to store Y-axis acceleration (in counts).
-	 * @param z_acc Pointer to store Z-axis acceleration (in counts).
+	 * @s Pointer to sensor data.
+	 * @x_acc Pointer to store X-axis acceleration (in counts).
+	 * @y_acc Pointer to store Y-axis acceleration (in counts).
+	 * @z_acc Pointer to store Z-axis acceleration (in counts).
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*read)(void *drv_data,
-		    int * const x_acc,
-		    int * const y_acc,
-		    int * const z_acc);
+	int (*read)(const struct motion_sensor_t *s,
+			int *x_acc,
+			int *y_acc,
+			int *z_acc);
 
 	/**
 	 * Setter and getter methods for the sensor range. The sensor range
 	 * defines the maximum value that can be returned from read(). As the
 	 * range increases, the resolution gets worse.
-	 * @param drv_data Pointer to sensor data.
-	 * @param range Range (Units are +/- G's for accel, +/- deg/s for gyro)
-	 * @param rnd Rounding flag. If true, it rounds up to nearest valid
+	 * @s Pointer to sensor data.
+	 * @range Range (Units are +/- G's for accel, +/- deg/s for gyro)
+	 * @rnd Rounding flag. If true, it rounds up to nearest valid
 	 * value. Otherwise, it rounds down.
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*set_range)(void *drv_data,
-			 const int range,
-			 const int rnd);
-	int (*get_range)(void *drv_data,
-			 int * const range);
+	int (*set_range)(const struct motion_sensor_t *s,
+			int range,
+			int rnd);
+	int (*get_range)(const struct motion_sensor_t *s,
+			int *range);
 
 	/**
 	 * Setter and getter methods for the sensor resolution.
-	 * @param drv_data Pointer to sensor data.
-	 * @param range Resolution (Units are number of bits)
+	 * @s Pointer to sensor data.
+	 * @range Resolution (Units are number of bits)
 	 * param rnd Rounding flag. If true, it rounds up to nearest valid
 	 * value. Otherwise, it rounds down.
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*set_resolution)(void *drv_data,
-			      const int res,
-			      const int rnd);
-	int (*get_resolution)(void *drv_data,
-			      int * const res);
+	int (*set_resolution)(const struct motion_sensor_t *s,
+				int res,
+				int rnd);
+	int (*get_resolution)(const struct motion_sensor_t *s,
+				int *res);
 
 	/**
 	 * Setter and getter methods for the sensor output data range. As the
 	 * ODR increases, the LPF roll-off frequency also increases.
-	 * @param drv_data Pointer to sensor data.
-	 * @param rate Output data rate (units are mHz)
-	 * @param rnd Rounding flag. If true, it rounds up to nearest valid
+	 * @s Pointer to sensor data.
+	 * @rate Output data rate (units are Hz)
+	 * @rnd Rounding flag. If true, it rounds up to nearest valid
 	 * value. Otherwise, it rounds down.
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*set_datarate)(void *drv_data,
-			    const int rate,
-			    const int rnd);
-	int (*get_datarate)(void *drv_data,
-			    int * const rate);
+	int (*set_data_rate)(const struct motion_sensor_t *s,
+				int rate,
+				int rnd);
+	int (*get_data_rate)(const struct motion_sensor_t *s,
+				int *rate);
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	/**
 	 * Setup a one-time accel interrupt. If the threshold is low enough, the
 	 * interrupt may trigger due simply to noise and not any real motion.
 	 * If the threshold is 0, the interrupt will fire immediately.
-	 * @param drv_data Pointer to sensor data.
-	 * @param threshold Threshold for interrupt in units of counts.
+	 * @s Pointer to sensor data.
+	 * @threshold Threshold for interrupt in units of counts.
 	 */
-	int (*set_interrupt)(void *drv_data,
+	int (*set_interrupt)(const struct motion_sensor_t *s,
 			     unsigned int threshold);
 #endif
 };
diff --git a/include/math_util.h b/include/math_util.h
index ffdbec424b..1dc92c4999 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -40,11 +40,11 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
  * Rotate vector v by rotation matrix R.
  *
  * @param v Vector to be rotated.
- * @param R Pointer to rotation matrix.
- * @param res Pointer to the resultant vector.
+ * @param R Rotation matrix.
+ * @param res Resultant vector.
  */
-void rotate(const vector_3_t v, const matrix_3x3_t (* const R),
-		vector_3_t *res);
+void rotate(const vector_3_t v, const matrix_3x3_t R,
+		vector_3_t res);
 
 
 #ifdef CONFIG_ACCEL_CALIBRATE
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 0319514fd7..4991fcaba4 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -93,20 +93,51 @@ void accel_int_lid(enum gpio_signal signal);
 void accel_int_base(enum gpio_signal signal);
 
 enum sensor_location_t {
-	LOCATION_BASE,
-	LOCATION_LID,
+	LOCATION_BASE = 0,
+	LOCATION_LID  = 1,
+};
+
+enum sensor_type_t {
+	SENSOR_ACCELEROMETER = 0x1,
+	SENSOR_GYRO          = 0x2,
+};
+
+enum sensor_chip_t {
+	SENSOR_CHIP_KXCJ9 = 0,
+	SENSOR_CHIP_LSM6DS0 = 1,
+};
+
+enum sensor_state {
+	SENSOR_NOT_INITIALIZED = 0,
+	SENSOR_INITIALIZED = 1,
+	SENSOR_INIT_ERROR = 2
+};
+
+enum sensor_power {
+	SENSOR_POWER_OFF = 0,
+	SENSOR_POWER_ON  = 1
 };
 
 struct motion_sensor_t {
+	/* RO fields */
 	char *name;
+	enum sensor_chip_t chip;
+	enum sensor_type_t type;
 	enum sensor_location_t location;
-	const struct accelgyro_info *drv;
+	const struct accelgyro_drv *drv;
+	struct mutex *mutex;
 	void *drv_data;
 	uint8_t i2c_addr;
+
+	/* RW fields */
+	enum sensor_state state;
+	enum sensor_power power;
+	vector_3_t raw_xyz;
+	vector_3_t xyz;
 };
 
 /* Defined at board level. */
-extern const struct motion_sensor_t motion_sensors[];
+extern struct motion_sensor_t motion_sensors[];
 extern const unsigned int motion_sensor_count;
 
 #endif /* __CROS_EC_MOTION_SENSE_H */