driver: lsm6dsm/lsm6dsl/lis2dh: Use STM MEMs common functions

LSM6DSM now use STM MEMs common functions already
used by LIS2DH12 device. Added st_raw_read_n_noinc
function to read i2c data not using auto increment
protocol.

BUG=b:73546254
BRANCH=master
TEST=Tested on discovery BOARD with LSM6DSM sensor connected on
I2C master bus interface of target board.
Using motion sense console commands is possible to enable, set data
rate, set full scale and show data from sensor (acc and gyro).
Commons function works properly for LSM6DSM.

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

6 files changed, 150 insertions, 227 deletions

diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c
index 017621ff6a..e937205755 100644
--- a/driver/accel_lis2dh.c
+++ b/driver/accel_lis2dh.c
@@ -19,6 +19,9 @@
 #include "driver/accel_lis2dh.h"
 #include "driver/stm_mems_common.h"
 
+#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+
 #ifdef CONFIG_ACCEL_FIFO
 /**
  * enable_fifo - Enable/Disable FIFO in LIS2DH
@@ -317,10 +320,6 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 	/* Transform from LSB to real data with rotation and gain */
 	st_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;
 }
 
diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index c685615f9f..11aceb5347 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -4,21 +4,19 @@
  */
 
 /**
- * LSM6DSM accelerometer and gyro module for Chrome EC
+ * LSM6DSx (x is L or M) accelerometer and gyro module for Chrome EC
  * 3D digital accelerometer & 3D digital gyroscope
+ * This driver supports both devices LSM6DSM and LSM6DSL
  */
 
-#include "accelgyro.h"
-#include "common.h"
-#include "console.h"
 #include "driver/accelgyro_lsm6dsm.h"
 #include "hooks.h"
-#include "i2c.h"
 #include "math_util.h"
 #include "task.h"
-#include "util.h"
 
+#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
 
 /**
  * @return output base register for sensor
@@ -30,64 +28,22 @@ static inline int get_xyz_reg(enum motionsensor_type type)
 }
 
 /**
- * Read single register
- */
-static inline int raw_read8(const int port, const int addr, const int reg,
-			    int *data_ptr)
-{
-	return i2c_read8(port, addr, reg, data_ptr);
-}
-
-/**
- * Write single register
- */
-static inline int raw_write8(const int port, const int addr, const int reg,
-			     int data)
-{
-	return i2c_write8(port, addr, reg, data);
-}
-
- /**
- * write_data_with_mask - Write register with mask
- * @s: Motion sensor pointer
- * @reg: Device register
- * @mask: The mask to search
- * @data: Data pointer
- */
-static 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_range - set full scale range
  * @s: Motion sensor pointer
  * @range: Range
  * @rnd: Round up/down flag
+ * Note: Range is sensitivity/gain for speed purpose
  */
 static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 {
 	int err;
 	uint8_t ctrl_reg, reg_val;
-	struct lsm6dsm_data *data = s->drv_data;
+	struct stprivate_data *data = s->drv_data;
 	int newrange = range;
 
 	ctrl_reg = LSM6DSM_RANGE_REG(s->type);
 	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
-		/* Adjust and check rounded value for acc */
+		/* Adjust and check rounded value for acc. */
 		if (rnd && (newrange < LSM6DSM_ACCEL_NORMALIZE_FS(newrange)))
 			newrange <<= 1;
 		if (newrange > LSM6DSM_ACCEL_FS_MAX_VAL)
@@ -95,7 +51,7 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 
 		reg_val = LSM6DSM_ACCEL_FS_REG(newrange);
 	} else {
-		/* Adjust and check rounded value for gyro */
+		/* Adjust and check rounded value for gyro. */
 		if (rnd && (newrange < LSM6DSM_GYRO_NORMALIZE_FS(newrange)))
 			newrange <<= 1;
 		if (newrange > LSM6DSM_GYRO_FS_MAX_VAL)
@@ -105,18 +61,26 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 	}
 
 	mutex_lock(s->mutex);
