driver: lsm6dso: Add support to LSM6DSO IMU

Added support to LSM6DSO IMU sensor.
Features included in this driver are:
 - Basic Sensor Read acc/gyro data
 - ODR and FS runtime configuration
 - FIFO water mark interrupt
 - Shared commons function with ST MEMs devices

BUG=none
BRANCH=master
TEST=Tested on discovery target BOARD with
LSM6DSO connected to EC i2c master bus and motion
sense task running.

Commands used to test LSM6DSO device are:
 - accelinit
 - accelrange
 - accelinfo

All basic features tested, including:
1) ODR change:
 - accelrate 0 [13000:208000]
 - accelrate 1 [13000:208000]

2) FS Range change:
 - accelrange 0 [2:16]
 - accelrange 1 [250:2000]

3) Interrupt on FIFO water mark

Signed-off-by: Mario Tesi <mario.tesi@st.com>

Change-Id: If2984f7d0d30b0ef475e0525aca2bc365aa4fe21
Signed-off-by: Mario Tesi <mario.tesi@st.com>
Reviewed-on: https://chromium-review.googlesource.com/1371364
Commit-Ready: ChromeOS CL Exonerator Bot <chromiumos-cl-exonerator@appspot.gserviceaccount.com>
Tested-by: Enrico Granata <egranata@chromium.org>
Reviewed-by: Enrico Granata <egranata@chromium.org>

7 files changed, 743 insertions, 0 deletions

diff --git a/common/build.mk b/common/build.mk
index 3b25e0bc99..a52173a385 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -13,6 +13,7 @@ common-$(CONFIG_ACCELGYRO_BMA255)+=math_util.o
 common-$(CONFIG_ACCELGYRO_BMI160)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DS0)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DSM)+=math_util.o
+common-$(CONFIG_ACCELGYRO_LSM6DSO)+=math_util.o
 common-$(CONFIG_ACCEL_LIS2DH)+=math_util.o
 common-$(CONFIG_ACCEL_KXCJ9)+=math_util.o
 common-$(CONFIG_ACCEL_KX022)+=math_util.o
