driver/sensorhub_lsm6dsm: Add sensor hub support in LSM6DSM module

Add support for sensor hub in LSM6DSM accelerometer module so that
external sensors module like magnetometer can be supported.

BRANCH=none
BUG=b:115587004
TEST=Collect magnetometer readings using ectool motionsense

Change-Id: Id0fd4eea56b7106a89d55925ae488af6b0300119
Signed-off-by: Karthikeyan Ramasubramanian <kramasub@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1257503
Reviewed-by: Furquan Shaikh <furquan@chromium.org>
Reviewed-by: Jett Rink <jettrink@chromium.org>

5 files changed, 467 insertions, 0 deletions

diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index a85b460c33..afb669c077 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -19,6 +19,11 @@
 #define LSM6DSM_ADDR0			LSM6DSM_I2C_ADDR(0x6a)
 #define LSM6DSM_ADDR1			LSM6DSM_I2C_ADDR(0x6b)
 
+/* Access to embedded sensor hub register bank */
+#define LSM6DSM_FUNC_CFG_ACC_ADDR	0x01
+#define LSM6DSM_FUNC_CFG_EN			0x80
+#define LSM6DSM_FUNC_CFG_EN_B			0x20
+
 /* Who Am I */
 #define LSM6DSM_WHO_AM_I_REG		0x0f
 #define LSM6DSM_WHO_AM_I		0x6a
@@ -32,6 +37,8 @@
 #define LSM6DSM_ACCEL_OUT_X_L_ADDR	0x28
 
 #define LSM6DSM_CTRL1_ADDR		0x10
+#define LSM6DSM_XL_ODR_MASK			0xf0
+
 #define LSM6DSM_CTRL2_ADDR		0x11
 #define LSM6DSM_CTRL3_ADDR		0x12
 #define LSM6DSM_SW_RESET			0x01
@@ -46,6 +53,14 @@
 #define LSM6DSM_CTRL6_ADDR		0x15
 #define LSM6DSM_CTRL7_ADDR		0x16
 
+#define LSM6DSM_CTRL10_ADDR		0x19
+#define LSM6DSM_EMBED_FUNC_EN			0x4
+
+#define LSM6DSM_MASTER_CFG_ADDR		0x1a
+#define LSM6DSM_I2C_MASTER_ON			0x1
+#define LSM6DSM_I2C_PASS_THRU_MODE		0x4
+#define LSM6DSM_EXT_TRIGGER_EN			0x10
+
 #define LSM6DSM_STATUS_REG		0x1e
 
 /* Output data rate registers and masks */
@@ -102,6 +117,27 @@
  * Value is limited to 416 Hz
  */
 
+
+/* Register values for Sensor Hub Slave 0 */
+#define LSM6DSM_SLV0_ADD_ADDR		0x02
+#define LSM6DSM_SLV0_ADDR_SHFT			1
+#define LSM6DSM_SLV0_ADDR_MASK			0xfe
+#define LSM6DSM_SLV0_RD_BIT			0x01
+
+#define LSM6DSM_SLV0_SUBADD_ADDR	0x03
+
+#define LSM6DSM_SLV0_CONFIG_ADDR	0x04
+#define LSM6DSM_SLV0_SLV_RATE_SHFT		6
+#define LSM6DSM_SLV0_SLV_RATE_MASK		0xc0
+#define LSM6DSM_SLV0_AUX_SENS_SHFT		4
+#define LSM6DSM_SLV0_AUX_SENS_MASK		0x30
+#define LSM6DSM_SLV0_NUM_OPS_MASK		0x07
+
+#define LSM6DSM_SLV1_CONFIG_ADDR	0x07
+#define LSM6DSM_SLV0_WR_ONCE_MASK		0x20
+
+#define LSM6DSM_SENSORHUB1_REG		0x2e
+
 /* Registers value for sensor Hub */
 #define LSM6DSM_FUNC_SRC1		0x53
 #define LSM6DSM_SENSORHUB_END_OP	0x01