-	err = write_data_with_mask(s, ctrl_reg, LSM6DSM_RANGE_MASK, reg_val);
+	err = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_RANGE_MASK, reg_val);
 	if (err == EC_SUCCESS)
-		/* Save internally gain for speed optimization in read polling data */
+		/* Save internally gain for speed optimization. */
 		data->base.range = (s->type == MOTIONSENSE_TYPE_ACCEL ?
-			LSM6DSM_ACCEL_FS_GAIN(newrange) : LSM6DSM_GYRO_FS_GAIN(newrange));
+				    LSM6DSM_ACCEL_FS_GAIN(newrange) :
+				    LSM6DSM_GYRO_FS_GAIN(newrange));
 	mutex_unlock(s->mutex);
+
 	return EC_SUCCESS;
 }
 
+/**
+ * get_range - get full scale range
+ * @s: Motion sensor pointer
+ *
+ * For mag range is fixed to LIS2MDL_RANGE by hardware
+ */
 static int get_range(const struct motion_sensor_t *s)
 {
-	struct lsm6dsm_data *data = s->drv_data;
+	struct stprivate_data *data = s->drv_data;
 
 	if (MOTIONSENSE_TYPE_ACCEL == s->type)
 		return LSM6DSM_ACCEL_GAIN_FS(data->base.range);
@@ -124,85 +88,61 @@ static int get_range(const struct motion_sensor_t *s)
 	return LSM6DSM_GYRO_GAIN_FS(data->base.range);
 }
 
-static int get_resolution(const struct motion_sensor_t *s)
-{
-	/* Only one resolution, LSM6DSM_RESOLUTION, so nothing to do. */
-	return LSM6DSM_RESOLUTION;
-}
-
+/**
+ * set_data_rate
+ * @s: Motion sensor pointer
+ * @range: Rate (mHz)
+ * @rnd: Round up/down flag
+ *
+ * For mag in cascade with lsm6dsm/l we use acc trigger and FIFO decimator
+ */
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
-	int ret, normalized_rate;
-	struct lsm6dsm_data *data = s->drv_data;
+	int ret, normalized_rate = LSM6DSM_ODR_MIN_VAL;
+	struct stprivate_data *data = s->drv_data;
 	uint8_t ctrl_reg, reg_val;
 
-	reg_val = LSM6DSM_ODR_TO_REG(rate);
 	ctrl_reg = LSM6DSM_ODR_REG(s->type);
-	normalized_rate = LSM6DSM_ODR_TO_NORMALIZE(rate);
 
 	if (rate == 0) {
-		/* Power Off device */
-		ret = write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK,
-					   LSM6DSM_ODR_POWER_OFF_VAL);
+		/* Power off acc or gyro. */
+		mutex_lock(s->mutex);
+
+		ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK,
+					      LSM6DSM_ODR_0HZ_VAL);
+		if (ret == EC_SUCCESS)
+			data->base.odr = LSM6DSM_ODR_0HZ_VAL;
+
+		mutex_unlock(s->mutex);
+
 		return ret;
 	}
 
+	reg_val = LSM6DSM_ODR_TO_REG(rate);
+	normalized_rate = LSM6DSM_ODR_TO_NORMALIZE(rate);
+
 	if (rnd && (normalized_rate < rate)) {
 		reg_val++;
 		normalized_rate <<= 1;
 	}
 
-	/* Adjust rounded value for acc and gyro because ODR are shared */
-	if (reg_val > LSM6DSM_ODR_833HZ_VAL) {
-		reg_val = LSM6DSM_ODR_833HZ_VAL;
+	/* Adjust rounded value for acc and gyro because ODR are shared. */
+	if (reg_val > LSM6DSM_ODR_416HZ_VAL) {
+		reg_val = LSM6DSM_ODR_416HZ_VAL;
 		normalized_rate = LSM6DSM_ODR_MAX_VAL;
 	} else if (reg_val < LSM6DSM_ODR_13HZ_VAL) {
 		reg_val = LSM6DSM_ODR_13HZ_VAL;
 		normalized_rate = LSM6DSM_ODR_MIN_VAL;
 	}
 
-	/*
-	 * Lock accel resource to prevent another task from attempting
-	 * to write accel parameters until we are done
-	 */
 	mutex_lock(s->mutex);
-	ret = write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, reg_val);
+	ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, reg_val);
 	if (ret == EC_SUCCESS)
 		data->base.odr = normalized_rate;
 
 	mutex_unlock(s->mutex);