diff --git a/driver/accelgyro_lsm6dso.c b/driver/accelgyro_lsm6dso.c
new file mode 100644
index 0000000000..d50e24aa17
--- /dev/null
+++ b/driver/accelgyro_lsm6dso.c
@@ -0,0 +1,501 @@
+/* Copyright 2019 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.
+ */
+
+/**
+ * LSM6DSO Accel and Gyro module for Chrome EC
+ * 3D digital accelerometer & 3D digital gyroscope
+ *
+ * For any details on driver implementation please
+ * Refer to AN5192 Application Note on www.st.com
+ */
+
+#include "driver/accelgyro_lsm6dso.h"
+#include "hooks.h"
+#include "hwtimer.h"
+#include "math_util.h"
+#include "task.h"
+#include "timer.h"
+
+#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
+
+#ifdef CONFIG_ACCEL_FIFO
+static volatile uint32_t last_interrupt_timestamp;
+#endif /* CONFIG_ACCEL_FIFO */
+
+/*
+ * When ODR change, the sensor filters need settling time;
+ * Add a counter to discard a well known number of data with
+ * incorrect values.
+ */
+static uint32_t samples_to_discard[LSM6DSO_FIFO_DEV_NUM];
+
+/**
+ * @return output data base register for sensor
+ */
+static inline int get_xyz_reg(enum motionsensor_type type)
+{
+	return LSM6DSO_ACCEL_OUT_X_L_ADDR -
+	       (LSM6DSO_ACCEL_OUT_X_L_ADDR - LSM6DSO_GYRO_OUT_X_L_ADDR) * type;
+}
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+/**
+ * Configure interrupt int 1 to fire handler for:
+ *
+ * FIFO threshold on watermark (1 sample)
+ *
+ * @s: Motion sensor pointer
+ */
+static int config_interrupt(const struct motion_sensor_t *s)
+{
+	int ret = EC_SUCCESS;
+
+#ifdef CONFIG_ACCEL_FIFO
+	int int1_ctrl_val;
+
+	ret = st_raw_read8(s->port, s->addr, LSM6DSO_INT1_CTRL,
+			   &int1_ctrl_val);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/*
+	 * Configure FIFO threshold to 1 sample: interrupt on watermark
+	 * will be generated every time a new data sample will be stored
+	 * in FIFO. The interrupr on watermark is cleared only when the
+	 * number or samples still present in FIFO exceeds the configured
+	 * threshold.
+	 */
+	ret = st_raw_write8(s->port, s->addr, LSM6DSO_FIFO_CTRL1_ADDR, 1);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	int1_ctrl_val |= LSM6DSO_INT_FIFO_TH | LSM6DSO_INT_FIFO_OVR |
+		LSM6DSO_INT_FIFO_FULL;
+
+	ret = st_raw_write8(s->port, s->addr, LSM6DSO_INT1_CTRL,
+			    int1_ctrl_val);
+#endif /* CONFIG_ACCEL_FIFO */
+
+	return ret;
+}
+
+#ifdef CONFIG_ACCEL_FIFO
+/**
+ * fifo_disable - set fifo mode to LSM6DSO_FIFO_MODE_BYPASS_VAL
+ * @s: Motion sensor pointer: must be MOTIONSENSE_TYPE_ACCEL.
+ */
+static int fifo_disable(const struct motion_sensor_t *s)
+{
+	return st_raw_write8(s->port, s->addr, LSM6DSO_FIFO_CTRL4_ADDR,
+			     LSM6DSO_FIFO_MODE_BYPASS_VAL);
+}
+
+/**
+ * set_fifo_params - Configure internal FIFO parameters
+ *
+ * Configure FIFO decimator to have every time the right pattern
+ * with acc/gyro
+ */
+static int fifo_enable(const struct motion_sensor_t *s)
+{
+	return st_raw_write8(s->port, s->addr, LSM6DSO_FIFO_CTRL4_ADDR,
+			     LSM6DSO_FIFO_MODE_CONTINUOUS_VAL);
+}
+
+/**
+ * push_fifo_data - Scan data pattern and push upside
+ */
+static void push_fifo_data(struct motion_sensor_t *main_s, uint8_t *fifo,
+			   uint32_t saved_ts)
+{
+	struct ec_response_motion_sensor_data vect;
+	struct motion_sensor_t *sensor;
+	uint8_t tag;
+	int id;
+	int *axis;
+	uint8_t *ptr;
+	uint8_t ag_maps[] = {
+		MOTIONSENSE_TYPE_GYRO,
+		MOTIONSENSE_TYPE_ACCEL,
+	};
+
+	/*
+	 * FIFO pattern is as follow (i.e. Acc/Gyro @ same ODR)
+	 *  ________ ____________ _______ ____________
+	 * | TAG_XL | Acc[x,y,z] | TAG_G | Gyr[x,y,z] |
+	 * |________|____________|_______|____________|
+	 * |<-------- 1 -------->|<-------- 2 ------->| (FIFO Threshold)
+	 *
+	 * First byte is tag, next data.
+	 * Data pattern len is fixed for each sample.
+	 * FIFO threshold is related to sample data (7 byte).
+	 */
+	ptr = fifo + LSM6DSO_TAG_SIZE;
+	tag = (*fifo >> 3) - LSM6DSO_GYRO_TAG;
+	id = ag_maps[tag];
+
+	/* Discard samples every ODR changes. */
+	if (samples_to_discard[id] > 0) {
+		samples_to_discard[id]--;
+		return;
+	}
+
+	sensor = main_s + id;
+	axis = sensor->raw_xyz;
+
+	/* Apply precision, sensitivity and rotation. */
+	st_normalize(sensor, axis, ptr);
+	vect.data[X] = axis[X];
+	vect.data[Y] = axis[Y];
+	vect.data[Z] = axis[Z];
+
+	vect.flags = 0;
+	vect.sensor_num = sensor - motion_sensors;
+	motion_sense_fifo_add_data(&vect, sensor, 3, saved_ts);
+}
+
+static inline int load_fifo(struct motion_sensor_t *s,
+			    const struct lsm6dso_fstatus *fsts,
+			    uint32_t saved_ts)
+{
+	uint8_t fifo[FIFO_READ_LEN], *ptr;
+	int i, err, read_len = 0, word_len, fifo_len;
+	uint16_t fifo_depth;
+
+	fifo_depth = fsts->len & LSM6DSO_FIFO_DIFF_MASK;
+	fifo_len = fifo_depth * LSM6DSO_FIFO_SAMPLE_SIZE;
+	while (read_len < fifo_len) {
+		word_len = GENERIC_MIN(fifo_len - read_len, sizeof(fifo));
+		err = st_raw_read_n_noinc(s->port, s->addr,
+					  LSM6DSO_FIFO_DATA_ADDR_TAG,
+					  fifo, word_len);
+		if (err != EC_SUCCESS)
+			return err;
+
+		for (i = 0; i < word_len; i += LSM6DSO_FIFO_SAMPLE_SIZE) {
+			ptr = &fifo[i];
+			push_fifo_data(LSM6DSO_MAIN_SENSOR(s), ptr, saved_ts);
+		}
+		read_len += word_len;
+	}
+
+	return read_len;
+}
+
+/**
+ * accelgyro_config_fifo - update mode and ODR for FIFO decimator
+ */
+static int accelgyro_config_fifo(const struct motion_sensor_t *s)
+{
+	int err;
+	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
+	uint8_t reg_val;
+	uint8_t fifo_odr_mask;
+
+	/* Changing in ODR must stop FIFO. */
+	err = fifo_disable(s);
+	if (err != EC_SUCCESS)
+		return err;
+
+	/*
+	 * If ODR changes restore to default discard samples number
+	 * the counter related to this sensor.
+	 */
+	samples_to_discard[s->type] = LSM6DSO_DISCARD_SAMPLES;
+
+	fifo_odr_mask = LSM6DSO_FIFO_ODR_TO_REG(s);
+	reg_val = LSM6DSO_ODR_TO_REG(data->base.odr);
+	err = st_write_data_with_mask(s, LSM6DSO_FIFO_CTRL3_ADDR,
+				      fifo_odr_mask, reg_val);
+	if (err != EC_SUCCESS)
+		return err;
+
+	return fifo_enable(s);
+}
+#endif /* CONFIG_ACCEL_FIFO */
+
+/**
+ * lsm6dso_interrupt - interrupt from int1 pin of sensor
+ */
+void lsm6dso_interrupt(enum gpio_signal signal)
+{
+#ifdef CONFIG_ACCEL_FIFO
+	last_interrupt_timestamp = __hw_clock_source_read();
+#endif /* CONFIG_ACCEL_FIFO */
+
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCEL_LSM6DSO_INT_EVENT, 0);
+}
+
+/**
+ * irq_handler - bottom half of the interrupt task sheduled by consumer
+ */
+static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	int ret = EC_SUCCESS;
+	struct lsm6dso_fstatus fsts;
+
+	if (((s->type != MOTIONSENSE_TYPE_ACCEL) &&
+	     (s->type != MOTIONSENSE_TYPE_GYRO)) ||
+	    (!(*event & CONFIG_ACCEL_LSM6DSO_INT_EVENT)))
+		return EC_ERROR_NOT_HANDLED;
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* Read how many data patterns on FIFO to read. */
+	ret = st_raw_read_n_noinc(s->port, s->addr,
+				  LSM6DSO_FIFO_STS1_ADDR,
+				  (uint8_t *)&fsts, sizeof(fsts));
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	if (fsts.len & (LSM6DSO_FIFO_DATA_OVR | LSM6DSO_FIFO_FULL)) {
+		CPRINTS("%s FIFO Overrun: %04x\n", s->name, fsts.len);
+	}
+
+	if (fsts.len & LSM6DSO_FIFO_DIFF_MASK)
+		ret = load_fifo(s, &fsts, last_interrupt_timestamp);
+#endif /* CONFIG_ACCEL_FIFO */
+
+	return ret;
+}
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+
+/**
+ * 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 stprivate_data *data = LSM6DSO_GET_DATA(s);
+	int newrange = range;
+
+	ctrl_reg = LSM6DSO_RANGE_REG(s->type);
+	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
+		/* Adjust and check rounded value for Acc. */
+		if (rnd && (newrange < LSM6DSO_ACCEL_NORMALIZE_FS(newrange)))
+			newrange *= 2;
+
+		if (newrange > LSM6DSO_ACCEL_FS_MAX_VAL)
+			newrange = LSM6DSO_ACCEL_FS_MAX_VAL;
+
+		reg_val = lsm6dso_accel_fs_reg(newrange);
+	} else {
+		/* Adjust and check rounded value for Gyro. */
+		reg_val = LSM6DSO_GYRO_FS_REG(range);
+		if (rnd && (range > LSM6DSO_GYRO_NORMALIZE_FS(reg_val)))
+			reg_val++;
+
+		if (reg_val > LSM6DSO_GYRO_FS_MAX_REG_VAL)
+			reg_val = LSM6DSO_GYRO_FS_MAX_REG_VAL;
+
+		newrange = LSM6DSO_GYRO_NORMALIZE_FS(reg_val);
+	}
+
+	mutex_lock(s->mutex);
+	err = st_write_data_with_mask(s, ctrl_reg, LSM6DSO_RANGE_MASK,
+				      reg_val);
+	if (err == EC_SUCCESS)
+		data->base.range = newrange;
+
+	mutex_unlock(s->mutex);
+
+	return EC_SUCCESS;
+}
+
+/**
+ * get_range - get full scale range
+ * @s: Motion sensor pointer
+ */
+static int get_range(const struct motion_sensor_t *s)
+{
+	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
+
+	return data->base.range;
+}
+
+/**
+ * set_data_rate set sensor data rate
+ * @s: Motion sensor pointer
+ * @range: Rate (mHz)
+ * @rnd: Round up/down flag
+ */
+static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
+{
+	int ret, normalized_rate = 0;
+	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
+	uint8_t ctrl_reg, reg_val = 0;
+
+	ctrl_reg = LSM6DSO_ODR_REG(s->type);
+	if (rate > 0) {
+		reg_val = LSM6DSO_ODR_TO_REG(rate);
+		normalized_rate = LSM6DSO_REG_TO_ODR(reg_val);
+
+		if (rnd && (normalized_rate < rate)) {
+			reg_val++;
+			normalized_rate = LSM6DSO_REG_TO_ODR(reg_val);
+		}
+
+		if (normalized_rate < LSM6DSO_ODR_MIN_VAL ||
+		    normalized_rate > MIN(LSM6DSO_ODR_MAX_VAL,
+					  CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ))
+			return EC_RES_INVALID_PARAM;
+	}
+
+	mutex_lock(s->mutex);
+	ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSO_ODR_MASK, reg_val);
+	if (ret == EC_SUCCESS) {
+		data->base.odr = normalized_rate;
+#ifdef CONFIG_ACCEL_FIFO
+		accelgyro_config_fifo(s);
+#endif /* CONFIG_ACCEL_FIFO */
+	}
+
+	mutex_unlock(s->mutex);
+
+	return ret;
+}
+
+static int is_data_ready(const struct motion_sensor_t *s, int *ready)
+{
+	int ret, tmp;
+
+	ret = st_raw_read8(s->port, s->addr, LSM6DSO_STATUS_REG, &tmp);
+	if (ret != EC_SUCCESS) {
+		CPRINTS("%s type:0x%X RS Error", s->name, s->type);
+
+		return ret;
+	}
+
+	if (MOTIONSENSE_TYPE_ACCEL == s->type)
+		*ready = (LSM6DSO_STS_XLDA_UP == (tmp & LSM6DSO_STS_XLDA_MASK));
+	else
+		*ready = (LSM6DSO_STS_GDA_UP == (tmp & LSM6DSO_STS_GDA_MASK));
+
+	return EC_SUCCESS;
+}
+
+/*
+ * Is not very efficient to collect the data in read: better have an interrupt
+ * and collect in FIFO, even if it has one item: we don't have to check if the
+ * sensor is ready (minimize I2C access).
+ */
+static int read(const struct motion_sensor_t *s, intv3_t v)
+{
+	uint8_t raw[OUT_XYZ_SIZE];
+	uint8_t xyz_reg;
+	int ret, tmp = 0;
+
+	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;
+	}
+
+	xyz_reg = get_xyz_reg(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;
+
+	/* Apply precision, sensitivity and rotation vector. */
+	st_normalize(s, v, raw);
+
+	return EC_SUCCESS;
+}
+
+static int init(const struct motion_sensor_t *s)
+{
+	int ret = 0, tmp;
+	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
+
+	ret = st_raw_read8(s->port, s->addr, LSM6DSO_WHO_AM_I_REG, &tmp);
+	if (ret != EC_SUCCESS)
+		return EC_ERROR_UNKNOWN;
+
+	if (tmp != LSM6DSO_WHO_AM_I)
+		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
+	 * LSM6DSO supports both Acc & Gyro features
+	 * Board will see two virtual sensor devices: Acc & Gyro
+	 * Requirement: Acc need be init before Gyro
+	 */
+	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
+		mutex_lock(s->mutex);
+
+		/* Software reset. */
+		ret = st_raw_write8(s->port, s->addr, LSM6DSO_CTRL3_ADDR,
+				    LSM6DSO_SW_RESET);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+
+		/*
+		 * Output data not updated until have been read.
+		 * Prefer interrupt to be active low.
+		 */
+		ret = st_raw_write8(s->port, s->addr, LSM6DSO_CTRL3_ADDR,
+				    LSM6DSO_BDU | LSM6DSO_IF_INC);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+
+#ifdef CONFIG_ACCEL_FIFO
+		ret = fifo_disable(s);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+#endif /* CONFIG_ACCEL_FIFO */
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+		ret = config_interrupt(s);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+
+		mutex_unlock(s->mutex);
+	}
+
+	/* Set default resolution common to Acc and Gyro. */
+	data->resol = LSM6DSO_RESOLUTION;
+	return sensor_init_done(s);
+
+err_unlock:
+	mutex_unlock(s->mutex);
+	CPRINTS("%s: MS Init type:0x%X Error\n", s->name, s->type);
+
+	return ret;
+}
+
+const struct accelgyro_drv lsm6dso_drv = {
+	.init = init,
+	.read = read,
+	.set_range = set_range,
+	.get_range = get_range,
+	.get_resolution = st_get_resolution,
+	.set_data_rate = set_data_rate,
+	.get_data_rate = st_get_data_rate,
+	.set_offset = st_set_offset,
+	.get_offset = st_get_offset,
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.irq_handler = irq_handler,
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+};
diff --git a/driver/accelgyro_lsm6dso.h b/driver/accelgyro_lsm6dso.h
new file mode 100644
index 0000000000..7748c7fe94
--- /dev/null
+++ b/driver/accelgyro_lsm6dso.h
@@ -0,0 +1,233 @@
+/* Copyright 2019 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.
+ */
+
+/* LSM6DSO Accel and Gyro driver for Chrome EC */
+
+#ifndef __CROS_EC_ACCELGYRO_LSM6DSO_H
+#define __CROS_EC_ACCELGYRO_LSM6DSO_H
+
+#include "stm_mems_common.h"
+
+#define LSM6DSO_I2C_ADDR(__x)		(__x << 1)
+
+/*
+ * 7-bit address is 110101xb. Where 'x' is determined
+ * by the voltage on the ADDR pin
+ */
+#define LSM6DSO_ADDR0			LSM6DSO_I2C_ADDR(0x6a)
+#define LSM6DSO_ADDR1			LSM6DSO_I2C_ADDR(0x6b)
+
+/* Access to embedded sensor hub register bank */
+#define LSM6DSO_FUNC_CFG_ACC_ADDR	0x01
+#define LSM6DSO_FUNC_CFG_EN			0x80
+
+/* Who Am I */
+#define LSM6DSO_WHO_AM_I_REG		0x0f
+#define LSM6DSO_WHO_AM_I		0x6c
+
+/* Common defines for Acc and Gyro sensors */
+#define LSM6DSO_EN_BIT				0x01
+#define LSM6DSO_DIS_BIT				0x00
+
+#define LSM6DSO_GYRO_OUT_X_L_ADDR	0x22
+#define LSM6DSO_ACCEL_OUT_X_L_ADDR	0x28
+
+#define LSM6DSO_CTRL1_ADDR		0x10
+#define LSM6DSO_CTRL2_ADDR		0x11
+#define LSM6DSO_CTRL3_ADDR		0x12
+#define LSM6DSO_SW_RESET			0x01
+#define LSM6DSO_IF_INC				0x04
+#define LSM6DSO_PP_OD				0x10
+#define LSM6DSO_H_L_ACTIVE			0x20
+#define LSM6DSO_BDU				0x40
+
+#define LSM6DSO_CTRL4_ADDR		0x13
+#define LSM6DSO_INT2_ON_INT1_MASK		0x20
+
+#define LSM6DSO_CTRL5_ADDR		0x14
+#define LSM6DSO_CTRL6_ADDR		0x15
+#define LSM6DSO_CTRL7_ADDR		0x16
+#define LSM6DSO_CTRL8_ADDR		0x17
+#define LSM6DSO_CTRL9_ADDR		0x18
+
+#define LSM6DSO_CTRL10_ADDR		0x19
+#define LSM6DSO_TIMESTAMP_EN    		0x20
+
+#define LSM6DSO_STATUS_REG		0x1e
+
+/* Output data rate registers and masks */
+#define LSM6DSO_ODR_REG(_sensor) \
+	(LSM6DSO_CTRL1_ADDR + (_sensor))
+#define LSM6DSO_ODR_MASK			0xf0
+
+/* Hardware FIFO size in byte */
+#define LSM6DSO_MAX_FIFO_SIZE		4096
+#define LSM6DSO_MAX_FIFO_LENGTH	(LSM6DSO_MAX_FIFO_SIZE / OUT_XYZ_SIZE)
+
+/* FIFO decimator registers and bitmask */
+#define LSM6DSO_FIFO_CTRL1_ADDR		0x07
+#define LSM6DSO_FIFO_CTRL2_ADDR		0x08
+
+#define LSM6DSO_FIFO_CTRL3_ADDR		0x09
+#define LSM6DSO_FIFO_ODR_XL_MASK		0x0f
+#define LSM6DSO_FIFO_ODR_G_MASK			0xf0
+
+#define LSM6DSO_FIFO_CTRL4_ADDR		0x0a
+#define LSM6DSO_FIFO_MODE_MASK			0x07
+
+#define LSM6DSO_INT1_CTRL		0x0d
+#define LSM6DSO_INT2_CTRL		0x0e
+#define LSM6DSO_INT_FIFO_TH			0x08
+#define LSM6DSO_INT_FIFO_OVR			0x10
+#define LSM6DSO_INT_FIFO_FULL			0x20
+
+#define LSM6DSO_FIFO_STS1_ADDR		0x3a
+#define LSM6DSO_FIFO_STS2_ADDR		0x3b
+#define LSM6DSO_FIFO_DIFF_MASK			0x07ff
+#define LSM6DSO_FIFO_FULL			0x2000
+#define LSM6DSO_FIFO_DATA_OVR			0x4000
+#define LSM6DSO_FIFO_WATERMARK			0x8000
+
+/* Out FIFO data register */
+#define LSM6DSO_FIFO_DATA_ADDR_TAG	0x78
+
+/* Registers value for supported FIFO mode */
+#define LSM6DSO_FIFO_MODE_BYPASS_VAL	0x00
+#define LSM6DSO_FIFO_MODE_CONTINUOUS_VAL	0x06
+
+/* Define device available in FIFO pattern */
+enum lsm6dso_dev_fifo {
+	LSM6DSO_FIFO_DEV_INVALID = -1,
+	LSM6DSO_FIFO_DEV_GYRO = 0,
+	LSM6DSO_FIFO_DEV_ACCEL,
+	LSM6DSO_FIFO_DEV_NUM,
+};
+
+/* Define FIFO data pattern, tag and len */
+#define LSM6DSO_SAMPLE_SIZE		6
+#define LSM6DSO_TS_SAMPLE_SIZE		4
+#define LSM6DSO_TAG_SIZE		1
+#define LSM6DSO_FIFO_SAMPLE_SIZE	LSM6DSO_SAMPLE_SIZE + LSM6DSO_TAG_SIZE
+#define LSM6DSO_MAX_FIFO_DEPTH		416
+
+enum lsm6dso_tag_fifo {
+	LSM6DSO_GYRO_TAG = 0x01,
+	LSM6DSO_ACC_TAG = 0x02,
+};
+
+struct lsm6dso_fstatus {
+	uint16_t len;
+	uint16_t pattern;
+};
+
+/* Absolute maximum rate for Acc and Gyro sensors */
+#define LSM6DSO_ODR_MIN_VAL		13000
+#define LSM6DSO_ODR_MAX_VAL \
+	MOTION_MAX_SENSOR_FREQUENCY(416000, 13000)
+
+/* ODR reg value from selected data rate in mHz */
+#define LSM6DSO_ODR_TO_REG(_odr) (__fls(_odr / LSM6DSO_ODR_MIN_VAL) + 1)
+
+#define LSM6DSO_FIFO_ODR_TO_REG(_s) \
+	(_s->type == MOTIONSENSE_TYPE_ACCEL ? LSM6DSO_FIFO_ODR_XL_MASK : \
+	 LSM6DSO_FIFO_ODR_G_MASK)
+
+/* Normalized ODR values from selected data rate in mHz */
+#define LSM6DSO_REG_TO_ODR(_reg) (LSM6DSO_ODR_MIN_VAL << (_reg - 1))
+
+/* Full Scale ranges value and gain for Acc */
+#define LSM6DSO_FS_LIST_NUM		4
+
+#define LSM6DSO_ACCEL_FS_ADDR		0x10
+#define LSM6DSO_ACCEL_FS_MASK			0x0c
+
+#define LSM6DSO_ACCEL_FS_2G_VAL		0x00
+#define LSM6DSO_ACCEL_FS_4G_VAL		0x02
+#define LSM6DSO_ACCEL_FS_8G_VAL		0x03
+#define LSM6DSO_ACCEL_FS_16G_VAL	0x01
+
+#define LSM6DSO_ACCEL_FS_MAX_VAL	16
+
+/* Accel reg value from Full Scale range */
+static inline uint8_t lsm6dso_accel_fs_reg(int fs)
+{
+	uint8_t ret;
+
+	switch(fs) {
+	case 2:
+		ret = LSM6DSO_ACCEL_FS_2G_VAL;
+		break;
+	case 16:
+		ret = LSM6DSO_ACCEL_FS_16G_VAL;
+		break;
+	default:
+		ret = __fls(fs);
+		break;
+	}
+
+	return ret;
+}
+
+/* Accel normalized FS value from Full Scale */
+#define LSM6DSO_ACCEL_NORMALIZE_FS(_fs) (1 << __fls(_fs))
+
+/* Full Scale range value and gain for Gyro */
+#define LSM6DSO_GYRO_FS_ADDR		0x11
+#define LSM6DSO_GYRO_FS_MASK			0x0c
+
+/* Minimal Gyro range in mDPS */
+#define LSM6DSO_GYRO_FS_MIN_VAL_MDPS ((8750 << 15) / 1000)
+#define LSM6DSO_GYRO_FS_MAX_REG_VAL 3
+
+/* Gyro reg value for Full Scale selection in DPS */
+#define LSM6DSO_GYRO_FS_REG(_fs) \
+	__fls(MAX(1, (_fs * 1000) / LSM6DSO_GYRO_FS_MIN_VAL_MDPS))
+
+/* Gyro normalized FS value (in DPS) from Full Scale register */
+#define LSM6DSO_GYRO_NORMALIZE_FS(_reg) \
+	((LSM6DSO_GYRO_FS_MIN_VAL_MDPS << (_reg)) / 1000)
+
+/* FS register address/mask for Acc/Gyro sensors */
+#define LSM6DSO_RANGE_REG(_sensor)  	(LSM6DSO_ACCEL_FS_ADDR + (_sensor))
+#define LSM6DSO_RANGE_MASK  			0x0c
+
+/* Status register bit for Acc/Gyro data ready */
+enum lsm6dso_status {
+	LSM6DSO_STS_DOWN = 0x00,
+	LSM6DSO_STS_XLDA_UP = 0x01,
+	LSM6DSO_STS_GDA_UP = 0x02
+};
+
+/* Status register bitmask for Acc/Gyro data ready */
+#define LSM6DSO_STS_XLDA_MASK			0x01
+#define LSM6DSO_STS_GDA_MASK			0x02
+
+/* Sensor resolution in number of bits: fixed 16 bit */
+#define LSM6DSO_RESOLUTION      	16
+
+/* Aggregate private data for all supported sensor (Acc, Gyro) */
+struct lsm6dso_data {
+	struct stprivate_data st_data[LSM6DSO_FIFO_DEV_NUM];
+};
+
+/*
+ * Note: The specific number of samples to discard depends on the filters
+ * configured for the chip, as well as the ODR being set. For most of our
+ * allowed ODRs, 3 should suffice.
+ * See: ST's LSM6DSO application notes (AN5192) Tables 12 and 18 for details
+ */
+#define LSM6DSO_DISCARD_SAMPLES 3
+
+#define LSM6DSO_GET_DATA(_s) ((struct stprivate_data *)((_s)->drv_data))
+
+/* Macro to initialize motion_sensors structure */
+#define LSM6DSO_ST_DATA(g, type) (&(&(g))->st_data[(type)])
+#define LSM6DSO_MAIN_SENSOR(_s) ((_s) - (_s)->type)
+
+extern const struct accelgyro_drv lsm6dso_drv;
+
+void lsm6dso_interrupt(enum gpio_signal signal);
+
+#endif /* __CROS_EC_ACCELGYRO_LSM6DSO_H */
diff --git a/driver/build.mk b/driver/build.mk
index 313a4fe6dd..714fae0449 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 stm_mems_common.o
+driver-$(CONFIG_ACCELGYRO_LSM6DSO)+=accelgyro_lsm6dso.o stm_mems_common.o
 driver-$(CONFIG_ACCEL_LIS2D_COMMON)+=accel_lis2dh.o stm_mems_common.o
 driver-$(CONFIG_MAG_LIS2MDL)+=mag_lis2mdl.o
 driver-$(CONFIG_SENSORHUB_LSM6DSM)+=sensorhub_lsm6dsm.o
