driver: accel: Add acc driver basics and FIFO for LIS2DH/LIS2DH12

    Add driver for acc sensor ST lis2dh/lis2dh12
    Support interrupt management for FIFO watermark
    Starting to share common code with other devices
    like lsm6dsm/lsm6dsl (acc/gyro) or new lis2mdl (mag)
    TODO: Add all embedded functions support (click,
    tap and so on)

    BUG=none
    BRANCH=master
    TEST=Tested on discovery BOARD with sensor connected on
    EC i2c master bus. Added motion sense task on discovery
    board task list, added gpio info in board configuration
    file and tested with motion sense console commands. Data
    for acc seems ok: can successfully change ODR and
    full scale range. Also FIFO and interrupt tested
    Device tested is lis2dh (lis2dh12 simply differs for low
    pin count but share the same registers)

Change-Id: I16abeac3f139a604094b38d8d8b857a62c93a242
Signed-off-by: Mario Tesi <mario.tesi@st.com>
Reviewed-on: https://chromium-review.googlesource.com/412700
Commit-Ready: mario tesi <mario.tesi@st.com>
Tested-by: mario tesi <mario.tesi@st.com>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

5 files changed, 884 insertions, 0 deletions

diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c
new file mode 100644
index 0000000000..fbc91c270c
--- /dev/null
+++ b/driver/accel_lis2dh.c
@@ -0,0 +1,413 @@
+/* Copyright (c) 2016 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/**
+ * LIS2DH/LIS2DH12 accelerometer module for Chrome EC 3D digital accelerometer
+ */
+
+#include "accelgyro.h"
+#include "common.h"
+#include "console.h"
+#include "hooks.h"
+#include "i2c.h"
+#include "math_util.h"
+#include "task.h"
+#include "util.h"
+#include "driver/stm_mems_common.h"
+
+/* Internal data structure */
+struct stprivate_data lis2dh_a_data;
+#ifdef CONFIG_ACCEL_FIFO
+static uint8_t fifo[FIFO_READ_LEN];
+#endif /* CONFIG_ACCEL_FIFO */
+
+#ifdef CONFIG_ACCEL_FIFO
+/**
+ * enable_fifo - Enable/Disable FIFO in LIS2DH
+ * @s: Motion sensor pointer
+ * @mode: fifo_modes
+ * @en_dis: LIS2DH_EN_BIT/LIS2DH_DIS_BIT
+ */
+static int enable_fifo(const struct motion_sensor_t *s, int mode, int en_dis)
+{
+	int ret;
+
+	ret = write_data_with_mask(s, LIS2DH_FIFO_CTRL_REG, LIS2DH_FIFO_MODE_MASK,
+				   mode);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = write_data_with_mask(s, LIS2DH_CTRL5_ADDR, LIS2DH_FIFO_EN_MASK,
+				   en_dis);
+
+	return ret;
+}
+#endif /* CONFIG_ACCEL_FIFO */
+
+/**
+ * set_range - set full scale range
+ * @s: Motion sensor pointer
+ * @range: Range
+ * @rnd: Round up/down flag
+ */
+static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+{
+	int err, normalized_rate;
+	struct stprivate_data *data = s->drv_data;
+	int val;
+
+	val = LIS2DH_FS_TO_REG(range);
+	normalized_rate = LIS2DH_FS_TO_NORMALIZE(range);
+
+	if (rnd && (range < normalized_rate))
+		val++;
+
+	/* Adjust rounded values */
+	if (val > LIS2DH_FS_16G_VAL) {
+		val = LIS2DH_FS_16G_VAL;
+		normalized_rate = 16;
+	}
+
+	if (val < LIS2DH_FS_2G_VAL) {
+		val = LIS2DH_FS_2G_VAL;
+		normalized_rate = 2;
+	}
+
+	/* Lock accel resource to prevent another task from attempting
+	 * to write accel parameters until we are done */
+	mutex_lock(s->mutex);
+	err = write_data_with_mask(s, LIS2DH_CTRL4_ADDR, LIS2DH_FS_MASK, val);
+
+	/* Save Gain in range for speed up data path */
+	if (err == EC_SUCCESS)
+		data->base.range = LIS2DH_FS_TO_GAIN(normalized_rate);
+
+	mutex_unlock(s->mutex);
+	return EC_SUCCESS;
+}
+
+static int get_range(const struct motion_sensor_t *s)
+{
+	struct stprivate_data *data = s->drv_data;
+
+	return LIS2DH_GAIN_TO_FS(data->base.range);
+}
+
+static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
+{
+	int ret, normalized_rate;
+	struct stprivate_data *data = s->drv_data;
+	uint8_t reg_val;
+
+	mutex_lock(s->mutex);
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* FIFO stop collecting events. Restart FIFO in Bypass mode */
+	ret = enable_fifo(s, LIS2DH_FIFO_BYPASS_MODE, LIS2DH_DIS_BIT);
+	if (ret != EC_SUCCESS)
+		goto unlock_rate;
+#endif /* CONFIG_ACCEL_FIFO */
+
+	if (rate == ODR_POWER_OFF_VAL) {
+		/* Power Off device */
+		ret = write_data_with_mask(s, LIS2DH_CTRL1_ADDR,
+					   LIS2DH_ACC_ODR_MASK, ODR_POWER_OFF_VAL);
+		goto unlock_rate;
+	}
+
+	reg_val = LIS2DH_ODR_TO_REG(rate);
+	normalized_rate = LIS2DH_ODR_TO_NORMALIZE(rate);
+
+	if (rnd && (normalized_rate < rate)) {
+		reg_val++;
+		normalized_rate = LIS2DH_REG_TO_NORMALIZE(reg_val);
+	}
+
+	/* Adjust rounded value */
+	if (reg_val > LIS2DH_ODR_400HZ_VAL) {
+		reg_val = LIS2DH_ODR_400HZ_VAL;
+		normalized_rate = 400000;
+	} else if (reg_val < LIS2DH_ODR_1HZ_VAL) {
+		reg_val = LIS2DH_ODR_1HZ_VAL;
+		normalized_rate = 1000;
+	}
+
+	/*
+	 * Lock accel resource to prevent another task from attempting
+	 * to write accel parameters until we are done
+	 */
+	ret = write_data_with_mask(s, LIS2DH_CTRL1_ADDR, LIS2DH_ACC_ODR_MASK,
+				   reg_val);
+	if (ret == EC_SUCCESS)
+		data->base.odr = normalized_rate;
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* FIFO restart collecting events */
+	ret = enable_fifo(s, LIS2DH_FIFO_STREAM_MODE, LIS2DH_EN_BIT);
+#endif /* CONFIG_ACCEL_FIFO */
+
+unlock_rate:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+#ifdef CONFIG_ACCEL_FIFO
+/*
+ * Load data from internal sensor FIFO (deep 32 byte)
+ */
+static int load_fifo(struct motion_sensor_t *s)
+{
+	int ret, tmp, nsamples, i;
+	struct ec_response_motion_sensor_data vect;
+	int done = 0;
+	int *axis = s->raw_xyz;
+
+	/* Try to Empty FIFO */
+	do {
+		/* Read samples number in status register */
+		ret = raw_read8(s->port, s->addr, LIS2DH_FIFO_SRC_REG, &tmp);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/* Check FIFO empty flag */
+		if (tmp & LIS2DH_FIFO_EMPTY_FLAG)
+			return EC_SUCCESS;
+
+		nsamples = (tmp & LIS2DH_FIFO_UNREAD_MASK) * OUT_XYZ_SIZE;
+
+		/* Limit FIFO read data to burst of FIFO_READ_LEN size because
+		 * read operatios in under i2c mutex lock */
+		if (nsamples > FIFO_READ_LEN)
+			nsamples = FIFO_READ_LEN;
+		else
+			done = 1;
+
+		ret = raw_read_n(s->port, s->addr, LIS2DH_OUT_X_L_ADDR, fifo,
+				 nsamples, AUTO_INC);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		for (i = 0; i < nsamples; i += OUT_XYZ_SIZE) {
+			/* Apply precision, sensitivity and rotation vector */
+			normalize(s, axis, &fifo[i]);
+
+			/* Fill vector array */
+			vect.data[0] = axis[0];
+			vect.data[1] = axis[1];
+			vect.data[2] = axis[2];
+			vect.flags = 0;
+			vect.sensor_num = 0;
+			motion_sense_fifo_add_unit(&vect, s, 3);
+		}
+	} while(!done);
+
+	return EC_SUCCESS;
+}
+#endif  /* CONFIG_ACCEL_FIFO */
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+static int config_interrupt(const struct motion_sensor_t *s)
+{
+	int ret;
+
+#ifdef CONFIG_ACCEL_FIFO_THRES
+	/* configure FIFO watermark level */
+	ret = write_data_with_mask(s, LIS2DH_FIFO_CTRL_REG,
+				   LIS2DH_FIFO_THR_MASK, CONFIG_ACCEL_FIFO_THRES);
+	if (ret != EC_SUCCESS)
+		return ret;
+	/* enable interrupt on FIFO watermask and route to int1 */
+	ret = write_data_with_mask(s, LIS2DH_CTRL3_ADDR,
+				   LIS2DH_FIFO_WTM_INT_MASK, 1);
+#endif /* CONFIG_ACCEL_FIFO */
+
+	return ret;
+}
+
+/**
+ * lis2dh_interrupt - interrupt from int1/2 pin of sensor
+ */
+void lis2dh_interrupt(enum gpio_signal signal)
+{
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCEL_LIS2DH_INT_EVENT, 0);
+}
+
+/**
+ * irq_handler - bottom half of the interrupt stack.
+ */
+static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	int interrupt;
+
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+	    (!(*event & CONFIG_ACCEL_LIS2DH_INT_EVENT))) {
+		return EC_ERROR_NOT_HANDLED;
+	}
+
+	/* read interrupt status register to reset source */
+	raw_read8(s->port, s->addr, LIS2DH_INT1_SRC_REG, &interrupt);
+
+#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
+	*event |= CONFIG_GESTURE_TAP_EVENT;
+#endif
+#ifdef CONFIG_GESTURE_SIGMO
+	*event |= CONFIG_GESTURE_SIGMO_EVENT;
+#endif
+	/*
+	 * No need to read the FIFO here, motion sense task is
+	 * doing it on every interrupt.
+	 */
+	return EC_SUCCESS;
+}
+#endif  /* CONFIG_ACCEL_INTERRUPTS */
+
+static int is_data_ready(const struct motion_sensor_t *s, int *ready)
+{
+	int ret, tmp;
+
+	ret = raw_read8(s->port, s->addr, LIS2DH_STATUS_REG, &tmp);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("[%T %s type:0x%X RS Error]", s->name, s->type);
+		return ret;
+	}
+
+	*ready = (LIS2DH_STS_XLDA_UP == (tmp & LIS2DH_STS_XLDA_UP));
+
+	return EC_SUCCESS;
+}
+
+static int read(const struct motion_sensor_t *s, vector_3_t v)
+{
+	uint8_t raw[OUT_XYZ_SIZE];
+	int ret, i, tmp = 0;
+	struct stprivate_data *data = s->drv_data;
+
+	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) {
+		if (v != s->raw_xyz)
+			memcpy(v, s->raw_xyz, sizeof(s->raw_xyz));
+		return EC_SUCCESS;
+	}
+
+	/* Read output data bytes starting at LIS2DH_OUT_X_L_ADDR */
+	ret = raw_read_n(s->port, s->addr, LIS2DH_OUT_X_L_ADDR, raw,
+			 OUT_XYZ_SIZE, AUTO_INC);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("[%T %s type:0x%X RD XYZ Error]",
+			s->name, s->type);
+		return ret;
+	}
+
+	/* Transform from LSB to real data with rotation and gain */
+	normalize(s, v, raw);
+
+	/* apply offset in the device coordinates */
+	for (i = X; i <= Z; i++)
+		v[i] += (data->offset[i] << 5) / data->base.range;
+
+	return EC_SUCCESS;
+}
+
+static int init(const struct motion_sensor_t *s)
+{
+	int ret = 0, tmp;
+	struct stprivate_data *data = s->drv_data;
+
+	ret = raw_read8(s->port, s->addr, LIS2DH_WHO_AM_I_REG, &tmp);
+	if (ret != EC_SUCCESS)
+		return EC_ERROR_UNKNOWN;
+
+	if (tmp != LIS2DH_WHO_AM_I)
+		return EC_ERROR_ACCESS_DENIED;
+
+	mutex_lock(s->mutex);
+	/* Device can be re-initialized after a reboot so any control
+	 * register must be restored to it's default
+	 */
+	/* Enable all accel axes data and clear old settings */
+	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL1_ADDR,
+			 LIS2DH_ENABLE_ALL_AXES);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL2_ADDR,
+			 LIS2DH_CTRL2_RESET_VAL);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL3_ADDR,
+			 LIS2DH_CTRL3_RESET_VAL);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	/* Enable BDU */
+	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL4_ADDR,
+			 LIS2DH_BDU_MASK);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL5_ADDR,
+			 LIS2DH_CTRL5_RESET_VAL);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL6_ADDR,
+			 LIS2DH_CTRL6_RESET_VAL);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	mutex_unlock(s->mutex);
+
+	/* Config initial Acc Range */
+	ret = set_range(s, s->default_range, 0);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* Set default resolution */
+	data->resol = LIS2DH_RESOLUTION;
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	ret = config_interrupt(s);
+#endif
+
+	CPRINTF("[%T %s: MS Done Init type:0x%X range:%d]\n",
+		s->name, s->type, get_range(s));
+	return ret;
+
+err_unlock:
+	CPRINTF("[%T %s: MS Init type:0x%X Error]\n", s->name, s->type);
+	mutex_unlock(s->mutex);
+
+	return EC_ERROR_UNKNOWN;
+}
+
+const struct accelgyro_drv lis2dh_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,
+	.set_offset = set_offset,
+	.get_offset = get_offset,
+	.perform_calib = NULL,
+#ifdef CONFIG_ACCEL_FIFO
+	.load_fifo = load_fifo,
+#endif /* CONFIG_ACCEL_FIFO */
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.irq_handler = irq_handler,
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+};
diff --git a/driver/accel_lis2dh.h b/driver/accel_lis2dh.h
new file mode 100644
index 0000000000..eac16d5e20
--- /dev/null
+++ b/driver/accel_lis2dh.h
@@ -0,0 +1,161 @@
+/* Copyright (c) 2016 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* LIS2DH accelerometer module for Chrome EC */
+
+#ifndef __CROS_EC_ACCEL_LIS2DH_H
+#define __CROS_EC_ACCEL_LIS2DH_H
+
+#define LIS2DH_I2C_ADDR(__x)	(__x << 1)
+
+/* 7-bit address is 000110Xb. Where 'X' is determined
+ * by the voltage on the ADDR pin
+ */
+#define LIS2DH_ADDR0		LIS2DH_I2C_ADDR(0x18)
+#define LIS2DH_ADDR1		LIS2DH_I2C_ADDR(0x19)
+
+/* Who Am I  */
+#define LIS2DH_WHO_AM_I_REG	0x0f
+#define LIS2DH_WHO_AM_I		0x33
+
+/* COMMON DEFINE FOR ACCEL SENSOR */
+#define LIS2DH_EN_BIT		0x01
+#define LIS2DH_DIS_BIT		0x00
+
+#define LIS2DH_INT2_ON_INT1_ADDR	0x13
+#define LIS2DH_INT2_ON_INT1_MASK	0x20
+
+#define LIS2DH_OUT_X_L_ADDR	0x28
+
+#define LIS2DH_CTRL1_ADDR	0x20
+#define LIS2DH_INT2_ON_INT1_MASK	0x20
+#define LIS2DH_ENABLE_ALL_AXES	0x07
+
+#define LIS2DH_CTRL2_ADDR	0x21
+#define LIS2DH_CTRL2_RESET_VAL	0x00
+
+#define LIS2DH_CTRL3_ADDR	0x22
+#define LIS2DH_CTRL3_RESET_VAL	0x00
+
+#define LIS2DH_CTRL4_ADDR	0x23
+#define LIS2DH_BDU_MASK		0x80
+
+#define LIS2DH_CTRL5_ADDR	0x24
+#define LIS2DH_CTRL5_RESET_VAL	0x00
+
+#define LIS2DH_CTRL6_ADDR	0x25
+#define LIS2DH_CTRL6_RESET_VAL	0x00
+
+#define LIS2DH_STATUS_REG	0x27
+#define LIS2DH_STS_XLDA_UP	0x80
+
+#ifdef CONFIG_ACCEL_FIFO
+
+/* FIFO regs, masks and define */
+#define LIS2DH_FIFO_WTM_INT_MASK	0x04
+#define LIS2DH_FIFO_CTRL_REG	0x2e
+#define LIS2DH_FIFO_MODE_MASK	0xc0
+#define LIS2DH_FIFO_THR_MASK	0x1f
+
+/* Select FIFO supported mode:
+ * BYPASS - Bypass FIFO
+ * FIFO - FIFO mode collect data
+ * STREAM - FIFO older data is replaced by new data
+ * SFIFO - Stream-to-FIFO mode. Mix FIFO & STREAM
+ */
+enum lis2dh_fifo_modes {
+	LIS2DH_FIFO_BYPASS_MODE = 0x00,
+	LIS2DH_FIFO_MODE,
+	LIS2DH_FIFO_STREAM_MODE,
+	LIS2DH_FIFO_SFIFO_MODE
+};
+
+/* Defines for LIS2DH_CTRL5_ADDR FIFO register */
+#define LIS2DH_FIFO_EN_MASK	0x40
+
+#define LIS2DH_FIFO_SRC_REG	0x2f
+#define LIS2DH_FIFO_EMPTY_FLAG	0x20
+#define LIS2DH_FIFO_UNREAD_MASK	0x1f
+#endif /* CONFIG_ACCEL_FIFO */
+
+/* Interrupt source status register */
+#define LIS2DH_INT1_SRC_REG	0x31
+
+/* Output data rate Mask register */
+#define LIS2DH_ACC_ODR_MASK	0xf0
+
+/* Acc data rate */
+enum lis2dh_odr {
+	LIS2DH_ODR_1HZ_VAL = 0x01,
+	LIS2DH_ODR_10HZ_VAL,
+	LIS2DH_ODR_25HZ_VAL,
+	LIS2DH_ODR_50HZ_VAL,
+	LIS2DH_ODR_100HZ_VAL,
+	LIS2DH_ODR_200HZ_VAL,
+	LIS2DH_ODR_400HZ_VAL,
+	LIS2DH_ODR_LIST_NUM
+};
+
+/* Return ODR reg value based on data rate set */
+#define LIS2DH_ODR_TO_REG(_odr) \
+	(_odr <= 1000) ? LIS2DH_ODR_1HZ_VAL : \
+	(_odr <= 10000) ? LIS2DH_ODR_10HZ_VAL : \
+	((31 - __builtin_clz(_odr / 25000))) + 3
+
+/* Return ODR real value normalized to sensor capabilities */
+#define LIS2DH_ODR_TO_NORMALIZE(_odr) \
+	(_odr <= 1000) ? 1000 : (_odr <= 10000) ? 10000 : \
+	(25000 * (1 << (31 - __builtin_clz(_odr / 25000))))
+
+/* Return ODR real value normalized to sensor capabilities from reg value */
+#define LIS2DH_REG_TO_NORMALIZE(_reg) \
+	(_reg == LIS2DH_ODR_1HZ_VAL) ? 1000 : \
+	(_reg == LIS2DH_ODR_10HZ_VAL) ? 10000 : (25000 * (1 << (_reg - 3)))
+
+/* Full scale range Mask register */
+#define LIS2DH_FS_MASK		0x30
+
+/* Acc FS value */
+enum lis2dh_fs {
+	LIS2DH_FS_2G_VAL = 0x00,
+	LIS2DH_FS_4G_VAL,
+	LIS2DH_FS_8G_VAL,
+	LIS2DH_FS_16G_VAL,
+	LIS2DH_FS_LIST_NUM
+};
+
+/* FS reg value from Full Scale */
+#define LIS2DH_FS_TO_REG(_fs) \
+	(31 - __builtin_clz(_fs) - 1)
+
+/* Full Scale range real value normalized to sensor capabilities */
+#define LIS2DH_FS_TO_NORMALIZE(_fs) \
+	(1 << (31 - __builtin_clz(_fs)))
+
+/* Acc Gain value */
+#define LIS2DH_FS_2G_GAIN	4
+#define LIS2DH_FS_4G_GAIN	8
+#define LIS2DH_FS_8G_GAIN	16
+#define LIS2DH_FS_16G_GAIN	48
+
+/* Return Gain from Full Scale range
+ * TODO: This work only for 2, 4 and 8 G full scale */
+#define LIS2DH_FS_TO_GAIN(_fs) \
+	(LIS2DH_FS_2G_GAIN << (30 - __builtin_clz(_fs)))
+
+/* Return Full Scale Range from normalized gain */
+#define LIS2DH_GAIN_TO_FS(_gain) \
+	(1 << (30 - __builtin_clz(_gain)))
+
+/* Sensor resolution in number of bits
+ * This sensor has variable precision (8/10/12 bits) depending Power Mode
+ * selected
+ * TODO: Only Normal Power mode supported (10 bits)
+ */
+#define LIS2DH_RESOLUTION      	10
+
+extern const struct accelgyro_drv lis2dh_drv;
+
+#endif /* __CROS_EC_ACCEL_LIS2DH_H */
diff --git a/driver/build.mk b/driver/build.mk
index 32da5f9686..687c5f5bba 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -14,6 +14,7 @@ driver-$(CONFIG_ACCELGYRO_LSM6DS0)+=accelgyro_lsm6ds0.o
 driver-$(CONFIG_ACCELGYRO_BMI160)+=accelgyro_bmi160.o
 driver-$(CONFIG_MAG_BMI160_BMM150)+=mag_bmm150.o
 driver-$(CONFIG_ACCELGYRO_LSM6DSM)+=accelgyro_lsm6dsm.o
