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>

4 files changed, 577 insertions, 302 deletions

diff --git a/driver/accel_kxcj9.c b/driver/accel_kxcj9.c
index de05f85559..e54564cb6e 100644
--- a/driver/accel_kxcj9.c
+++ b/driver/accel_kxcj9.c
@@ -45,6 +45,7 @@ static const struct accel_param_pair resolutions[] = {
 
 /* List of ODR values in mHz and their associated register values. */
 static const struct accel_param_pair datarates[] = {
+	{0,       KXCJ9_OSA_0_000HZ},
 	{781,     KXCJ9_OSA_0_781HZ},
 	{1563,    KXCJ9_OSA_1_563HZ},
 	{3125,    KXCJ9_OSA_3_125HZ},
@@ -113,33 +114,27 @@ static int raw_write8(const int addr, const int reg, int data)
  *
  * @return EC_SUCCESS if successful, EC_ERROR_* otherwise
  */
-static int disable_sensor(struct kxcj9_data *data, int *ctrl1)
+static int disable_sensor(const struct motion_sensor_t *s, int *ctrl1)
 {
-	int ret;
+	int i, ret;
 
 	/*
-	 * Read the current state of the ctrl1 register so that we can restore
-	 * it later.
+	 * Read the current state of the ctrl1 register
+	 * so that we can restore it later.
 	 */
-	ret = raw_read8(data->accel_addr, KXCJ9_CTRL1, ctrl1);
-	if (ret != EC_SUCCESS)
-		return ret;
+	for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) {
+		ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, ctrl1);
+		if (ret != EC_SUCCESS)
+			continue;
 
-	/*
-	 * Before disabling the sensor, acquire mutex to prevent another task
-	 * from attempting to access accel parameters until we enable sensor.
-	 */
-	mutex_lock(&data->accel_mutex);
+		*ctrl1 &= ~KXCJ9_CTRL1_PC1;
 
-	/* Disable sensor. */
-	*ctrl1 &= ~KXCJ9_CTRL1_PC1;
-	ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, *ctrl1);
-	if (ret != EC_SUCCESS) {
-		mutex_unlock(&data->accel_mutex);
-		return ret;
+		ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, *ctrl1);
+		if (ret == EC_SUCCESS)
+			return EC_SUCCESS;
 	}
-
-	return EC_SUCCESS;
+	CPRINTF("Error trying to disable accelerometer\n");
+	return ret;
 }
 
 /**
@@ -152,49 +147,50 @@ static int disable_sensor(struct kxcj9_data *data, int *ctrl1)
  *
  * @return EC_SUCCESS if successful, EC_ERROR_* otherwise
  */
-static int enable_sensor(struct kxcj9_data *data, const int ctrl1)
+static int enable_sensor(const struct motion_sensor_t *s, int ctrl1)
 {
 	int i, ret;
 
 	for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) {
+		ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, &ctrl1);
+		if (ret != EC_SUCCESS)
+			continue;
+
 		/* Enable accelerometer based on ctrl1 value. */
-		ret = raw_write8(data->accel_addr, KXCJ9_CTRL1,
+		ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1,
 				ctrl1 | KXCJ9_CTRL1_PC1);
 
 		/* On first success, we are done. */
-		if (ret == EC_SUCCESS) {
-			mutex_unlock(&data->accel_mutex);
+		if (ret == EC_SUCCESS)
 			return EC_SUCCESS;
-		}
 	}
 
-	/* Release mutex. */
-	mutex_unlock(&data->accel_mutex);
-
 	/* Cannot enable accel, print warning and return an error. */
 	CPRINTF("Error trying to enable accelerometer\n");
-
 	return ret;
 }
 
-static int accel_set_range(void *drv_data,
-			   const int range,
-			   const int rnd)
+static int set_range(const struct motion_sensor_t *s,
+				int range,
+				int rnd)
 {
 	int ret, ctrl1, ctrl1_new, index;
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 
 	/* Find index for interface pair matching the specified range. */
 	index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges));
 
 	/* Disable the sensor to allow for changing of critical parameters. */
-	ret = disable_sensor(data, &ctrl1);
-	if (ret != EC_SUCCESS)
+	mutex_lock(s->mutex);
+	ret = disable_sensor(s, &ctrl1);
+	if (ret != EC_SUCCESS) {
+		mutex_unlock(s->mutex);
 		return ret;
+	}
 
 	/* Determine new value of CTRL1 reg and attempt to write it. */
 	ctrl1_new = (ctrl1 & ~KXCJ9_GSEL_ALL) | ranges[index].reg;
-	ret = raw_write8(data->accel_addr,  KXCJ9_CTRL1, ctrl1_new);
+	ret = raw_write8(s->i2c_addr,  KXCJ9_CTRL1, ctrl1_new);
 
 	/* If successfully written, then save the range. */
 	if (ret == EC_SUCCESS) {
@@ -203,38 +199,44 @@ static int accel_set_range(void *drv_data,
 	}
 
 	/* Re-enable the sensor. */
-	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
-		return EC_ERROR_UNKNOWN;
+	if (enable_sensor(s, ctrl1) != EC_SUCCESS)
+		ret = EC_ERROR_UNKNOWN;
+
+	mutex_unlock(s->mutex);
 
 	return ret;
 }
 
-static int accel_get_range(void *drv_data, int * const range)
+static int get_range(const struct motion_sensor_t *s,
+				int * const range)
 {
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 	*range = ranges[data->sensor_range].val;
 	return EC_SUCCESS;
 }
 
