Refactor accel / gyro driver to accomodate various configurations

Previously our accel / gyro drivers assumed that we had exactly two of
each identical part in the system. Some systems may have different
configurations, so allow this to be specified at the board-level.

Note that our motion_sense algorithm currently assumes that we have one
accelerometer in the lid and one in the base -- we'll need to fix that
in another CL.

BUG=chrome-os-partner:27320
TEST=Compile-only. Tested in future Samus commit.
BRANCH=None.

Change-Id: I1fae1f6c578fedebe78b473a5d66a5794ccaae00
Signed-off-by: Shawn Nematbakhsh <shawnn@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/212321
Reviewed-by: Alec Berg <alecaberg@chromium.org>

5 files changed, 207 insertions, 193 deletions

diff --git a/driver/accel_kxcj9.c b/driver/accel_kxcj9.c
index 1dc491151b..de05f85559 100644
--- a/driver/accel_kxcj9.c
+++ b/driver/accel_kxcj9.c
@@ -5,7 +5,7 @@
 
 /* KXCJ9 gsensor module for Chrome EC */
 
-#include "accelerometer.h"
+#include "accelgyro.h"
 #include "common.h"
 #include "console.h"
 #include "driver/accel_kxcj9.h"
@@ -31,20 +31,20 @@ struct accel_param_pair {
 };
 
 /* List of range values in +/-G's and their associated register values. */