+driver-$(CONFIG_ACCEL_LIS2DH)+=accel_lis2dh.o stm_mems_common.o
 
 # Gyrometers
 driver-$(CONFIG_GYRO_L3GD20H)+=gyro_l3gd20h.o
diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c
new file mode 100644
index 0000000000..9e482583b3
--- /dev/null
+++ b/driver/stm_mems_common.c
@@ -0,0 +1,182 @@
+/* Copyright (c) 2016 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/**
+ * Commons acc/gyro function for ST sensors in Chrome EC
+ */
+#include "stm_mems_common.h"
+
+/**
+ * Read single register
+ */
+inline int raw_read8(const int port, const int addr, const int reg,
+		     int *data_ptr)
+{
+	/* TODO: Implement SPI interface support */
+	return i2c_read8(port, addr, reg, data_ptr);
+}
+
+/**
+ * Write single register
+ */
+inline int raw_write8(const int port, const int addr, const int reg,
+		      int data)
+{
+	/* TODO: Implement SPI interface support */
+	return i2c_write8(port, addr, reg, data);
+}
+
+/**
+ * Read n bytes for read
+ * NOTE: Some chip use MSB for auto-increments in SUB address
+ * MSB must be set for autoincrement in multi read when auto_inc
+ * is set
+ */
+int raw_read_n(const int port, const int addr, const uint8_t reg,
+	       uint8_t *data_ptr, const int len, int auto_inc)
+{
+	int rv = -EC_ERROR_PARAM1;
+	uint8_t reg_a = reg;
+
+	if (auto_inc)
+		reg_a |= AUTO_INC;
+
+	/* TODO: Implement SPI interface support */
+	i2c_lock(port, 1);
+	rv = i2c_xfer(port, addr, &reg_a, 1, data_ptr, len, I2C_XFER_SINGLE);
+	i2c_lock(port, 0);
+
+	return rv;
+}
+
+ /**
+ * write_data_with_mask - Write register with mask
+ * @s: Motion sensor pointer
+ * @reg: Device register
+ * @mask: The mask to search
+ * @data: Data pointer
+ */
+int write_data_with_mask(const struct motion_sensor_t *s, int reg,
+			 uint8_t mask, uint8_t data)
+{
+	int err;
+	int new_data = 0x00, old_data = 0x00;
+
+	err = raw_read8(s->port, s->addr, reg, &old_data);
+	if (err != EC_SUCCESS)
+		return err;
+
+	new_data = ((old_data & (~mask)) | ((data << __builtin_ctz(mask)) & mask));
+
+	if (new_data == old_data)
+		return EC_SUCCESS;
+
+	return raw_write8(s->port, s->addr, reg, new_data);
+}
+
+ /**
+ * set_resolution - Set bit resolution
+ * @s: Motion sensor pointer
+ * @res: Bit resolution
+ * @rnd: Round bit
+ *
+ * TODO: must support multiple resolution
+ */
+int set_resolution(const struct motion_sensor_t *s, int res, int rnd)
+{
+	return EC_SUCCESS;
+}
+
+ /**
+ * get_resolution - Get bit resolution
+ * @s: Motion sensor pointer
+ *
+ * TODO: must support multiple resolution
+ */
+int get_resolution(const struct motion_sensor_t *s)
+{
+	struct stprivate_data *data = s->drv_data;
+
+	return data->resol;
+}
+
+/**
+ * set_offset - Set data offset
+ * @s: Motion sensor pointer
+ * @offset: offset vector
+ * @temp: Temp
+ */
+int set_offset(const struct motion_sensor_t *s,
+	       const int16_t *offset, int16_t temp)
+{
+	struct stprivate_data *data = s->drv_data;
+
+	data->offset[X] = offset[X];
+	data->offset[Y] = offset[Y];
+	data->offset[Z] = offset[Z];
+	return EC_SUCCESS;
+}
+
+/**
+ * get_offset - Get data offset
+ * @s: Motion sensor pointer
+ * @offset: offset vector
+ * @temp: Temp
+ */
+int get_offset(const struct motion_sensor_t *s,
+	       int16_t *offset, int16_t *temp)
+{
+	struct stprivate_data *data = s->drv_data;
+
+	offset[X] = data->offset[X];
+	offset[Y] = data->offset[Y];
+	offset[Z] = data->offset[Z];
+	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
+	return EC_SUCCESS;
+}
+
+/**
+ * get_data_rate - Get data rate (ODR)
+ * @s: Motion sensor pointer
+ */
+int get_data_rate(const struct motion_sensor_t *s)
+{
+	struct stprivate_data *data = s->drv_data;
+
+	return data->base.odr;
+}
+
+/**
+ * normalize - Apply LSB data sens. and rotation based on sensor resolution
+ * @s: Motion sensor pointer
+ * @v: output vector
+ * @data: LSB raw data
+ */
+void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
+{
+	int i;
+	struct stprivate_data *drvdata = s->drv_data;
+
+	/* Adjust data to sensor Sensitivity and Precision:
+	 * - devices with 16 bits resolution has gain in ug/LSB
+	 * - devices with 8/10 bits resolution has gain in mg/LSB
+	 */
+	for (i = 0; i < 3; i++) {
+		switch (drvdata->resol) {
+		case 10:
+			v[i] = ((int16_t)((data[i * 2 + 1] << 8) |
+				   data[i * 2]) >> 6);
+			v[i] = v[i] * drvdata->base.range;
+			break;
+		case 16:
+			v[i] = ((int16_t)(data[i * 2 + 1] << 8) |
+				    data[i * 2]);
+			v[i] = (v[i] * drvdata->base.range) / 1000;
+			break;
+		}
+	}
+
+	rotate(v, *s->rot_standard_ref, v);
+}
diff --git a/driver/stm_mems_common.h b/driver/stm_mems_common.h
new file mode 100644
index 0000000000..ca08de95de
--- /dev/null
+++ b/driver/stm_mems_common.h
@@ -0,0 +1,127 @@
+/* Copyright (c) 2016 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Commons acc/gyro function for ST sensors oin Chrome EC */
+
+#ifndef __CROS_EC_ST_COMMONS_H
+#define __CROS_EC_ST_COMMONS_H
+
+#include "common.h"
+#include "util.h"
+#include "accelgyro.h"
+#include "console.h"
+#include "i2c.h"
+#include "driver/accel_lis2dh.h"
+
+/* Common debug funcions */
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format "\n", ## args)
+
+/* Set auto increment subaddress on multiple access */
+#define AUTO_INC			0x80
+
+/* X, Y, Z axis data len */
+#define OUT_XYZ_SIZE			6
+
+/* Common define for poswer off ODR */
+#define ODR_POWER_OFF_VAL		0x00
+
+#ifdef CONFIG_ACCEL_FIFO
+#define FIFO_BUFFER_NUM_PATTERN		16
+/* Define number of data to be read from FIFO each time
+ * It must be a multiple of OUT_XYZ_SIZE.
+ * In case of LSM6DSM FIFO contains pattern depending ODR
+ * of Acc/gyro, be sure that FIFO can contains almost
+ * FIFO_BUFFER_NUM_PATTERNpattern
+ */
+#define FIFO_READ_LEN			(FIFO_BUFFER_NUM_PATTERN * OUT_XYZ_SIZE)
+
+#endif /* CONFIG_ACCEL_FIFO */
+
+/**
+ * Read single register
+ */
+inline int raw_read8(const int port, const int addr, const int reg,
+		     int *data_ptr);
+
+/**
+ * Write single register
+ */
+inline int raw_write8(const int port, const int addr, const int reg,
+		      int data);
+
+/**
+ * Read n bytes for read
+ * NOTE: Some chip use MSB for auto-increments in SUB address
+ * 		 MSB must be set for autoincrement in multi read when auto_inc
+ * 		 is set
+ */
+int raw_read_n(const int port, const int addr, const uint8_t reg,
+	       uint8_t *data_ptr, const int len, int auto_inc);
+
+ /**
+ * write_data_with_mask - Write register with mask
+ * @s: Motion sensor pointer
+ * @reg: Device register
+ * @mask: The mask to search
+ * @data: Data pointer
+ */
+int write_data_with_mask(const struct motion_sensor_t *s, int reg,
+			 uint8_t mask, uint8_t data);
+
+ /**
+ * set_resolution - Set bit resolution
+ * @s: Motion sensor pointer
+ * @res: Bit resolution
+ * @rnd: Round bit
+ */
+int set_resolution(const struct motion_sensor_t *s, int res, int rnd);
+
+ /**
+ * get_resolution - Get bit resolution
+ * @s: Motion sensor pointer
+ *
+ * TODO: must support multiple resolution
+ */
+int get_resolution(const struct motion_sensor_t *s);
+
+/**
+ * set_offset - Set data offset
+ * @s: Motion sensor pointer
+ * @offset: offset vector
+ * @temp: Temp
+ */
+int set_offset(const struct motion_sensor_t *s,
+	       const int16_t *offset, int16_t temp);
+
+/**
+ * get_offset - Get data offset
+ * @s: Motion sensor pointer
+ * @offset: offset vector
+ * @temp: Temp
+ */
+int get_offset(const struct motion_sensor_t *s, int16_t *offset, int16_t *temp);
+
+/**
+ * get_data_rate - Get data rate (ODR)
+ * @s: Motion sensor pointer
+ */
+int get_data_rate(const struct motion_sensor_t *s);
+
+/**
+ * normalize - Apply to LSB data sensitivity and rotation
+ * @s: Motion sensor pointer
+ * @v: vector
+ * @data: LSB raw data
+ */
+void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data);
+
+/* Internal data structure for sensors */
+struct stprivate_data {
+	struct accelgyro_saved_data_t base;
+	int16_t offset[3];
+	uint8_t resol;
+};
+
+#endif /* __CROS_EC_ST_COMMONS_H */