-	return ret;
-}
-
-static int get_data_rate(const struct motion_sensor_t *s)
-{
-	struct lsm6dsm_data *data = s->drv_data;
-
-	return data->base.odr;
-}
 
-static int set_offset(const struct motion_sensor_t *s,
-		      const int16_t *offset, int16_t temp)
-{
-	struct lsm6dsm_data *data = s->drv_data;
-
-	data->offset[X] = offset[X];
-	data->offset[Y] = offset[Y];
-	data->offset[Z] = offset[Z];
-	return EC_SUCCESS;
-}
-
-static int get_offset(const struct motion_sensor_t *s,
-		      int16_t *offset,
-		      int16_t *temp)
-{
-	struct lsm6dsm_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;
+	return ret;
 }
 
 static int is_data_ready(const struct motion_sensor_t *s, int *ready)
@@ -228,14 +168,13 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready)
  *
  * Is not very efficient to collect the data in read: better have an interrupt
  * and collect the FIFO, even if it has one item: we don't have to check if the
- * sensor is ready (minimize I2C access)
+ * sensor is ready (minimize I2C access).
  */
 static int read(const struct motion_sensor_t *s, vector_3_t v)
 {
-	uint8_t raw[LSM6DSM_OUT_XYZ_SIZE];
+	uint8_t raw[OUT_XYZ_SIZE];
 	uint8_t xyz_reg;
-	int ret, i, range, tmp = 0;
-	struct lsm6dsm_data *data = s->drv_data;
+	int ret, tmp = 0;
 
 	ret = is_data_ready(s, &tmp);
 	if (ret != EC_SUCCESS)
@@ -254,31 +193,13 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 
 	xyz_reg = get_xyz_reg(s->type);
 
-	/* Read data bytes starting at xyz_reg */
-	i2c_lock(s->port, 1);
-	ret = i2c_xfer(s->port, s->addr, &xyz_reg, 1, raw,
-		       LSM6DSM_OUT_XYZ_SIZE, I2C_XFER_SINGLE);
-	i2c_lock(s->port, 0);
-
-	if (ret != EC_SUCCESS) {
-		CPRINTF("[%T %s type:0x%X RD XYZ Error]",
-			s->name, s->type);
+	/* Read data bytes starting at xyz_reg. */
+	ret = st_raw_read_n_noinc(s->port, s->addr, xyz_reg, raw, OUT_XYZ_SIZE);
+	if (ret != EC_SUCCESS)
 		return ret;
-	}
 
-	for (i = X; i <= Z; i++) {
-		v[i] = ((int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2]));
-		/* On range we seved gain related to FS */
-		v[i] = v[i] * data->base.range;
-	}
-
-	/* Apply rotation matrix */
-	rotate(v, *s->rot_standard_ref, v);
-
-	/* Apply offset in the device coordinates */
-	range = get_range(s);
-	for (i = X; i <= Z; i++)
-		v[i] += (data->offset[i] << 5) / range;
+	/* Apply precision, sensitivity and rotation vector. */
+	st_normalize(s, v, raw);
 
 	return EC_SUCCESS;
 }
@@ -286,6 +207,7 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 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, LSM6DSM_WHO_AM_I_REG, &tmp);
 	if (ret != EC_SUCCESS)