diff --git a/include/config.h b/include/config.h
index 107a94b469..0ca81be60b 100644
--- a/include/config.h
+++ b/include/config.h
@@ -79,6 +79,8 @@
 #undef CONFIG_ACCELGYRO_LSM6DS0
 /* Use CONFIG_ACCELGYRO_LSM6DSM for LSM6DSL, LSM6DSM, and/or LSM6DS3 */
 #undef CONFIG_ACCELGYRO_LSM6DSM
+#undef CONFIG_ACCELGYRO_LSM6DSO
+
 /* Add sensorhub function for LSM6DSM, required if 2nd device attached. */
 #undef CONFIG_SENSORHUB_LSM6DSM
 
@@ -222,6 +224,7 @@
  */
 #undef CONFIG_ACCELGYRO_BMI160_INT_EVENT
 #undef CONFIG_ACCEL_LSM6DSM_INT_EVENT
+#undef CONFIG_ACCEL_LSM6DSO_INT_EVENT
 #undef CONFIG_ALS_SI114X_INT_EVENT
 
 /*
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 7e7b59fa1c..fb33d7179e 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -2433,6 +2433,7 @@ enum motionsensor_chip {
 	MOTIONSENSE_CHIP_LIS2DE = 15,
 	MOTIONSENSE_CHIP_LIS2MDL = 16,
 	MOTIONSENSE_CHIP_LSM6DS3 = 17,
+	MOTIONSENSE_CHIP_LSM6DSO = 18,
 	MOTIONSENSE_CHIP_MAX,
 };
 
diff --git a/util/ectool.c b/util/ectool.c
index cb0de928cb..36fd2be7b7 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -4344,6 +4344,9 @@ static int cmd_motionsense(int argc, char **argv)
 		case MOTIONSENSE_CHIP_LSM6DS3:
 			printf("lsm6ds3\n");
 			break;
+		case MOTIONSENSE_CHIP_LSM6DSO:
+			printf("lsm6dso\n");
+			break;
 		default:
 			printf("unknown\n");
 		}