-static int accel_set_resolution(void *drv_data,
-				const int res,
-				const int rnd)
+static int set_resolution(const struct motion_sensor_t *s,
+				int res,
+				int rnd)
 {
 	int ret, ctrl1, ctrl1_new, index;
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 
 	/* Find index for interface pair matching the specified resolution. */
 	index = find_param_index(res, rnd, resolutions,
 			ARRAY_SIZE(resolutions));
 
 	/* Disable the sensor to allow for changing of critical parameters. */
-	ret = disable_sensor(data, &ctrl1);
-	if (ret != EC_SUCCESS)
+	mutex_lock(s->mutex);
+	ret = disable_sensor(s, &ctrl1);
+	if (ret != EC_SUCCESS) {
+		mutex_unlock(s->mutex);
 		return ret;
+	}
 
 	/* Determine new value of CTRL1 reg and attempt to write it. */
 	ctrl1_new = (ctrl1 & ~KXCJ9_RES_12BIT) | resolutions[index].reg;
-	ret = raw_write8(data->accel_addr,  KXCJ9_CTRL1, ctrl1_new);
+	ret = raw_write8(s->i2c_addr,  KXCJ9_CTRL1, ctrl1_new);
 
 	/* If successfully written, then save the range. */
 	if (ret == EC_SUCCESS) {
@@ -243,36 +245,41 @@ static int accel_set_resolution(void *drv_data,
 	}
 
 	/* Re-enable the sensor. */
-	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
-		return EC_ERROR_UNKNOWN;
+	if (enable_sensor(s, ctrl1) != EC_SUCCESS)
+		ret = EC_ERROR_UNKNOWN;
 
+	mutex_unlock(s->mutex);
 	return ret;
 }
 
-static int accel_get_resolution(void *drv_data, int * const res)
+static int get_resolution(const struct motion_sensor_t *s,
+			int *res)
 {
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 	*res = resolutions[data->sensor_resolution].val;
 	return EC_SUCCESS;
 }
 
-static int accel_set_datarate(void *drv_data,
-			      const int rate,
-			      const int rnd)
+static int set_data_rate(const struct motion_sensor_t *s,
+			int rate,
+			int rnd)
 {
 	int ret, ctrl1, index;
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 
 	/* Find index for interface pair matching the specified rate. */
 	index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates));
 
 	/* Disable the sensor to allow for changing of critical parameters. */
-	ret = disable_sensor(data, &ctrl1);
-	if (ret != EC_SUCCESS)
+	mutex_lock(s->mutex);
+	ret = disable_sensor(s, &ctrl1);
+	if (ret != EC_SUCCESS) {
+		mutex_unlock(s->mutex);
 		return ret;
+	}
 
 	/* Set output data rate. */
-	ret = raw_write8(data->accel_addr,  KXCJ9_DATA_CTRL,
+	ret = raw_write8(s->i2c_addr,  KXCJ9_DATA_CTRL,
 			datarates[index].reg);
 
 	/* If successfully written, then save the range. */
@@ -280,33 +287,39 @@ static int accel_set_datarate(void *drv_data,
 		data->sensor_datarate = index;
 
 	/* Re-enable the sensor. */
-	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
-		return EC_ERROR_UNKNOWN;
+	if (enable_sensor(s, ctrl1) != EC_SUCCESS)
+		ret = EC_ERROR_UNKNOWN;
 
+	mutex_unlock(s->mutex);
 	return ret;
 }
 
-static int accel_get_datarate(void *drv_data, int * const rate)
+static int get_data_rate(const struct motion_sensor_t *s,
+				int *rate)
 {
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 	*rate = datarates[data->sensor_datarate].val;
 	return EC_SUCCESS;
 }
 
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
-static int accel_set_interrupt(void *drv_data, unsigned int threshold)
+static int set_interrupt(const struct motion_sensor_t *s,
+		unsigned int threshold)
 {
 	int ctrl1, tmp, ret;
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 
 	/* Disable the sensor to allow for changing of critical parameters. */
-	ret = disable_sensor(data, &ctrl1);
-	if (ret != EC_SUCCESS)
+	mutex_lock(s->mutex);
+	ret = disable_sensor(s, &ctrl1);
+	if (ret != EC_SUCCESS) {
+		mutex_unlock(s->mutex);
 		return ret;
+	}
 
 	/* Set interrupt timer to 1 so it wakes up immediately. */
-	ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_TIMER, 1);
+	ret = raw_write8(s->i2c_addr, KXCJ9_WAKEUP_TIMER, 1);
 	if (ret != EC_SUCCESS)
 		goto error_enable_sensor;
 
@@ -315,7 +328,7 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold)
 	 * first we need to divide by 16 to get the value to send.
 	 */
 	threshold >>= 4;
-	ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_THRESHOLD, threshold);
+	ret = raw_write8(s->i2c_addr, KXCJ9_WAKEUP_THRESHOLD, threshold);
 	if (ret != EC_SUCCESS)
 		goto error_enable_sensor;
 
@@ -324,11 +337,11 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold)
 	 * function is called once, the interrupt stays enabled and it is
 	 * only necessary to clear KXCJ9_INT_REL to allow the next interrupt.
 	 */