@@ -295,36 +217,42 @@ static int init(const struct motion_sensor_t *s)
 		return EC_ERROR_ACCESS_DENIED;
 
 	/*
-	 * This sensor can be powered through an EC reboot, so the state of
-	 * the sensor is unknown here so reset it
-	 * lsm6dsm supports both accel & gyro features
+	 * This sensor can be powered through an EC reboot, so the state of the
+	 * sensor is unknown here so reset it
+	 * LSM6DSM/L supports both accel & gyro features
 	 * Board will see two virtual sensor devices: accel & gyro
-	 * Requirement: Accel need be init before gyro
+	 * Requirement: Accel need be init before gyro and mag
 	 */
 	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
 		mutex_lock(s->mutex);
 
-		/* Software reset */
-		ret = write_data_with_mask(s, LSM6DSM_RESET_ADDR,
-					   LSM6DSM_RESET_MASK, LSM6DSM_EN_BIT);
+		/* Software reset. */
+		ret = st_write_data_with_mask(s,
+				LSM6DSM_RESET_ADDR,
+				LSM6DSM_RESET_MASK,
+				LSM6DSM_EN_BIT);
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 
-		/* Output data not updated until have been read */
-		ret = write_data_with_mask(s, LSM6DSM_BDU_ADDR,
-					   LSM6DSM_BDU_MASK, LSM6DSM_EN_BIT);
+		/* Output data not updated until have been read. */
+		ret = st_write_data_with_mask(s,
+				LSM6DSM_BDU_ADDR,
+				LSM6DSM_BDU_MASK,
+				LSM6DSM_EN_BIT);
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 
 		mutex_unlock(s->mutex);
 	}
 
+	/* Set default resolution common to acc and gyro. */
+	data->resol = LSM6DSM_RESOLUTION;
 	return sensor_init_done(s);
 
 err_unlock:
 	mutex_unlock(s->mutex);
 