@@ -111,6 +147,7 @@ enum dev_fifo {
 	FIFO_DEV_INVALID = -1,
 	FIFO_DEV_GYRO = 0,
 	FIFO_DEV_ACCEL,
+	FIFO_DEV_MAG,
 	FIFO_DEV_NUM,
 };
 
diff --git a/driver/build.mk b/driver/build.mk
index 303800bc13..a9f266ba16 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -15,6 +15,7 @@ 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_ACCEL_LIS2D_COMMON)+=accel_lis2dh.o stm_mems_common.o
+driver-$(CONFIG_SENSORHUB_LSM6DSM)+=sensorhub_lsm6dsm.o
 driver-$(CONFIG_SYNC)+=sync.o
 
 # BC1.2 Charger Detection Devices
diff --git a/driver/sensorhub_lsm6dsm.c b/driver/sensorhub_lsm6dsm.c
new file mode 100644
index 0000000000..ba7229c06d
--- /dev/null
+++ b/driver/sensorhub_lsm6dsm.c
@@ -0,0 +1,345 @@
+/* Copyright 2018 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.
+ */
+
+/*
+ * Sensor Hub Driver for LSM6DSM acce/gyro module to enable connecting
+ * external sensors like magnetometer
+ */
+
+#include "console.h"
+#include "driver/accelgyro_lsm6dsm.h"
+#include "driver/sensorhub_lsm6dsm.h"
+#include "driver/stm_mems_common.h"
+
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+
+static int set_reg_bit_field(const struct motion_sensor_t *s,
+					uint8_t reg, uint8_t bit_field)
+{
+	int tmp;
+	int ret;
+
+	ret = st_raw_read8(s->port, s->addr, reg, &tmp);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	tmp |= bit_field;
+	return st_raw_write8(s->port, s->addr, reg, tmp);
+}
+
+static int clear_reg_bit_field(const struct motion_sensor_t *s,
+					uint8_t reg, uint8_t bit_field)
+{
+	int tmp;
+	int ret;
+
+	ret = st_raw_read8(s->port, s->addr, reg, &tmp);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	tmp &= ~(bit_field);
+	return st_raw_write8(s->port, s->addr, reg, tmp);
+}
+
+static inline int enable_sensorhub_func(const struct motion_sensor_t *s)
+{
+	return set_reg_bit_field(s, LSM6DSM_CTRL10_ADDR,
+						LSM6DSM_EMBED_FUNC_EN);
+}
+
+static inline int disable_sensorhub_func(const struct motion_sensor_t *s)
+{
+	return clear_reg_bit_field(s, LSM6DSM_CTRL10_ADDR,
+						LSM6DSM_EMBED_FUNC_EN);
+}
+
+/*
+ * Sensor hub includes embedded register banks associated with external
+ * sensors. 4 external sensor slaves can be attached to the sensor hub
+ * and hence 4 such register banks exist. The access to them are disabled
+ * by default. Below 2 helper functions help enable/disable access to those
+ * register banks.
+ */
+static inline int enable_ereg_bank_acc(const struct motion_sensor_t *s)
+{
+	return set_reg_bit_field(s, LSM6DSM_FUNC_CFG_ACC_ADDR,
+							LSM6DSM_FUNC_CFG_EN);
+}
+
+static inline int disable_ereg_bank_acc(const struct motion_sensor_t *s)
+{
+	return clear_reg_bit_field(s, LSM6DSM_FUNC_CFG_ACC_ADDR,
+							LSM6DSM_FUNC_CFG_EN);
+}
+
+static inline int enable_aux_i2c_master(const struct motion_sensor_t *s)
+{
+	return set_reg_bit_field(s, LSM6DSM_MASTER_CFG_ADDR,
+							LSM6DSM_I2C_MASTER_ON);
+}
+
+static inline int disable_aux_i2c_master(const struct motion_sensor_t *s)
+{
+	return clear_reg_bit_field(s, LSM6DSM_MASTER_CFG_ADDR,
+							LSM6DSM_I2C_MASTER_ON);
+}
+
+static inline int restore_master_cfg(const struct motion_sensor_t *s,
+								int cache)
+{
+	return st_raw_write8(s->port, s->addr, LSM6DSM_MASTER_CFG_ADDR, cache);
+}
+
+static int enable_i2c_pass_through(const struct motion_sensor_t *s,
+							int *cache)
+{
+	int ret;
+
+	ret = st_raw_read8(s->port, s->addr, LSM6DSM_MASTER_CFG_ADDR, cache);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x MCR error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	/*
+	 * Fake set sensor hub to external trigger event and wait for 10ms.
+	 * Wait is for any pending bus activity(probably read) to settle down
+	 * so that there is no bus contention.
+	 */
+	ret = st_raw_write8(s->port, s->addr, LSM6DSM_MASTER_CFG_ADDR,
+					*cache | LSM6DSM_EXT_TRIGGER_EN);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x MCETEN error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+	msleep(10);
+
+	ret = st_raw_write8(s->port, s->addr, LSM6DSM_MASTER_CFG_ADDR,
+		*cache & ~(LSM6DSM_EXT_TRIGGER_EN | LSM6DSM_I2C_MASTER_ON));
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x MCC error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		restore_master_cfg(s, *cache);
+		return ret;
+	}
+
+	return st_raw_write8(s->port, s->addr,
+			LSM6DSM_MASTER_CFG_ADDR, LSM6DSM_I2C_PASS_THRU_MODE);
+}
+
+static inline int power_down_accel(const struct motion_sensor_t *s,
+						int *cache)
+{
+	int ret;
+
+	ret = st_raw_read8(s->port, s->addr, LSM6DSM_CTRL1_ADDR, cache);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x CTRL1R error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	return st_raw_write8(s->port, s->addr, LSM6DSM_CTRL1_ADDR,
+					*cache & ~LSM6DSM_XL_ODR_MASK);
+}
+
+static inline int restore_ctrl1(const struct motion_sensor_t *s, int cache)
+{
+	return st_raw_write8(s->port, s->addr,
+				LSM6DSM_CTRL1_ADDR, cache);
+}
+
+static int config_slv0_read(const struct motion_sensor_t *s, uint8_t addr,
+						uint8_t reg, uint8_t len)
+{
+	int ret;
+
+	ret = st_raw_write8(s->port, s->addr, LSM6DSM_SLV0_ADD_ADDR,
+			(addr << LSM6DSM_SLV0_ADDR_SHFT) | LSM6DSM_SLV0_RD_BIT);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x SA error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	ret = st_raw_write8(s->port, s->addr, LSM6DSM_SLV0_SUBADD_ADDR, reg);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x RA error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	/*
+	 * No decimation for external sensor 0,
+	 * Number of sensors connected to external sensor hub 1
+	 */
+	ret = st_raw_write8(s->port, s->addr, LSM6DSM_SLV0_CONFIG_ADDR,
+					(len & LSM6DSM_SLV0_NUM_OPS_MASK));
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x CFG error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	return EC_SUCCESS;
+}
+
+int sensorhub_set_ext_data_rate(const struct motion_sensor_t *s,
+					int rate, int rnd, int *ret_rate)
+{
+	int xl_rate;
+	int ret = EC_SUCCESS;
+
+	if (!s || s->parent)
+		return EC_ERROR_INVAL;
+
+	xl_rate = st_get_data_rate(s);
+	*ret_rate = MIN(rate, xl_rate);
+#ifdef CONFIG_ACCEL_FIFO
+	ret = accelgyro_config_fifo(s);
+#endif
+	return ret;
+}
+
+int sensorhub_config_ext_reg(const struct motion_sensor_t *s,
+				uint8_t slv_addr, uint8_t reg, uint8_t val)
+{
+	int ret;
+	int tmp;
+
+	if (!s || s->parent)
+		return EC_ERROR_INVAL;
+
+	ret = enable_i2c_pass_through(s, &tmp);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x ENI2C error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	ret = st_raw_write8(s->port, slv_addr, reg, val);
+	restore_master_cfg(s, tmp);
+	return ret;
+}
+
+int sensorhub_config_slv0_read(const struct motion_sensor_t *s,
+			uint8_t slv_addr, uint8_t reg, int len)
+{
+	int tmp_xl_cfg;
+	int ret;
+
+	if (!s || s->parent)
+		return EC_ERROR_INVAL;
+
+	if (len <= 0 || len > OUT_XYZ_SIZE) {
+		CPRINTF("%s: %s type:0x%x Invalid length: %d\n",
+			__func__, s->name, s->type, len);
+		return EC_ERROR_INVAL;
+	}
+
+	ret = power_down_accel(s, &tmp_xl_cfg);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x PDXL error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	ret = enable_ereg_bank_acc(s);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x ENERB error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		goto out_restore_ctrl1;
+	}
+
+	ret = config_slv0_read(s, slv_addr, reg, len);
+	disable_ereg_bank_acc(s);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x CS0R error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		goto out_restore_ctrl1;
+	}
+
+	ret = enable_sensorhub_func(s);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x ENSH error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		goto out_restore_ctrl1;
+	}
+
+	ret = enable_aux_i2c_master(s);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x ENI2CM error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		disable_sensorhub_func(s);
+	}
+out_restore_ctrl1:
+	restore_ctrl1(s, tmp_xl_cfg);
+	return ret;
+}
+
+int sensorhub_slv0_data_read(const struct motion_sensor_t *s, intv3_t v)
+{
+	uint8_t raw[OUT_XYZ_SIZE];
+	int ret;
+
+	if (!s || s->parent)
+		return EC_ERROR_INVAL;
+
+	/*
+	 * Accel/Gyro is already reading slave 0 data into the sensorhub1
+	 * register as soon as the accel is in power-up mode. So return the
+	 * contents of that register.
+	 */
+	ret = st_raw_read_n_noinc(s->port, s->addr, LSM6DSM_SENSORHUB1_REG,
+							raw, OUT_XYZ_SIZE);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x SH1R error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	/* Apply precision, sensitivity and rotation vector */
+	st_normalize(s, v, raw);
+	return EC_SUCCESS;
+}
+
+int sensorhub_check_and_rst(const struct motion_sensor_t *s, uint8_t slv_addr,
+					uint8_t whoami_reg, uint8_t whoami_val,
+					uint8_t rst_reg, uint8_t rst_val)
+{
+	int ret, tmp;
+	int tmp_master_cfg;
+
+	if (!s || s->parent)
+		return EC_ERROR_INVAL;
+
+	ret = enable_i2c_pass_through(s, &tmp_master_cfg);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x ENI2C error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		return ret;
+	}
+
+	ret = st_raw_read8(s->port, slv_addr, whoami_reg, &tmp);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("%s: %s type:0x%x WAIR error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		goto err_restore_master_cfg;
+	}
+
+	if (tmp != whoami_val) {
+		CPRINTF("%s: %s type:0x%x WAIC error ret: %d\n",
+			__func__, s->name, s->type, ret);
+		ret = EC_ERROR_UNKNOWN;
+		goto err_restore_master_cfg;
+	}
+
+	ret = st_raw_write8(s->port, slv_addr, rst_reg, rst_val);
+err_restore_master_cfg:
+	restore_master_cfg(s, tmp_master_cfg);
+	return ret;
+}
diff --git a/driver/sensorhub_lsm6dsm.h b/driver/sensorhub_lsm6dsm.h
new file mode 100644
index 0000000000..6bb318e281
--- /dev/null
+++ b/driver/sensorhub_lsm6dsm.h
@@ -0,0 +1,83 @@
+/* Copyright 2018 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 Sensor Hub driver to enable interfacing with external sensors
+ * like magnetometer for Chrome EC
+ */
+
+#ifndef __CROS_EC_SENSORHUB_LSM6DSM_H
+#define __CROS_EC_SENSORHUB_LSM6DSM_H
+
+#include "common.h"
+#include "motion_sense.h"
+
+/**
+ * Set the output data rate of the external sensor that is attached
+ * to the sensor hub to the requested rate.
+ *
+ * @param s Pointer to external motion sensor's data structure.
+ * @param rate Preferred output data rate from the sensor.
+ * @param rnd Flag to indicate rounding to the nearest supported
+ *            output data rate by the sensor.
+ * @param ret_rate Rate configured by the sensor hub depending on the
+ *                 accelerometer data rate.
+ * @return EC_SUCCESS on success, EC error codes on failure.
+ */
+int sensorhub_set_ext_data_rate(const struct motion_sensor_t *s,
+					int rate, int rnd, int *ret_rate);
+
+/**
+ * Configure the register of an external sensor that is attached to sensor
+ * hub with a specific value.
+ *
+ * @param s Pointer to external motion sensor's data structure.
+ * @param slv_addr I2C Slave Address of the external sensor.
+ * @param reg Register Address to write within the external sensor.
+ * @param val Value to be written into the external sensor register.
+ * @return EC_SUCCESS on success, EC error codes on failure.
+ */
+int sensorhub_config_ext_reg(const struct motion_sensor_t *s,
+				uint8_t slv_addr, uint8_t reg, uint8_t val);
+
+/**
+ * Configure the sensor hub to read data from a specific register of an
+ * external sensor that is attached to it.
+ *
+ * @param s Pointer to external motion sensor's data structure.
+ * @param slv_addr I2C Slave Address of the external sensor.
+ * @param reg Register Address to read from the external sensor.
+ * @param len Length of data to be read.
+ * @return EC_SUCCESS on success, EC error codes on failure.
+ */
+int sensorhub_config_slv0_read(const struct motion_sensor_t *s,
+				uint8_t slv_addr, uint8_t reg, int len);
+
+/**
+ * Reads the data from the register bank that is associated with the slave0
+ * of the sensor hub.
+ *
+ * @param s Pointer to external motion sensor's data structure.
+ * @param v Vector to hold the data from the external sensor.
+ * @return EC_SUCCESS on success, EC error codes on failure.
+ */
+int sensorhub_slv0_data_read(const struct motion_sensor_t *s, intv3_t v);
+
+/**
+ * Check the identity of the external sensor and then reset the external
+ * sensor that is attached to the sensor hub.
+ *
+ * @param s Pointer to external motion sensor's data structure.
+ * @param slv_addr I2C Slave Address of the external sensor.
+ * @param whoami_reg Register address to identify the sensor.
+ * @param whoami_val Expected value to be read from the whoami_reg.
+ * @param rst_reg Register address to reset the external sensor.
+ * @param rst_val Value to be written to the reset register.
+ * @return EC_SUCCESS on success, EC error codes on failure.
+ */
+int sensorhub_check_and_rst(const struct motion_sensor_t *s, uint8_t slv_addr,
+					uint8_t whoami_reg, uint8_t whoami_val,
+					uint8_t rst_reg, uint8_t rst_val);
+#endif /* __CROS_EC_SENSORHUB_LSM6DSM_H */
diff --git a/include/config.h b/include/config.h
index eb40d5e48c..b4f0d4fbf6 100644
--- a/include/config.h
+++ b/include/config.h
@@ -78,6 +78,7 @@
 #undef CONFIG_ACCELGYRO_LSM6DS0
 #undef CONFIG_ACCELGYRO_BMI160
 #undef CONFIG_ACCELGYRO_LSM6DSM
+#undef CONFIG_SENSORHUB_LSM6DSM
 
 /* Support for BMI160 hardware orientation sensor */
 #undef CONFIG_BMI160_ORIENTATION_SENSOR