-	ret = raw_read8(data->accel_addr, KXCJ9_INT_CTRL1, &tmp);
+	ret = raw_read8(s->i2c_addr, KXCJ9_INT_CTRL1, &tmp);
 	if (ret != EC_SUCCESS)
 		goto error_enable_sensor;
 	if (!(tmp & KXCJ9_INT_CTRL1_IEN)) {
-		ret = raw_write8(data->accel_addr, KXCJ9_INT_CTRL1,
+		ret = raw_write8(s->i2c_addr, KXCJ9_INT_CTRL1,
 				tmp | KXCJ9_INT_CTRL1_IEN);
 		if (ret != EC_SUCCESS)
 			goto error_enable_sensor;
@@ -339,34 +352,34 @@ static int accel_set_interrupt(void *drv_data, unsigned int threshold)
 	 * Note: this register latches motion detected above threshold. Once
 	 * latched, no interrupt can occur until this register is cleared.
 	 */
-	ret = raw_read8(data->accel_addr, KXCJ9_INT_REL, &tmp);
+	ret = raw_read8(s->i2c_addr, KXCJ9_INT_REL, &tmp);
 
 error_enable_sensor:
 	/* Re-enable the sensor. */
-	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
-		return EC_ERROR_UNKNOWN;
-
+	if (enable_sensor(s, ctrl1) != EC_SUCCESS)
+		ret = EC_ERROR_UNKNOWN;
+	mutex_unlock(s->mutex);
 	return ret;
 }
 #endif
 
-static int accel_read(void *drv_data,
-		      int * const x_acc,
-		      int * const y_acc,
-		      int * const z_acc)
+static int read(const struct motion_sensor_t *s,
+			int *x_acc,
+			int *y_acc,
+			int *z_acc)
 {
 	uint8_t acc[6];
 	uint8_t reg = KXCJ9_XOUT_L;
 	int ret, multiplier;
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	struct kxcj9_data *data = (struct kxcj9_data *)s->drv_data;
 
 	/* Read 6 bytes starting at KXCJ9_XOUT_L. */
-	mutex_lock(&data->accel_mutex);
+	mutex_lock(s->mutex);
 	i2c_lock(I2C_PORT_ACCEL, 1);
-	ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, &reg, 1, acc, 6,
+	ret = i2c_xfer(I2C_PORT_ACCEL, s->i2c_addr, &reg, 1, acc, 6,
 			I2C_XFER_SINGLE);
 	i2c_lock(I2C_PORT_ACCEL, 0);
-	mutex_unlock(&data->accel_mutex);
+	mutex_unlock(s->mutex);
 
 	if (ret != EC_SUCCESS)
 		return ret;
@@ -405,74 +418,42 @@ static int accel_read(void *drv_data,
 	return EC_SUCCESS;
 }
 
-static int accel_init(void *drv_data, int i2c_addr)
+#ifdef CONFIG_ACCEL_INTERRUPTS
+static int config_interrupt(const struct motion_sensor_t *s)
 {
-	int ret = EC_SUCCESS;
-	int cnt = 0, ctrl1, ctrl2;
-	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
-
-	if (data == NULL)
-		return EC_ERROR_INVAL;
-
-	memset(&data->accel_mutex, sizeof(struct mutex), 0);
-	data->sensor_range = 0;
-	data->sensor_datarate = 6;
-	data->sensor_resolution = 1;
-	data->accel_addr = i2c_addr;
+	int ctrl1;
+	mutex_lock(s->mutex);
 
 	/* Disable the sensor to allow for changing of critical parameters. */
-	ret = disable_sensor(data, &ctrl1);
-	if (ret != EC_SUCCESS)
-		return ret;
-
-	/*
-	 * This sensor can be powered through an EC reboot, so the state of
-	 * the sensor is unknown here. Initiate software reset to restore
-	 * sensor to default.
-	 */
-	ret = raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_CTRL2_SRST);
+	ret = disable_sensor(s, &ctrl1);
 	if (ret != EC_SUCCESS)
-		return ret;
-
-	/* Wait until software reset is complete or timeout. */
-	while (1) {
-		ret = raw_read8(data->accel_addr, KXCJ9_CTRL2, &ctrl2);
+		goto cleanup_exit;
 
-		/* Reset complete. */
-		if (ret == EC_SUCCESS && !(ctrl2 & KXCJ9_CTRL2_SRST))
-			break;
-
-		/* Check for timeout. */
-		if (cnt++ > 5)
-			return EC_ERROR_TIMEOUT;
-
-		/* Give more time for reset action to complete. */
-		msleep(10);
-	}
-
-	/* Set resolution and range. */
-	ctrl1 = resolutions[data->sensor_resolution].reg |
-			ranges[data->sensor_range].reg;
-#ifdef CONFIG_ACCEL_INTERRUPTS
 	/* Enable wake up (motion detect) functionality. */
-	ctrl1 |= KXCJ9_CTRL1_WUFE;
-#endif
-	ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1);
+	ret = raw_read8(s->i2c_addr, KXCJ9_CTRL1, &tmp);
+	tmp &= ~KXCJ9_CTRL1_PC1;
+	tmp |= KXCJ9_CTRL1_WUFE;
+	ret = raw_write8(s->i2c_addr, KXCJ9_CTRL1, tmp);
 
-#ifdef CONFIG_ACCEL_INTERRUPTS
 	/* Set interrupt polarity to rising edge and keep interrupt disabled. */
-	ret |= raw_write8(data->accel_addr,
+	ret = raw_write8(s->i2c_addr,
 			  KXCJ9_INT_CTRL1,
 			  KXCJ9_INT_CTRL1_IEA);
+	if (ret != EC_SUCCESS)
+		goto cleanup_exit;
 
 	/* Set output data rate for wake-up interrupt function. */
-	ret |= raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ);
+	ret = raw_write8(s->i2c_addr, KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ);
+	if (ret != EC_SUCCESS)
+		goto cleanup_exit;
 
 	/* Set interrupt to trigger on motion on any axis. */
-	ret |= raw_write8(data->accel_addr, KXCJ9_INT_CTRL2,
+	ret = raw_write8(s->i2c_addr, KXCJ9_INT_CTRL2,
 			KXCJ9_INT_SRC2_XNWU | KXCJ9_INT_SRC2_XPWU |
 			KXCJ9_INT_SRC2_YNWU | KXCJ9_INT_SRC2_YPWU |
 			KXCJ9_INT_SRC2_ZNWU | KXCJ9_INT_SRC2_ZPWU);
+	if (ret != EC_SUCCESS)
+		goto cleanup_exit;
 
 	/*
 	 * Enable accel interrupts. Note: accels will not initiate an interrupt
@@ -480,30 +461,85 @@ static int accel_init(void *drv_data, int i2c_addr)
 	 */
 	gpio_enable_interrupt(GPIO_ACCEL_INT_LID);
 	gpio_enable_interrupt(GPIO_ACCEL_INT_BASE);
+
+	/* Enable the sensor. */
+	ret = enable_sensor(s, ctrl1);
+cleanup_exit:
+	mutex_unlock(s->mutex);
+	return ret;
+}
 #endif
 
-	/* Set output data rate. */
-	ret |= raw_write8(data->accel_addr, KXCJ9_DATA_CTRL,
-			datarates[data->sensor_datarate].reg);
+static int init(const struct motion_sensor_t *s)
+{
+	int ret = EC_SUCCESS;
+	int cnt = 0, tmp, range, rate;
 
-	/* Enable the sensor. */
-	ret |= enable_sensor(data, ctrl1);
+	/*
+	 * This sensor can be powered through an EC reboot, so the state of
+	 * the sensor is unknown here. Initiate software reset to restore
+	 * sensor to default.
+	 */
+	mutex_lock(s->mutex);
+	ret = raw_write8(s->i2c_addr, KXCJ9_CTRL2, KXCJ9_CTRL2_SRST);
+	mutex_unlock(s->mutex);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* Wait until software reset is complete or timeout. */
+	do {
+		/* Added 1m delay after software reset */
+		msleep(1);
+
+		ret = raw_read8(s->i2c_addr, KXCJ9_CTRL2, &tmp);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/* Reset complete. */
+		if (ret == EC_SUCCESS && !(tmp & KXCJ9_CTRL2_SRST))
+			break;
+
+		/* Check for timeout. */
+		if (cnt++ > 5) {
+			ret = EC_ERROR_TIMEOUT;
+			CPRINTF("%s: SRST Error.\n", s->name);
+			return ret;
+		}
+	} while (1);
+
+	ret = set_range(s, 2, 1);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = set_resolution(s, 12, 1);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = set_data_rate(s, 100000, 1);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	config_interrupt(s);
+#endif
+	get_range(s, &range);
+	get_data_rate(s, &rate);
+	CPRINTF("[%T %s: Done Init type:0x%X range:%d rate:%d]\n",
+		s->name, s->type, range, rate);
 
 	return ret;
 }
 
-const struct accelgyro_info accel_kxcj9 = {
-	.chip_type = CHIP_KXCJ9,
-	.sensor_type = SENSOR_ACCELEROMETER,
-	.init = accel_init,
-	.read = accel_read,
-	.set_range = accel_set_range,
-	.get_range = accel_get_range,
-	.set_resolution = accel_set_resolution,
-	.get_resolution = accel_get_resolution,
-	.set_datarate = accel_set_datarate,
-	.get_datarate = accel_get_datarate,
+const struct accelgyro_drv kxcj9_drv = {
+	.init = init,
+	.read = read,
+	.set_range = set_range,
+	.get_range = get_range,
+	.set_resolution = set_resolution,
+	.get_resolution = get_resolution,
+	.set_data_rate = set_data_rate,
+	.get_data_rate = get_data_rate,
 #ifdef CONFIG_ACCEL_INTERRUPTS
-	.set_interrupt = accel_set_interrupt,
+	.set_interrupt = set_interrupt,
 #endif
 };
diff --git a/driver/accel_kxcj9.h b/driver/accel_kxcj9.h
index 30bea0e737..37ff2eac66 100644
--- a/driver/accel_kxcj9.h
+++ b/driver/accel_kxcj9.h
@@ -88,6 +88,7 @@
 #define KXCJ9_INT_CTRL2_XPWUE		(1 << 4)
 #define KXCJ9_INT_CTRL2_XNWUE		(1 << 5)
 
+#define KXCJ9_OSA_0_000HZ	0
 #define KXCJ9_OSA_0_781HZ	8
 #define KXCJ9_OSA_1_563HZ	9
 #define KXCJ9_OSA_3_125HZ	0xa
@@ -102,7 +103,6 @@
 #define KXCJ9_OSA_1600_HZ	7
 
 struct kxcj9_data {
-	struct mutex accel_mutex;
 	/* Current range of accelerometer. */
 	int sensor_range;
 	/* Current output data rate of accelerometer. */
@@ -113,6 +113,6 @@ struct kxcj9_data {
 	int accel_addr;
 };
 
-extern const struct accelgyro_info accel_kxcj9;
+extern const struct accelgyro_drv kxcj9_drv;
 
 #endif /* __CROS_EC_ACCEL_KXCJ9_H */
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c
index 313ec9a312..8d50a64373 100644
--- a/driver/accelgyro_lsm6ds0.c
+++ b/driver/accelgyro_lsm6ds0.c
@@ -3,7 +3,10 @@
  * found in the LICENSE file.
  */
 
-/* LSM6DS0 accelerometer and gyro module for Chrome EC */
+/**
+ * LSM6DS0 accelerometer and gyro module for Chrome EC
+ * 3D digital accelerometer & 3D digital gyroscope
+ */
 
 #include "accelgyro.h"
 #include "common.h"
@@ -14,63 +17,139 @@
 #include "task.h"
 #include "util.h"
 
+#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+
 /*
  * Struct for pairing an engineering value with the register value for a
  * parameter.
  */
 struct accel_param_pair {
 	int val; /* Value in engineering units. */
-	int reg; /* Corresponding register value. */
+	int reg_val; /* Corresponding register value. */
 };
 
 /* List of range values in +/-G's and their associated register values. */
-static const struct accel_param_pair ranges[] = {
+static const struct accel_param_pair g_ranges[] = {
 	{2, LSM6DS0_GSEL_2G},
 	{4, LSM6DS0_GSEL_4G},
 	{8, LSM6DS0_GSEL_8G}
 };
 
-/* List of ODR values in mHz and their associated register values. */
-static const struct accel_param_pair datarates[] = {
+/*
+ * List of angular rate range values in +/-dps's
+ * and their associated register values.
+ */
+const struct accel_param_pair dps_ranges[] = {
+	{245, LSM6DS0_DPS_SEL_245},
+	{500, LSM6DS0_DPS_SEL_500},
+	{1000, LSM6DS0_DPS_SEL_1000},
+	{2000, LSM6DS0_DPS_SEL_2000}
+};
+
+static inline const struct accel_param_pair *get_range_table(
+		enum sensor_type_t type, int *psize)
+{
+	if (SENSOR_ACCELEROMETER == type) {
+		if (psize)
+			*psize = ARRAY_SIZE(g_ranges);
+		return g_ranges;
+	} else {
+		if (psize)
+			*psize = ARRAY_SIZE(dps_ranges);
+		return dps_ranges;
+	}
+}
+
+/* List of ODR (gyro off) values in mHz and their associated register values.*/
+const struct accel_param_pair gyro_on_odr[] = {
+	{0,        LSM6DS0_ODR_PD},
+	{15000,    LSM6DS0_ODR_15HZ},
+	{59000,    LSM6DS0_ODR_59HZ},
+	{119000,   LSM6DS0_ODR_119HZ},
+	{238000,   LSM6DS0_ODR_238HZ},
+	{476000,   LSM6DS0_ODR_476HZ},
+	{952000,   LSM6DS0_ODR_952HZ}
+};
+
+/* List of ODR (gyro on) values in mHz and their associated register values. */
+const struct accel_param_pair gyro_off_odr[] = {
+	{0,        LSM6DS0_ODR_PD},
 	{10000,    LSM6DS0_ODR_10HZ},
 	{50000,    LSM6DS0_ODR_50HZ},
 	{119000,   LSM6DS0_ODR_119HZ},
 	{238000,   LSM6DS0_ODR_238HZ},
 	{476000,   LSM6DS0_ODR_476HZ},
-	{952000,   LSM6DS0_ODR_982HZ}
+	{952000,   LSM6DS0_ODR_952HZ}
 };
 
+static inline const struct accel_param_pair *get_odr_table(
+		enum sensor_type_t type, int *psize)
+{
+	if (SENSOR_ACCELEROMETER == type) {
+		if (psize)
+			*psize = ARRAY_SIZE(gyro_off_odr);
+		return gyro_off_odr;
+	} else {
+		if (psize)
+			*psize = ARRAY_SIZE(gyro_on_odr);
+		return gyro_on_odr;
+	}
+}
+
+static inline int get_ctrl_reg(enum sensor_type_t type)
+{
+	return (SENSOR_ACCELEROMETER == type) ?
+		LSM6DS0_CTRL_REG6_XL : LSM6DS0_CTRL_REG1_G;
+}
+
+static inline int get_xyz_reg(enum sensor_type_t type)
+{
+	return (SENSOR_ACCELEROMETER == type) ?
+		LSM6DS0_OUT_X_L_XL : LSM6DS0_OUT_X_L_G;
+}
+
 /**
- * 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.
+ * @return reg value 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 reg value. If the request is
+ * outside the range of values, it returns the closest valid reg value.
  */
-static int find_param_index(const int eng_val, const int round_up,
+static int get_reg_val(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;
+			break;
 
 		if (eng_val < pairs[i+1].val) {
 			if (round_up)
-				return i + 1;
-			else
-				return i;
+				i += 1;
+			break;
 		}
 	}
+	return pairs[i].reg_val;
+}
 
-	return i;
+/**
+ * @return engineering value that matches the given reg val
+ */
+static int get_engineering_val(const int reg_val,
+		const struct accel_param_pair *pairs, const int size)
+{
+	int i;
+	for (i = 0; i < size; i++) {
+		if (reg_val == pairs[i].reg_val)
+			break;
+	}
+	return pairs[i].val;
 }
 
 /**
  * Read register from accelerometer.
  */
-static int raw_read8(const int addr, const int reg, int *data_ptr)
+static inline int raw_read8(const int addr, const int reg, int *data_ptr)
 {
 	return i2c_read8(I2C_PORT_ACCEL, addr, reg, data_ptr);
 }
@@ -78,107 +157,139 @@ static int raw_read8(const int addr, const int reg, int *data_ptr)
 /**
  * Write register from accelerometer.
  */
-static int raw_write8(const int addr, const int reg, int data)
+static inline int raw_write8(const int addr, const int reg, int data)
 {
 	return i2c_write8(I2C_PORT_ACCEL, addr, reg, data);
 }
 
-static int accel_set_range(void *drv_data,
-			   const int range,
-			   const int rnd)
+static int set_range(const struct motion_sensor_t *s,
+				int range,
+				int rnd)
 {
-	int ret, index, ctrl_reg6;
-	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
+	int ret, ctrl_val, range_tbl_size;
+	uint8_t ctrl_reg, reg_val;
+	const struct accel_param_pair *ranges;
 
-	/* Find index for interface pair matching the specified range. */
-	index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges));
+	ctrl_reg = get_ctrl_reg(s->type);
+	ranges = get_range_table(s->type, &range_tbl_size);
+
+	reg_val = get_reg_val(range, rnd, ranges, range_tbl_size);
 
 	/*
 	 * Lock accel resource to prevent another task from attempting
 	 * to write accel parameters until we are done.
 	 */
-	mutex_lock(&data->accel_mutex);
+	mutex_lock(s->mutex);
 
-	ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
+	ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val);
 	if (ret != EC_SUCCESS)
 		goto accel_cleanup;
 
-	ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_GSEL_ALL) | ranges[index].reg;
-	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
+	ctrl_val = (ctrl_val & ~LSM6DS0_RANGE_MASK) | reg_val;
+	ret = raw_write8(s->i2c_addr, ctrl_reg, ctrl_val);
 
 accel_cleanup:
-	/* Unlock accel resource and save new range if written successfully. */
-	mutex_unlock(&data->accel_mutex);
-	if (ret == EC_SUCCESS)
-		data->sensor_range = index;
-
+	mutex_unlock(s->mutex);
 	return EC_SUCCESS;
 }
 
-static int accel_get_range(void *drv_data, int * const range)
+static int get_range(const struct motion_sensor_t *s,
+				int *range)
 {
-	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
-	*range = ranges[data->sensor_range].val;
-	return EC_SUCCESS;
+	int ret, ctrl_val, range_tbl_size;
+	uint8_t ctrl_reg;
+	const struct accel_param_pair *ranges;
+	ranges = get_range_table(s->type, &range_tbl_size);
+	ctrl_reg = get_ctrl_reg(s->type);
+	ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val);
+	*range = get_engineering_val(ctrl_val & LSM6DS0_RANGE_MASK,
+		ranges, range_tbl_size);
+	return ret;
 }
 
