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