-	return EC_ERROR_UNKNOWN;
+	return ret;
 }
 
 const struct accelgyro_drv lsm6dsm_drv = {
@@ -332,9 +260,9 @@ const struct accelgyro_drv lsm6dsm_drv = {
 	.read = read,
 	.set_range = set_range,
 	.get_range = get_range,
-	.get_resolution = get_resolution,
+	.get_resolution = st_get_resolution,
 	.set_data_rate = set_data_rate,
-	.get_data_rate = get_data_rate,
-	.set_offset = set_offset,
-	.get_offset = get_offset,
+	.get_data_rate = st_get_data_rate,
+	.set_offset = st_set_offset,
+	.get_offset = st_get_offset,
 };
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index bf40741140..1c1dea606d 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -1,29 +1,27 @@
-/* Copyright (c) 2016 The Chromium OS Authors. All rights reserved.
+/* Copyright 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.
  */
 
-/* LSM6DSM Accel and Gyro driver for Chrome EC */
+/* LSM6DSM (also LSM6DSL) Accel and Gyro driver for Chrome EC */
 
 #ifndef __CROS_EC_ACCELGYRO_LSM6DSM_H
 #define __CROS_EC_ACCELGYRO_LSM6DSM_H
 
-#include "accelgyro.h"
+#include "stm_mems_common.h"
 
 #define LSM6DSM_I2C_ADDR(__x)		(__x << 1)
 
 /*
- * 7-bit address is 110101Xb. Where 'X' is determined
- * by the voltage on the ADDR pin.
+ * 7-bit address is 110101xb. Where 'x' is determined
+ * by the voltage on the ADDR pin
  */
-#define LSM6DSM_ADDR0             	LSM6DSM_I2C_ADDR(0x6a)
-#define LSM6DSM_ADDR1             	LSM6DSM_I2C_ADDR(0x6b)
+#define LSM6DSM_ADDR0			LSM6DSM_I2C_ADDR(0x6a)
+#define LSM6DSM_ADDR1			LSM6DSM_I2C_ADDR(0x6b)
 
 /* Who Am I */
 #define LSM6DSM_WHO_AM_I_REG		0x0f
-#define LSM6DSM_WHO_AM_I          	0x6a
-
-#define LSM6DSM_OUT_XYZ_SIZE 		6
+#define LSM6DSM_WHO_AM_I		0x6a
 
 /* Sensor Software Reset Bit */
 #define LSM6DSM_RESET_ADDR		0x12
@@ -33,15 +31,9 @@
 #define LSM6DSM_EN_BIT			0x01
 #define LSM6DSM_DIS_BIT			0x00
 
-#define LSM6DSM_LIR_ADDR		0x58
-#define LSM6DSM_LIR_MASK		0x01
-
 #define LSM6DSM_BDU_ADDR		0x12
 #define LSM6DSM_BDU_MASK		0x40
 
-#define LSM6DSM_INT2_ON_INT1_ADDR	0x13
-#define LSM6DSM_INT2_ON_INT1_MASK	0x20
-
 #define LSM6DSM_GYRO_OUT_X_L_ADDR	0x22
 #define LSM6DSM_ACCEL_OUT_X_L_ADDR	0x28
 
@@ -60,20 +52,19 @@
 
 /* Common Acc/Gyro data rate */
 enum lsm6dsm_odr {
-	LSM6DSM_ODR_POWER_OFF_VAL = 0x00,
+	LSM6DSM_ODR_0HZ_VAL = 0,
 	LSM6DSM_ODR_13HZ_VAL,
 	LSM6DSM_ODR_26HZ_VAL,
 	LSM6DSM_ODR_52HZ_VAL,
 	LSM6DSM_ODR_104HZ_VAL,
 	LSM6DSM_ODR_208HZ_VAL,
 	LSM6DSM_ODR_416HZ_VAL,
-	LSM6DSM_ODR_833HZ_VAL,
 	LSM6DSM_ODR_LIST_NUM
 };
 
 /* Absolute maximum rate for acc and gyro sensors */
 #define LSM6DSM_ODR_MIN_VAL		13000
-#define LSM6DSM_ODR_MAX_VAL		833000
+#define LSM6DSM_ODR_MAX_VAL		416000
 
 /* ODR reg value from selected data rate in mHz */
 #define LSM6DSM_ODR_TO_REG(_odr) \
@@ -83,7 +74,7 @@ enum lsm6dsm_odr {
 #define LSM6DSM_ODR_TO_NORMALIZE(_odr) \
 	(LSM6DSM_ODR_MIN_VAL << __fls(_odr/LSM6DSM_ODR_MIN_VAL))
 
-/* Full Scale range value for Accel */
+/* Full Scale range value and gain for Acc */
 #define LSM6DSM_FS_LIST_NUM		4
 
 #define LSM6DSM_ACCEL_FS_ADDR		0x10
@@ -120,7 +111,7 @@ enum lsm6dsm_odr {
 #define LSM6DSM_ACCEL_NORMALIZE_FS(_fs) \
 	(1 << (32 - __builtin_clz(_fs / 2)))
 
-/* Full Scale range value for Gyro */
+/* Full Scale range value and gain for Gyro */
 #define LSM6DSM_GYRO_FS_ADDR		0x11
 #define LSM6DSM_GYRO_FS_MASK		0x0c
 
@@ -154,7 +145,7 @@ enum lsm6dsm_odr {
 	(_fs == 245 ? 245 : 500 << __fls(_fs / 500))
 
 /* FS register address/mask for Acc/Gyro sensors */
-#define LSM6DSM_RANGE_REG(_sensor)  	(LSM6DSM_ACCEL_FS_ADDR + (_sensor))
+#define LSM6DSM_RANGE_REG(_sensor)  (LSM6DSM_ACCEL_FS_ADDR + (_sensor))
 #define LSM6DSM_RANGE_MASK  		0x0c
 
 /* Status register bitmask for Acc/Gyro data ready */
@@ -167,14 +158,9 @@ enum lsm6dsm_status {
 #define LSM6DSM_STS_XLDA_MASK		0x01
 #define LSM6DSM_STS_GDA_MASK		0x02
 
-/* Sensor resolution in number of bits. This sensor has fixed resolution. */
+/* Sensor resolution in number of bits: fixed 16 bit */
 #define LSM6DSM_RESOLUTION      	16
 
 extern const struct accelgyro_drv lsm6dsm_drv;
 
-struct lsm6dsm_data {
-	struct accelgyro_saved_data_t base;
-	int16_t offset[3];
-};
-
 #endif /* __CROS_EC_ACCELGYRO_LSM6DSM_H */
diff --git a/driver/build.mk b/driver/build.mk
index 4fa26af667..23eb0a2d65 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -13,7 +13,7 @@ driver-$(CONFIG_ACCEL_KX022)+=accel_kionix.o
 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_ACCELGYRO_LSM6DSM)+=accelgyro_lsm6dsm.o stm_mems_common.o
 driver-$(CONFIG_ACCEL_LIS2DH)+=accel_lis2dh.o stm_mems_common.o
 driver-$(CONFIG_SYNC)+=sync.o
 
diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c
index afffba2d25..c2939ae714 100644
--- a/driver/stm_mems_common.c
+++ b/driver/stm_mems_common.c
@@ -27,10 +27,7 @@ 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
+ * st_raw_read_n - Read n bytes for read
  */
 int st_raw_read_n(const int port, const int addr, const uint8_t reg,
 	       uint8_t *data_ptr, const int len)
@@ -46,8 +43,24 @@ int st_raw_read_n(const int port, const int addr, const uint8_t reg,
 	return rv;
 }
 
+/**
+ * st_raw_read_n_noinc - Read n bytes for read (no auto inc address)
+ */
+int st_raw_read_n_noinc(const int port, const int addr, const uint8_t reg,
+	       uint8_t *data_ptr, const int len)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	/* TODO: Implement SPI interface support */
+	i2c_lock(port, 1);
+	rv = i2c_xfer(port, addr, &reg, 1, data_ptr, len, I2C_XFER_SINGLE);
+	i2c_lock(port, 0);
+
+	return rv;
+}
+
  /**
- * write_data_with_mask - Write register with mask
+ * st_write_data_with_mask - Write register with mask
  * @s: Motion sensor pointer
  * @reg: Device register
  * @mask: The mask to search
@@ -72,11 +85,9 @@ int st_write_data_with_mask(const struct motion_sensor_t *s, int reg,
 	return raw_write8(s->port, s->addr, reg, new_data);
 }
 
- /**
- * get_resolution - Get bit resolution
+/**
+ * st_get_resolution - Get bit resolution
  * @s: Motion sensor pointer
- *
- * TODO: must support multiple resolution
  */
 int st_get_resolution(const struct motion_sensor_t *s)
 {
@@ -86,7 +97,7 @@ int st_get_resolution(const struct motion_sensor_t *s)
 }
 
 /**
- * set_offset - Set data offset
+ * st_set_offset - Set data offset
  * @s: Motion sensor pointer
  * @offset: offset vector
  * @temp: Temp
@@ -103,7 +114,7 @@ int st_set_offset(const struct motion_sensor_t *s,
 }
 
 /**
- * get_offset - Get data offset
+ * st_get_offset - Get data offset
  * @s: Motion sensor pointer
  * @offset: offset vector
  * @temp: Temp
@@ -121,7 +132,7 @@ int st_get_offset(const struct motion_sensor_t *s,
 }
 
 /**
- * get_data_rate - Get data rate (ODR)
+ * st_get_data_rate - Get data rate (ODR)
  * @s: Motion sensor pointer
  */
 int st_get_data_rate(const struct motion_sensor_t *s)
@@ -132,34 +143,30 @@ int st_get_data_rate(const struct motion_sensor_t *s)
 }
 
 /**
- * normalize - Apply LSB data sens. and rotation based on sensor resolution
+ * st_normalize - Apply LSB data sens. and rotation based on sensor resolution
  * @s: Motion sensor pointer
  * @v: output vector
  * @data: LSB raw data
  */
 void st_normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
 {
-	int i;
+	int i, range;
 	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;
-		}
+	for (i = X; i <= Z; i++) {
+		v[i] = (int16_t)((data[i * 2 + 1] << 8) |
+				data[i * 2]) >> (16 - drvdata->resol);
 	}
 
 	rotate(v, *s->rot_standard_ref, v);
+
+	/* apply offset in the device coordinates */
+	range = s->drv->get_range(s);
+	for (i = X; i <= Z; i++)
+		/*
+		 * TODO(gwendal): only work for accel:
+		 * offsest is in 1/1024th g/LSB, so shift left and divide by
+		 * range to normalize the value.
+		 */
+		v[i] += (drvdata->offset[i] << 5) / range;
 }
diff --git a/driver/stm_mems_common.h b/driver/stm_mems_common.h
index 848c3d4825..b708812d63 100644
--- a/driver/stm_mems_common.h
+++ b/driver/stm_mems_common.h
@@ -14,14 +14,13 @@
 #include "console.h"
 #include "i2c.h"
 
-/* Common debug funcions */
-#define CPRINTF(format, args...) cprintf(CC_ACCEL, format "\n", ## args)
-
 /* X, Y, Z axis data len */
 #define OUT_XYZ_SIZE			6
 
+#define ST_NORMALIZE_RATE(_fS)    (1 << __fls(_fs))
+
 #ifdef CONFIG_ACCEL_FIFO
-#define FIFO_BUFFER_NUM_PATTERN		16
+#define FIFO_BUFFER_NUM_PATTERN		32
 /* 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
@@ -45,13 +44,19 @@ extern inline int raw_write8(const int port, const int addr, const int reg,
 		      int data);
 
 /**
- * Read n bytes for read
+ * st_raw_read_n - Read n bytes for read
  */
 int st_raw_read_n(const int port, const int addr, const uint8_t reg,
 	       uint8_t *data_ptr, const int len);
 
+/**
+ * st_raw_read_n_noinc - Read n bytes for read (no auto inc address)
+ */
+int st_raw_read_n_noinc(const int port, const int addr, const uint8_t reg,
+	       uint8_t *data_ptr, const int len);
+
  /**
- * write_data_with_mask - Write register with mask
+ * st_write_data_with_mask - Write register with mask
  * @s: Motion sensor pointer
  * @reg: Device register
  * @mask: The mask to search
@@ -61,15 +66,13 @@ int st_write_data_with_mask(const struct motion_sensor_t *s, int reg,
 			 uint8_t mask, uint8_t data);
 
  /**
- * get_resolution - Get bit resolution
+ * st_get_resolution - Get bit resolution
  * @s: Motion sensor pointer
- *
- * TODO: must support multiple resolution
  */
 int st_get_resolution(const struct motion_sensor_t *s);
 
 /**
- * set_offset - Set data offset
+ * st_set_offset - Set data offset
  * @s: Motion sensor pointer
  * @offset: offset vector
  * @temp: Temp
@@ -78,7 +81,7 @@ int st_set_offset(const struct motion_sensor_t *s,
 		  const int16_t *offset, int16_t temp);
 
 /**
- * get_offset - Get data offset
+ * st_get_offset - Get data offset
  * @s: Motion sensor pointer
  * @offset: offset vector
  * @temp: Temp
@@ -87,13 +90,13 @@ int st_get_offset(const struct motion_sensor_t *s,
 		  int16_t *offset, int16_t *temp);
 
 /**
- * get_data_rate - Get data rate (ODR)
+ * st_get_data_rate - Get data rate (ODR)
  * @s: Motion sensor pointer
  */
 int st_get_data_rate(const struct motion_sensor_t *s);
 
 /**
- * normalize - Apply to LSB data sensitivity and rotation
+ * st_normalize - Apply to LSB data sensitivity and rotation
  * @s: Motion sensor pointer
  * @v: vector
  * @data: LSB raw data