-static int accel_set_resolution(void *drv_data,
-				const int res,
-				const int rnd)
+static int set_resolution(const struct motion_sensor_t *s,
+				int res,
+				int rnd)
 {
 	/* Only one resolution, LSM6DS0_RESOLUTION, so nothing to do. */
 	return EC_SUCCESS;
 }
 
-static int accel_get_resolution(void *drv_data,
-				int * const res)
+static int get_resolution(const struct motion_sensor_t *s,
+				int *res)
 {
 	*res = LSM6DS0_RESOLUTION;
 	return EC_SUCCESS;
 }
 
-static int accel_set_datarate(void *drv_data,
-			      const int rate,
-			      const int rnd)
+static int set_data_rate(const struct motion_sensor_t *s,
+				int rate,
+				int rnd)
 {
-	int ret, index, ctrl_reg6;
-	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
+	int ret, val, odr_tbl_size;
+	uint8_t ctrl_reg, reg_val;
+	const struct accel_param_pair *data_rates;
 
-	/* Find index for interface pair matching the specified range. */
-	index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates));
+	ctrl_reg = get_ctrl_reg(s->type);
+	data_rates = get_odr_table(s->type, &odr_tbl_size);
+	reg_val = get_reg_val(rate, rnd, data_rates, odr_tbl_size);
 
 	/*
 	 * Lock accel resource to prevent another task from attempting
 	 * to write accel parameters until we are done.
 	 */
