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

1 files changed, 78 insertions, 150 deletions

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,
 };