-const struct accel_param_pair ranges[] = {
+static const struct accel_param_pair ranges[] = {
 	{2, KXCJ9_GSEL_2G},
 	{4, KXCJ9_GSEL_4G},
 	{8, KXCJ9_GSEL_8G_14BIT}
 };
 
 /* List of resolution values in bits and their associated register values. */
-const struct accel_param_pair resolutions[] = {
+static const struct accel_param_pair resolutions[] = {
 	{8,  KXCJ9_RES_8BIT},
 	{12, KXCJ9_RES_12BIT}
 };
 
 /* List of ODR values in mHz and their associated register values. */
-const struct accel_param_pair datarates[] = {
+static const struct accel_param_pair datarates[] = {
 	{781,     KXCJ9_OSA_0_781HZ},
 	{1563,    KXCJ9_OSA_1_563HZ},
 	{3125,    KXCJ9_OSA_3_125HZ},
@@ -59,24 +59,6 @@ const struct accel_param_pair datarates[] = {
 	{1600000, KXCJ9_OSA_1600_HZ}
 };
 
-/* Current range of each accelerometer. The value is an index into ranges[]. */
-static int sensor_range[ACCEL_COUNT] = {0, 0};
-
-/*
- * Current resolution of each accelerometer. The value is an index into
- * resolutions[].
- */
-static int sensor_resolution[ACCEL_COUNT] = {1, 1};
-
-/*
- * Current output data rate of each accelerometer. The value is an index into
- * datarates[].
- */
-static int sensor_datarate[ACCEL_COUNT] = {6, 6};
-
-
-static struct mutex accel_mutex[ACCEL_COUNT];
-
 /**
  * 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.
@@ -126,12 +108,12 @@ static int raw_write8(const int addr, const int reg, int data)
  *
  * Note: This is intended to be called in a pair with enable_sensor().
  *
- * @id Sensor index
+ * @data Pointer to motion sensor data
  * @ctrl1 Pointer to location to store KXCJ9_CTRL1 register after disabling
  *
  * @return EC_SUCCESS if successful, EC_ERROR_* otherwise
  */
-static int disable_sensor(const enum accel_id id, int *ctrl1)
+static int disable_sensor(struct kxcj9_data *data, int *ctrl1)
 {
 	int ret;
 
@@ -139,7 +121,7 @@ static int disable_sensor(const enum accel_id id, int *ctrl1)
 	 * Read the current state of the ctrl1 register so that we can restore
 	 * it later.
 	 */
-	ret = raw_read8(accel_addr[id], KXCJ9_CTRL1, ctrl1);
+	ret = raw_read8(data->accel_addr, KXCJ9_CTRL1, ctrl1);
 	if (ret != EC_SUCCESS)
 		return ret;
 
@@ -147,13 +129,13 @@ static int disable_sensor(const enum accel_id id, int *ctrl1)
 	 * Before disabling the sensor, acquire mutex to prevent another task
 	 * from attempting to access accel parameters until we enable sensor.
 	 */
-	mutex_lock(&accel_mutex[id]);
+	mutex_lock(&data->accel_mutex);
 
 	/* Disable sensor. */
 	*ctrl1 &= ~KXCJ9_CTRL1_PC1;
-	ret = raw_write8(accel_addr[id], KXCJ9_CTRL1, *ctrl1);
+	ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, *ctrl1);
 	if (ret != EC_SUCCESS) {
-		mutex_unlock(&accel_mutex[id]);
+		mutex_unlock(&data->accel_mutex);
 		return ret;
 	}
 
@@ -165,178 +147,166 @@ static int disable_sensor(const enum accel_id id, int *ctrl1)
  *
  * Note: This is intended to be called in a pair with disable_sensor().
  *
- * @id Sensor index
+ * @data Pointer to motion sensor data
  * @ctrl1 Value of KXCJ9_CTRL1 register to write to sensor
  *
  * @return EC_SUCCESS if successful, EC_ERROR_* otherwise
  */
-static int enable_sensor(const enum accel_id id, const int ctrl1)
+static int enable_sensor(struct kxcj9_data *data, const int ctrl1)
 {
 	int i, ret;
 
 	for (i = 0; i < SENSOR_ENABLE_ATTEMPTS; i++) {
 		/* Enable accelerometer based on ctrl1 value. */
-		ret = raw_write8(accel_addr[id], KXCJ9_CTRL1,
+		ret = raw_write8(data->accel_addr, KXCJ9_CTRL1,
 				ctrl1 | KXCJ9_CTRL1_PC1);
 
 		/* On first success, we are done. */
 		if (ret == EC_SUCCESS) {
-			mutex_unlock(&accel_mutex[id]);
+			mutex_unlock(&data->accel_mutex);
 			return EC_SUCCESS;
 		}
-
 	}
 
 	/* Release mutex. */
-	mutex_unlock(&accel_mutex[id]);
+	mutex_unlock(&data->accel_mutex);
 
 	/* Cannot enable accel, print warning and return an error. */
-	CPRINTF("Error trying to enable accelerometer %d\n", id);
+	CPRINTF("Error trying to enable accelerometer\n");
 
 	return ret;
 }
 
-int accel_set_range(const enum accel_id id, const int range, const int rnd)
+static int accel_set_range(void *drv_data,
+			   const int range,
+			   const int rnd)
 {
 	int ret, ctrl1, ctrl1_new, index;
-
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
+	struct kxcj9_data *data = (struct kxcj9_data *)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(id, &ctrl1);
+	ret = disable_sensor(data, &ctrl1);
 	if (ret != EC_SUCCESS)
 		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(accel_addr[id],  KXCJ9_CTRL1, ctrl1_new);
+	ret = raw_write8(data->accel_addr,  KXCJ9_CTRL1, ctrl1_new);
 
 	/* If successfully written, then save the range. */
 	if (ret == EC_SUCCESS) {
-		sensor_range[id] = index;
+		data->sensor_range = index;
 		ctrl1 = ctrl1_new;
 	}
 
 	/* Re-enable the sensor. */
-	if (enable_sensor(id, ctrl1) != EC_SUCCESS)
+	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
 		return EC_ERROR_UNKNOWN;
 
 	return ret;
 }
 
-int accel_get_range(const enum accel_id id, int * const range)
+static int accel_get_range(void *drv_data, int * const range)
 {
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
-
-	*range = ranges[sensor_range[id]].val;
+	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	*range = ranges[data->sensor_range].val;
 	return EC_SUCCESS;
 }
 
-int accel_set_resolution(const enum accel_id id, const int res, const int rnd)
+static int accel_set_resolution(void *drv_data,
+				const int res,
+				const int rnd)
 {
 	int ret, ctrl1, ctrl1_new, index;
-
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
+	struct kxcj9_data *data = (struct kxcj9_data *)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(id, &ctrl1);
+	ret = disable_sensor(data, &ctrl1);
 	if (ret != EC_SUCCESS)
 		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(accel_addr[id],  KXCJ9_CTRL1, ctrl1_new);
+	ret = raw_write8(data->accel_addr,  KXCJ9_CTRL1, ctrl1_new);
 
 	/* If successfully written, then save the range. */
 	if (ret == EC_SUCCESS) {
-		sensor_resolution[id] = index;
+		data->sensor_resolution = index;
 		ctrl1 = ctrl1_new;
 	}
 
 	/* Re-enable the sensor. */
-	if (enable_sensor(id, ctrl1) != EC_SUCCESS)
+	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
 		return EC_ERROR_UNKNOWN;
 
 	return ret;
 }
 
-int accel_get_resolution(const enum accel_id id, int * const res)
+static int accel_get_resolution(void *drv_data, int * const res)
 {
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
-
-	*res = resolutions[sensor_resolution[id]].val;
+	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	*res = resolutions[data->sensor_resolution].val;
 	return EC_SUCCESS;
 }
 
-int accel_set_datarate(const enum accel_id id, const int rate, const int rnd)
+static int accel_set_datarate(void *drv_data,
+			      const int rate,
+			      const int rnd)
 {
 	int ret, ctrl1, index;
-
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
+	struct kxcj9_data *data = (struct kxcj9_data *)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(id, &ctrl1);
+	ret = disable_sensor(data, &ctrl1);
 	if (ret != EC_SUCCESS)
 		return ret;
 
 	/* Set output data rate. */
-	ret = raw_write8(accel_addr[id],  KXCJ9_DATA_CTRL,
+	ret = raw_write8(data->accel_addr,  KXCJ9_DATA_CTRL,
 			datarates[index].reg);
 
 	/* If successfully written, then save the range. */
 	if (ret == EC_SUCCESS)
-		sensor_datarate[id] = index;
+		data->sensor_datarate = index;
 
 	/* Re-enable the sensor. */
-	if (enable_sensor(id, ctrl1) != EC_SUCCESS)
+	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
 		return EC_ERROR_UNKNOWN;
 
 	return ret;
 }
 
-int accel_get_datarate(const enum accel_id id, int * const rate)
+static int accel_get_datarate(void *drv_data, int * const rate)
 {
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
-
-	*rate = datarates[sensor_datarate[id]].val;
+	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
+	*rate = datarates[data->sensor_datarate].val;
 	return EC_SUCCESS;
 }
 
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
-int accel_set_interrupt(const enum accel_id id, unsigned int threshold)
+static int accel_set_interrupt(void *drv_data, unsigned int threshold)
 {
 	int ctrl1, tmp, ret;
+	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
 
 	/* Disable the sensor to allow for changing of critical parameters. */
-	ret = disable_sensor(id, &ctrl1);
+	ret = disable_sensor(data, &ctrl1);
 	if (ret != EC_SUCCESS)
 		return ret;
 
 	/* Set interrupt timer to 1 so it wakes up immediately. */
-	ret = raw_write8(accel_addr[id], KXCJ9_WAKEUP_TIMER, 1);
+	ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_TIMER, 1);
 	if (ret != EC_SUCCESS)
 		goto error_enable_sensor;
 
@@ -345,7 +315,7 @@ int accel_set_interrupt(const enum accel_id id, unsigned int threshold)
 	 * first we need to divide by 16 to get the value to send.
 	 */
 	threshold >>= 4;
-	ret = raw_write8(accel_addr[id], KXCJ9_WAKEUP_THRESHOLD, threshold);
+	ret = raw_write8(data->accel_addr, KXCJ9_WAKEUP_THRESHOLD, threshold);
 	if (ret != EC_SUCCESS)
 		goto error_enable_sensor;
 
@@ -354,11 +324,11 @@ int accel_set_interrupt(const enum accel_id id, 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(accel_addr[id], KXCJ9_INT_CTRL1, &tmp);
+	ret = raw_read8(data->accel_addr, KXCJ9_INT_CTRL1, &tmp);
 	if (ret != EC_SUCCESS)
 		goto error_enable_sensor;
 	if (!(tmp & KXCJ9_INT_CTRL1_IEN)) {
-		ret = raw_write8(accel_addr[id], KXCJ9_INT_CTRL1,
+		ret = raw_write8(data->accel_addr, KXCJ9_INT_CTRL1,
 				tmp | KXCJ9_INT_CTRL1_IEN);
 		if (ret != EC_SUCCESS)
 			goto error_enable_sensor;
@@ -369,41 +339,40 @@ int accel_set_interrupt(const enum accel_id id, 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(accel_addr[id], KXCJ9_INT_REL, &tmp);
+	ret = raw_read8(data->accel_addr, KXCJ9_INT_REL, &tmp);
 
 error_enable_sensor:
 	/* Re-enable the sensor. */
-	if (enable_sensor(id, ctrl1) != EC_SUCCESS)
+	if (enable_sensor(data, ctrl1) != EC_SUCCESS)
 		return EC_ERROR_UNKNOWN;
 
 	return ret;
 }
 #endif
 
-int accel_read(const enum accel_id id, int * const x_acc, int * const y_acc,
-		int * const z_acc)
+static int accel_read(void *drv_data,
+		      int * const x_acc,
+		      int * const y_acc,
+		      int * const z_acc)
 {
 	uint8_t acc[6];
 	uint8_t reg = KXCJ9_XOUT_L;
 	int ret, multiplier;
-
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
+	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
 
 	/* Read 6 bytes starting at KXCJ9_XOUT_L. */
-	mutex_lock(&accel_mutex[id]);
+	mutex_lock(&data->accel_mutex);
 	i2c_lock(I2C_PORT_ACCEL, 1);
-	ret = i2c_xfer(I2C_PORT_ACCEL, accel_addr[id], &reg, 1, acc, 6,
+	ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, &reg, 1, acc, 6,
 			I2C_XFER_SINGLE);
 	i2c_lock(I2C_PORT_ACCEL, 0);
-	mutex_unlock(&accel_mutex[id]);
+	mutex_unlock(&data->accel_mutex);
 
 	if (ret != EC_SUCCESS)
 		return ret;
 
 	/* Determine multiplier based on stored range. */
-	switch (ranges[sensor_range[id]].reg) {
+	switch (ranges[data->sensor_range].reg) {
 	case KXCJ9_GSEL_2G:
 		multiplier = 1;
 		break;
@@ -436,17 +405,23 @@ int accel_read(const enum accel_id id, int * const x_acc, int * const y_acc,
 	return EC_SUCCESS;
 }
 
-int accel_init(const enum accel_id id)
+static int accel_init(void *drv_data, int i2c_addr)
 {
 	int ret = EC_SUCCESS;
 	int cnt = 0, ctrl1, ctrl2;
+	struct kxcj9_data *data = (struct kxcj9_data *)drv_data;
 
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
+	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;
+
 	/* Disable the sensor to allow for changing of critical parameters. */
-	ret = disable_sensor(id, &ctrl1);
+	ret = disable_sensor(data, &ctrl1);
 	if (ret != EC_SUCCESS)
 		return ret;
 
@@ -455,13 +430,13 @@ int accel_init(const enum accel_id id)
 	 * the sensor is unknown here. Initiate software reset to restore
 	 * sensor to default.
 	 */
-	ret = raw_write8(accel_addr[id], KXCJ9_CTRL2, KXCJ9_CTRL2_SRST);
+	ret = raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_CTRL2_SRST);
 	if (ret != EC_SUCCESS)
 		return ret;
 
 	/* Wait until software reset is complete or timeout. */
 	while (1) {
-		ret = raw_read8(accel_addr[id], KXCJ9_CTRL2, &ctrl2);
+		ret = raw_read8(data->accel_addr, KXCJ9_CTRL2, &ctrl2);
 
 		/* Reset complete. */
 		if (ret == EC_SUCCESS && !(ctrl2 & KXCJ9_CTRL2_SRST))
@@ -476,23 +451,25 @@ int accel_init(const enum accel_id id)
 	}
 
 	/* Set resolution and range. */
-	ctrl1 = resolutions[sensor_resolution[id]].reg |
-			ranges[sensor_range[id]].reg;
+	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(accel_addr[id], KXCJ9_CTRL1, ctrl1);
+	ret = raw_write8(data->accel_addr, KXCJ9_CTRL1, ctrl1);
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	/* Set interrupt polarity to rising edge and keep interrupt disabled. */
-	ret |= raw_write8(accel_addr[id], KXCJ9_INT_CTRL1, KXCJ9_INT_CTRL1_IEA);
+	ret |= raw_write8(data->accel_addr,
+			  KXCJ9_INT_CTRL1,
+			  KXCJ9_INT_CTRL1_IEA);
 
 	/* Set output data rate for wake-up interrupt function. */
-	ret |= raw_write8(accel_addr[id], KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ);
+	ret |= raw_write8(data->accel_addr, KXCJ9_CTRL2, KXCJ9_OWUF_100_0HZ);
 
 	/* Set interrupt to trigger on motion on any axis. */
-	ret |= raw_write8(accel_addr[id], KXCJ9_INT_CTRL2,
+	ret |= raw_write8(data->accel_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);
@@ -506,11 +483,27 @@ int accel_init(const enum accel_id id)
 #endif
 
 	/* Set output data rate. */
-	ret |= raw_write8(accel_addr[id], KXCJ9_DATA_CTRL,
-			datarates[sensor_datarate[id]].reg);
+	ret |= raw_write8(data->accel_addr, KXCJ9_DATA_CTRL,
+			datarates[data->sensor_datarate].reg);
 
 	/* Enable the sensor. */
-	ret |= enable_sensor(id, ctrl1);
+	ret |= enable_sensor(data, ctrl1);
 
 	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,
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.set_interrupt = accel_set_interrupt,
+#endif
+};
diff --git a/driver/accel_kxcj9.h b/driver/accel_kxcj9.h
index ea15b6ec17..30bea0e737 100644
--- a/driver/accel_kxcj9.h
+++ b/driver/accel_kxcj9.h
@@ -8,6 +8,8 @@
 #ifndef __CROS_EC_ACCEL_KXCJ9_H
 #define __CROS_EC_ACCEL_KXCJ9_H
 
+#include "task.h"
+
 /*
  * 7-bit address is 000111Xb. Where 'X' is determined
  * by the voltage on the ADDR pin.
@@ -99,17 +101,18 @@
 #define KXCJ9_OSA_800_0HZ	6
 #define KXCJ9_OSA_1600_HZ	7
 
-
-#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 id Target accelerometer
- * @param threshold Threshold for interrupt in units of counts.
- */
-int accel_set_interrupt(const enum accel_id id, unsigned int threshold);
-#endif
+struct kxcj9_data {
+	struct mutex accel_mutex;
+	/* Current range of accelerometer. */
+	int sensor_range;
+	/* Current output data rate of accelerometer. */
+	int sensor_datarate;
+	/* Current resolution of accelerometer. */
+	int sensor_resolution;
+	/* Device address. */
+	int accel_addr;
+};
+
+extern const struct accelgyro_info accel_kxcj9;
 
 #endif /* __CROS_EC_ACCEL_KXCJ9_H */
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c
index 8c8bd76b49..313ec9a312 100644
--- a/driver/accelgyro_lsm6ds0.c
+++ b/driver/accelgyro_lsm6ds0.c
@@ -5,7 +5,7 @@
 
 /* LSM6DS0 accelerometer and gyro module for Chrome EC */
 
-#include "accelerometer.h"
+#include "accelgyro.h"
 #include "common.h"
 #include "console.h"
 #include "driver/accelgyro_lsm6ds0.h"
@@ -24,14 +24,14 @@ struct accel_param_pair {
 };
 
 /* List of range values in +/-G's and their associated register values. */
-const struct accel_param_pair ranges[] = {
+static const struct accel_param_pair ranges[] = {
 	{2, LSM6DS0_GSEL_2G},
 	{4, LSM6DS0_GSEL_4G},
 	{8, LSM6DS0_GSEL_8G}
 };
 
 /* List of ODR values in mHz and their associated register values. */
-const struct accel_param_pair datarates[] = {
+static const struct accel_param_pair datarates[] = {
 	{10000,    LSM6DS0_ODR_10HZ},
 	{50000,    LSM6DS0_ODR_50HZ},
 	{119000,   LSM6DS0_ODR_119HZ},
@@ -40,17 +40,6 @@ const struct accel_param_pair datarates[] = {
 	{952000,   LSM6DS0_ODR_982HZ}
 };
 
-/* Current range of each accelerometer. The value is an index into ranges[]. */
-static int sensor_range[ACCEL_COUNT] = {0, 0};
-
-/*
- * Current output data rate of each accelerometer. The value is an index into
- * datarates[].
- */
-static int sensor_datarate[ACCEL_COUNT] = {1, 1};
-
-static struct mutex accel_mutex[ACCEL_COUNT];
-
 /**
  * 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.
@@ -94,9 +83,12 @@ static int raw_write8(const int addr, const int reg, int data)
 	return i2c_write8(I2C_PORT_ACCEL, addr, reg, data);
 }
 
-int accel_set_range(const enum accel_id id, const int range, const int rnd)
+static int accel_set_range(void *drv_data,
+			   const int range,
+			   const int rnd)
 {
 	int ret, index, ctrl_reg6;
+	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
 
 	/* Find index for interface pair matching the specified range. */
 	index = find_param_index(range, rnd, ranges, ARRAY_SIZE(ranges));
@@ -105,61 +97,52 @@ int accel_set_range(const enum accel_id id, const int range, const int rnd)
 	 * Lock accel resource to prevent another task from attempting
 	 * to write accel parameters until we are done.
 	 */
-	mutex_lock(&accel_mutex[id]);
+	mutex_lock(&data->accel_mutex);
 
-	ret = raw_read8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
+	ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
 	if (ret != EC_SUCCESS)
 		goto accel_cleanup;
 
 	ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_GSEL_ALL) | ranges[index].reg;
-	ret = raw_write8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
+	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
 
 accel_cleanup:
 	/* Unlock accel resource and save new range if written successfully. */
-	mutex_unlock(&accel_mutex[id]);
+	mutex_unlock(&data->accel_mutex);
 	if (ret == EC_SUCCESS)
-		sensor_range[id] = index;
+		data->sensor_range = index;
 
 	return EC_SUCCESS;
 }
 
-int accel_get_range(const enum accel_id id, int * const range)
+static int accel_get_range(void *drv_data, int * const range)
 {
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
-
-	*range = ranges[sensor_range[id]].val;
+	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
+	*range = ranges[data->sensor_range].val;
 	return EC_SUCCESS;
 }
 
-int accel_set_resolution(const enum accel_id id, const int res, const int rnd)
+static int accel_set_resolution(void *drv_data,
+				const int res,
+				const int rnd)
 {
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
-
 	/* Only one resolution, LSM6DS0_RESOLUTION, so nothing to do. */
 	return EC_SUCCESS;
 }
 
-int accel_get_resolution(const enum accel_id id, int * const res)
+static int accel_get_resolution(void *drv_data,
+				int * const res)
 {
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
-
 	*res = LSM6DS0_RESOLUTION;
 	return EC_SUCCESS;
 }
 
-int accel_set_datarate(const enum accel_id id, const int rate, const int rnd)
+static int accel_set_datarate(void *drv_data,
+			      const int rate,
+			      const int rnd)
 {
 	int ret, index, ctrl_reg6;
-
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
+	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
 
 	/* Find index for interface pair matching the specified range. */
 	index = find_param_index(rate, rnd, datarates, ARRAY_SIZE(datarates));
@@ -168,62 +151,64 @@ int accel_set_datarate(const enum accel_id id, const int rate, const int rnd)
 	 * Lock accel resource to prevent another task from attempting
 	 * to write accel parameters until we are done.
 	 */
-	mutex_lock(&accel_mutex[id]);
+	mutex_lock(&data->accel_mutex);
 
-	ret = raw_read8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
+	ret = raw_read8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, &ctrl_reg6);
 	if (ret != EC_SUCCESS)
 		goto accel_cleanup;
 
 	ctrl_reg6 = (ctrl_reg6 & ~LSM6DS0_ODR_ALL) | datarates[index].reg;
-	ret = raw_write8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
+	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
 
 accel_cleanup:
 	/* Unlock accel resource and save new ODR if written successfully. */
-	mutex_unlock(&accel_mutex[id]);
+	mutex_unlock(&data->accel_mutex);
 	if (ret == EC_SUCCESS)
-		sensor_datarate[id] = index;
+		data->sensor_datarate = index;
 
 	return EC_SUCCESS;
 }
 
-int accel_get_datarate(const enum accel_id id, int * const rate)
+static int accel_get_datarate(void *drv_data,
+			      int * const rate)
 {
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
-		return EC_ERROR_INVAL;
-
-	*rate = datarates[sensor_datarate[id]].val;
+	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
+	*rate = datarates[data->sensor_datarate].val;
 	return EC_SUCCESS;
 }
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
-int accel_set_interrupt(const enum accel_id id, unsigned int threshold)
+static int accel_set_interrupt(void *drv_data,
+			       unsigned int threshold)
 {
 	/* Currently unsupported. */
 	return EC_ERROR_UNKNOWN;
 }
 #endif
 
-int accel_read(const enum accel_id id, int * const x_acc, int * const y_acc,
-		int * const z_acc)
+static int accel_read(void *drv_data,
+		      int * const x_acc,
+		      int * const y_acc,
+		      int * const z_acc)
 {
 	uint8_t acc[6];
 	uint8_t reg = LSM6DS0_OUT_X_L_XL;
 	int ret, multiplier;
+	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
 
 	/* Read 6 bytes starting at LSM6DS0_OUT_X_L_XL. */
-	mutex_lock(&accel_mutex[id]);
+	mutex_lock(&data->accel_mutex);
 	i2c_lock(I2C_PORT_ACCEL, 1);
-	ret = i2c_xfer(I2C_PORT_ACCEL, accel_addr[id], &reg, 1, acc, 6,
+	ret = i2c_xfer(I2C_PORT_ACCEL, data->accel_addr, &reg, 1, acc, 6,
 			I2C_XFER_SINGLE);
 	i2c_lock(I2C_PORT_ACCEL, 0);
-	mutex_unlock(&accel_mutex[id]);
+	mutex_unlock(&data->accel_mutex);
 
 	if (ret != EC_SUCCESS)
 		return ret;
 
 	/* Determine multiplier based on stored range. */
-	switch (ranges[sensor_range[id]].reg) {
+	switch (ranges[data->sensor_range].reg) {
 	case LSM6DS0_GSEL_2G:
 		multiplier = 1;
 		break;
@@ -254,32 +239,50 @@ int accel_read(const enum accel_id id, int * const x_acc, int * const y_acc,
 	return EC_SUCCESS;
 }
 
-int accel_init(const enum accel_id id)
+static int accel_init(void *drv_data, int i2c_addr)
 {
 	int ret, ctrl_reg6;
+	struct lsm6ds0_data *data = (struct lsm6ds0_data *)drv_data;
 
-	/* Check for valid id. */
-	if (id < 0 || id >= ACCEL_COUNT)
+	if (data == NULL)
 		return EC_ERROR_INVAL;
 
-	mutex_lock(&accel_mutex[id]);
+	memset(&data->accel_mutex, sizeof(struct mutex), 0);
+	data->sensor_range = 0;
+	data->sensor_datarate = 1;
+	data->accel_addr = i2c_addr;
 
 	/*
 	 * 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(accel_addr[id], LSM6DS0_CTRL_REG8, 1);
+	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG8, 1);
 	if (ret != EC_SUCCESS)
 		goto accel_cleanup;
 
 	/* Set ODR and range. */
-	ctrl_reg6 = datarates[sensor_datarate[id]].reg |
-			ranges[sensor_range[id]].reg;
+	ctrl_reg6 = datarates[data->sensor_datarate].reg |
+			ranges[data->sensor_range].reg;
 
-	ret = raw_write8(accel_addr[id], LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
+	ret = raw_write8(data->accel_addr, LSM6DS0_CTRL_REG6_XL, ctrl_reg6);
 
 accel_cleanup:
-	mutex_unlock(&accel_mutex[id]);
 	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,
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.set_interrupt = accel_set_interrupt,
+#endif
+};
diff --git a/driver/accelgyro_lsm6ds0.h b/driver/accelgyro_lsm6ds0.h
index 6c34570b15..5f1b64ca31 100644
--- a/driver/accelgyro_lsm6ds0.h
+++ b/driver/accelgyro_lsm6ds0.h
@@ -8,6 +8,8 @@
 #ifndef __CROS_EC_ACCEL_LSM6DS0_H
 #define __CROS_EC_ACCEL_LSM6DS0_H
 
+#include "task.h"
+
 /*
  * 7-bit address is 110101Xb. Where 'X' is determined
  * by the voltage on the ADDR pin.
@@ -42,4 +44,16 @@
 /* Sensor resolution in number of bits. This sensor has fixed resolution. */
 #define LSM6DS0_RESOLUTION      16
 
+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;
+};
+
+extern const struct accelgyro_info accel_lsm6ds0;
+
 #endif /* __CROS_EC_ACCEL_LSM6DS0_H */
diff --git a/driver/build.mk b/driver/build.mk
index d3872acf6e..a18deb2657 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -8,6 +8,7 @@
 
 # Accelerometers
 driver-$(CONFIG_ACCEL_KXCJ9)+=accel_kxcj9.o
+driver-$(CONFIG_ACCELGYRO_LSM6DS0)+=accelgyro_lsm6ds0.o
 
 # ALS drivers
 driver-$(CONFIG_ALS_ISL29035)+=als_isl29035.o