-	mutex_lock(&data->accel_mutex);
+	mutex_lock(s->mutex);
 
-	ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
+	ret = raw_read8(s->i2c_addr, ctrl_reg, &val);
 	if (ret != EC_SUCCESS)
 		goto accel_cleanup;
 
-	ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_ODR_ALL) | datarates[index].reg;
-	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
+	val = (val & ~LSM6DS0_ODR_MASK) | reg_val;
+	ret = raw_write8(s->i2c_addr, ctrl_reg, val);
 
-accel_cleanup:
-	/* Unlock accel resource and save new ODR if written successfully. */
-	mutex_unlock(&data->accel_mutex);
-	if (ret == EC_SUCCESS)
-		data->sensor_datarate = index;
+	/* CTRL_REG3_G 12h
+	 * [7] low-power mode = 0;
+	 * [6] high pass filter disabled;
+	 * [5:4] 0 keep const 0
+	 * [3:0] HPCF_G
+	 *       Table 48 Gyroscope high-pass filter cutoff frequency
+	 */
+	if (SENSOR_GYRO == s->type) {
+		ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG3_G, &val);
+		if (ret != EC_SUCCESS)
+			goto accel_cleanup;
+		val &= ~(0x3 << 4); /* clear bit [5:4] */
+		val = (rate > 119000) ?
+			(val | (1<<7))   /* set high-power mode */ :
+			(val & ~(1<<7)); /* set low-power mode */
+		ret = raw_write8(s->i2c_addr, LSM6DS0_CTRL_REG3_G, val);
+	}
 
+accel_cleanup:
+	mutex_unlock(s->mutex);
 	return EC_SUCCESS;
 }
 
-static int accel_get_datarate(void *drv_data,
-			      int * const rate)
+static int get_data_rate(const struct motion_sensor_t *s,
+				int *rate)
 {
-	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
-	*rate = datarates[data->sensor_datarate].val;
+	int ret, ctrl_val, odr_tbl_size;
+	uint8_t ctrl_reg;
+	const struct accel_param_pair *data_rates;
+	ctrl_reg = get_ctrl_reg(s->type);
+
+	ret = raw_read8(s->i2c_addr, ctrl_reg, &ctrl_val);
+	if (ret != EC_SUCCESS)
+		return EC_ERROR_UNKNOWN;
+
+	data_rates = get_odr_table(s->type, &odr_tbl_size);
+	*rate = get_engineering_val(ctrl_val & LSM6DS0_ODR_MASK,
+			data_rates, odr_tbl_size);
 	return EC_SUCCESS;
 }
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
-static int accel_set_interrupt(void *drv_data,
+static int set_interrupt(const struct motion_sensor_t *s,
 			       unsigned int threshold)
 {
 	/* Currently unsupported. */
@@ -186,103 +297,169 @@ static int accel_set_interrupt(void *drv_data,
 }
 #endif
 
-static int accel_read(void *drv_data,
-		      int * const x_acc,
-		      int * const y_acc,
-		      int * const z_acc)
+static int is_data_ready(const struct motion_sensor_t *s, int *ready)
+{
+	int ret, tmp;
+
+	ret = raw_read8(s->i2c_addr, LSM6DS0_STATUS_REG, &tmp);
+
+	if (ret != EC_SUCCESS) {
+		CPRINTF("[%T %s type:0x%X RS Error]", s->name, s->type);
+		return ret;
+	}
+
+	if (SENSOR_ACCELEROMETER == s->type)
+		*ready = (LSM6DS0_STS_XLDA_UP == (tmp & LSM6DS0_STS_XLDA_MASK));
+	else
+		*ready = (LSM6DS0_STS_GDA_UP == (tmp & LSM6DS0_STS_GDA_MASK));
+
+	return EC_SUCCESS;
+}
+
+static int read(const struct motion_sensor_t *s,
+			int *x,
+			int *y,
+			int *z)
 {
-	uint8_t acc[6];
-	uint8_t reg = LSM6DS0_OUT_X_L_XL;
-	int ret, multiplier;
-	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
+	uint8_t data[6];
+	uint8_t xyz_reg;
+	int ret, tmp = 0, range = 0;
+
+	ret = is_data_ready(s, &tmp);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/*
+	 * If sensor data is not ready, return the previous read data.
+	 * Note: return success so that motion senor task can read again
+	 * to get the latest updated sensor data quickly.
+	 */
+	if (!tmp) {
+		*x = s->raw_xyz[0];
+		*y = s->raw_xyz[1];
+		*z = s->raw_xyz[2];
+		return EC_SUCCESS;
+	}
+
+	xyz_reg = get_xyz_reg(s->type);
 
-	/* Read 6 bytes starting at LSM6DS0_OUT_X_L_XL. */
-	mutex_lock(&data->accel_mutex);
+	/* Read 6 bytes starting at xyz_reg */
 	i2c_lock(I2C_PORT_ACCEL, 1);
-	ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, &reg, 1, acc, 6,
-			I2C_XFER_SINGLE);
+	ret = i2c_xfer(I2C_PORT_ACCEL, s->i2c_addr,
+			&xyz_reg, 1, data, 6, I2C_XFER_SINGLE);
 	i2c_lock(I2C_PORT_ACCEL, 0);
-	mutex_unlock(&data->accel_mutex);
 
-	if (ret != EC_SUCCESS)
+	if (ret != EC_SUCCESS) {
+		CPRINTF("[%T %s type:0x%X RD XYZ Error]",
+			s->name, s->type);
 		return ret;
+	}
 
-	/* Determine multiplier based on stored range. */
-	switch (ranges[data->sensor_range].reg) {
-	case LSM6DS0_GSEL_2G:
-		multiplier = 1;
-		break;
-	case LSM6DS0_GSEL_4G:
-		multiplier = 2;
-		break;
-	case LSM6DS0_GSEL_8G:
-		multiplier = 4;
-		break;
-	default:
+	*x = ((int16_t)((data[1] << 8) | data[0]));
+	*y = ((int16_t)((data[3] << 8) | data[2]));
+	*z = ((int16_t)((data[5] << 8) | data[4]));
+
+	ret = get_range(s, &range);
+	if (ret)
 		return EC_ERROR_UNKNOWN;
-	}
 
-	/*
-	 * Convert data to signed 12-bit value. Note order of registers:
-	 *
-	 * acc[0] = LSM6DS0_OUT_X_L_XL
-	 * acc[1] = LSM6DS0_OUT_X_H_XL
-	 * acc[2] = LSM6DS0_OUT_Y_L_XL
-	 * acc[3] = LSM6DS0_OUT_Y_H_XL
-	 * acc[4] = LSM6DS0_OUT_Z_L_XL
-	 * acc[5] = LSM6DS0_OUT_Z_H_XL
-	 */
-	*x_acc = multiplier * ((int16_t)(acc[1] << 8 | acc[0])) >> 4;
-	*y_acc = multiplier * ((int16_t)(acc[3] << 8 | acc[2])) >> 4;
-	*z_acc = multiplier * ((int16_t)(acc[5] << 8 | acc[4])) >> 4;
+	*x *= range;
+	*y *= range;
+	*z *= range;
+
+	/* normalize the accel scale: 1G = 1024 */
+	if (SENSOR_ACCELEROMETER == s->type) {
+		*x >>= 5;
+		*y >>= 5;
+		*z >>= 5;
+	} else {
+		*x >>= 8;
+		*y >>= 8;
+		*z >>= 8;
+	}
 
 	return EC_SUCCESS;
 }
 
-static int accel_init(void *drv_data, int i2c_addr)
+static int init(const struct motion_sensor_t *s)
 {
-	int ret, ctrl_reg6;
-	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
+	int ret = 0, tmp;
 
-	if (data == NULL)
-		return EC_ERROR_INVAL;
+	ret = raw_read8(s->i2c_addr, LSM6DS0_WHO_AM_I_REG, &tmp);
+	if (ret)
+		return EC_ERROR_UNKNOWN;
 
-	memset(&data->accel_mutex, sizeof(struct mutex), 0);
-	data->sensor_range = 0;
-	data->sensor_datarate = 1;
-	data->accel_addr = i2c_addr;
+	if (tmp != LSM6DS0_WHO_AM_I)
+		return EC_ERROR_ACCESS_DENIED;
 
 	/*
 	 * This sensor can be powered through an EC reboot, so the state of
 	 * the sensor is unknown here. Initiate software reset to restore
 	 * sensor to default.
+	 * [6] BDU Enable Block Data Update.
+	 * [0] SW_RESET software reset
+	 *
+	 * lsm6ds0 supports both accel & gyro features
+	 * Board will see two virtual sensor devices: accel & gyro.
+	 * Requirement: Accel need be init before gyro.
+	 * SW_RESET is down for accel only!
 	 */
-	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG8, 1);
-	if (ret != EC_SUCCESS)
-		goto accel_cleanup;
+	if (SENSOR_ACCELEROMETER == s->type) {
 
-	/* Set ODR and range. */
-	ctrl_reg6 = datarates[data->sensor_datarate].reg |
-			ranges[data->sensor_range].reg;
+		mutex_lock(s->mutex);
+		ret = raw_read8(s->i2c_addr, LSM6DS0_CTRL_REG8, &tmp);
+		if (ret) {
+			mutex_unlock(s->mutex);
+			return EC_ERROR_UNKNOWN;
+		}
+		tmp |= (1 | LSM6DS0_BDU_ENABLE);
+		ret = raw_write8(s->i2c_addr, LSM6DS0_CTRL_REG8, tmp);
+		mutex_unlock(s->mutex);
+
+		if (ret)
+			return EC_ERROR_UNKNOWN;
+
+		/* Power Down Gyro */
+		ret = raw_write8(s->i2c_addr,
+			LSM6DS0_CTRL_REG1_G, 0x0);
+		if (ret)
+			return EC_ERROR_UNKNOWN;
+
+		ret = set_range(s, 2, 1);
+		if (ret)
+			return EC_ERROR_UNKNOWN;
+
+		ret = set_data_rate(s, 119000, 1);
+		if (ret)
+			return EC_ERROR_UNKNOWN;
+	}
 
-	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
+	if (SENSOR_GYRO == s->type) {
+		/* Config GYRO Range */
+		ret = set_range(s, 2000, 1);
+		if (ret)
+			return EC_ERROR_UNKNOWN;
 
-accel_cleanup:
+		/* Config ACCEL & GYRO ODR */
+		ret = set_data_rate(s, 119000, 1);
+		if (ret)
+			return EC_ERROR_UNKNOWN;
+	}
+
+	CPRINTF("[%T %s: Done Init type:0x%X]", s->name, s->type);
 	return ret;
 }
 
-const struct accelgyro_info accel_lsm6ds0 = {
-	.chip_type = CHIP_LSM6DS0,
-	.sensor_type = SENSOR_ACCELEROMETER,
-	.init = accel_init,
-	.read = accel_read,
-	.set_range = accel_set_range,
-	.get_range = accel_get_range,
-	.set_resolution = accel_set_resolution,
-	.get_resolution = accel_get_resolution,
-	.set_datarate = accel_set_datarate,
-	.get_datarate = accel_get_datarate,
+const struct accelgyro_drv lsm6ds0_drv = {
+	.init = init,
+	.read = read,
+	.set_range = set_range,
+	.get_range = get_range,
+	.set_resolution = set_resolution,
+	.get_resolution = get_resolution,
+	.set_data_rate = set_data_rate,
+	.get_data_rate = get_data_rate,
 #ifdef CONFIG_ACCEL_INTERRUPTS
-	.set_interrupt = accel_set_interrupt,
+	.set_interrupt = set_interrupt,
 #endif
 };
diff --git a/driver/accelgyro_lsm6ds0.h b/driver/accelgyro_lsm6ds0.h
index 5f1b64ca31..c71fc37767 100644
--- a/driver/accelgyro_lsm6ds0.h
+++ b/driver/accelgyro_lsm6ds0.h
@@ -17,43 +17,105 @@
 #define LSM6DS0_ADDR0             0xd4
 #define LSM6DS0_ADDR1             0xd6
 
+/* who am I  */
+#define LSM6DS0_WHO_AM_I          0x68
+
 /* Chip specific registers. */
+#define LSM6DS0_ACT_THS           0x04
+#define LSM6DS0_ACT_DUR           0x05
+#define LSM6DS0_INT_GEN_CFG_XL    0x06
+#define LSM6DS0_INT_GEN_THS_X_XL  0x07
+#define LSM6DS0_INT_GEN_THS_Y_XL  0x08
+#define LSM6DS0_INT_GEN_THS_Z_XL  0x09
+#define LSM6DS0_INT_GEN_DUR_XL    0x0a
+#define LSM6DS0_REFERENCE_G       0x0b
+#define LSM6DS0_INT_CTRL          0x0c
+#define LSM6DS0_WHO_AM_I_REG      0x0f
+#define LSM6DS0_CTRL_REG1_G       0x10
+#define LSM6DS0_CTRL_REG2_G       0x11
+#define LSM6DS0_CTRL_REG3_G       0x12
+#define LSM6DS0_ORIENT_CFG_G      0x13
+#define LSM6DS0_INT_GEN_SRC_G     0x14
+#define LSM6DS0_OUT_TEMP_L        0x15
+#define LSM6DS0_OUT_TEMP_H        0x16
+#define LSM6DS0_OUT_X_L_G         0x18
+#define LSM6DS0_OUT_X_H_G         0x19
+#define LSM6DS0_OUT_Y_L_G         0x1a
+#define LSM6DS0_OUT_Y_H_G         0x1b
+#define LSM6DS0_OUT_Z_L_G         0x1c
+#define LSM6DS0_OUT_Z_H_G         0x1d
+#define LSM6DS0_CTRL_REG4         0x1e
+#define LSM6DS0_CTRL_REG5_XL      0x1f
 #define LSM6DS0_CTRL_REG6_XL      0x20
+#define LSM6DS0_CTRL_REG7_XL      0x21
 #define LSM6DS0_CTRL_REG8         0x22
+#define LSM6DS0_CTRL_REG9         0x23
+#define LSM6DS0_CTRL_REG10        0x24
+#define LSM6DS0_INT_GEN_SRC_XL    0x26
+#define LSM6DS0_STATUS_REG        0x27
 #define LSM6DS0_OUT_X_L_XL        0x28
 #define LSM6DS0_OUT_X_H_XL        0x29
 #define LSM6DS0_OUT_Y_L_XL        0x2a
 #define LSM6DS0_OUT_Y_H_XL        0x2b
 #define LSM6DS0_OUT_Z_L_XL        0x2c
 #define LSM6DS0_OUT_Z_H_XL        0x2d
+#define LSM6DS0_FIFO_CTRL         0x2e
+#define LSM6DS0_FIFO_SRC          0x2f
+#define LSM6DS0_INT_GEN_CFG_G     0x30
+#define LSM6DS0_INT_GEN_THS_XH_G  0x31
+#define LSM6DS0_INT_GEN_THS_XL_G  0x32
+#define LSM6DS0_INT_GEN_THS_YH_G  0x33
+#define LSM6DS0_INT_GEN_THS_YL_G  0x34
+#define LSM6DS0_INT_GEN_THS_ZH_G  0x35
+#define LSM6DS0_INT_GEN_THS_ZL_G  0x36
+#define LSM6DS0_INT_GEN_DUR_G     0x37
 
-
+#define LSM6DS0_DPS_SEL_245     (0 << 3)
+#define LSM6DS0_DPS_SEL_500     (1 << 3)
+#define LSM6DS0_DPS_SEL_1000    (2 << 3)
+#define LSM6DS0_DPS_SEL_2000    (3 << 3)
 #define LSM6DS0_GSEL_2G         (0 << 3)
 #define LSM6DS0_GSEL_4G         (2 << 3)
 #define LSM6DS0_GSEL_8G         (3 << 3)
-#define LSM6DS0_GSEL_ALL        (3 << 3)
 
+#define LSM6DS0_RANGE_MASK      (3 << 3)
+
+#define LSM6DS0_ODR_PD          (0 << 5)
 #define LSM6DS0_ODR_10HZ        (1 << 5)
+#define LSM6DS0_ODR_15HZ        (1 << 5)
 #define LSM6DS0_ODR_50HZ        (2 << 5)
+#define LSM6DS0_ODR_59HZ        (2 << 5)
 #define LSM6DS0_ODR_119HZ       (3 << 5)
 #define LSM6DS0_ODR_238HZ       (4 << 5)
 #define LSM6DS0_ODR_476HZ       (5 << 5)
-#define LSM6DS0_ODR_982HZ       (6 << 5)
-#define LSM6DS0_ODR_ALL         (7 << 5)
+#define LSM6DS0_ODR_952HZ       (6 << 5)
 
-/* Sensor resolution in number of bits. This sensor has fixed resolution. */
-#define LSM6DS0_RESOLUTION      16
+#define LSM6DS0_ODR_MASK        (7 << 5)
 
-struct lsm6ds0_data {
-	struct mutex accel_mutex;
-	/* Current range of accelerometer. */
-	int sensor_range;
-	/* Current output data rate of accelerometer. */
-	int sensor_datarate;
-	/* Device address. */
-	int accel_addr;
+/*
+ * Register      : STATUS_REG
+ * Address       : 0X27
+ */
+enum lsm6ds0_status {
+	LSM6DS0_STS_DOWN                = 0x00,
+	LSM6DS0_STS_XLDA_UP             = 0x01,
+	LSM6DS0_STS_GDA_UP              = 0x02,
 };
+#define         LSM6DS0_STS_XLDA_MASK       0x01
+#define         LSM6DS0_STS_GDA_MASK        0x02
+
+/*
+ * Register      : CTRL_REG8
+ * Address       : 0X22
+ * Bit Group Name: BDU
+ */
+enum lsm6ds0_bdu {
+	LSM6DS0_BDU_DISABLE              = 0x00,
+	LSM6DS0_BDU_ENABLE               = 0x40,
+};
+/* Sensor resolution in number of bits. This sensor has fixed resolution. */
+#define LSM6DS0_RESOLUTION      16
 
-extern const struct accelgyro_info accel_lsm6ds0;
+extern const struct accelgyro_drv lsm6ds0_drv;
 
 #endif /* __CROS_EC_ACCEL_LSM6DS0_H */