driver: lis2dw12: Add driver support

Added ACC LIS2DW12 driver support.
Features included in this driver are:
   - Basic Sensor Read acceleration data
   - ODR and FS runtime configuration
   - FIFO support with watermark interrupt events
   - Shared commons function with ST MEMs devices
   - Switch Low Power to High perf. mode in case
     of ODR > 200 Hz
   - Configure D-TAP event detection in Hardware

BUG=b:73546254
BRANCH=master
TEST=Tested on discovery_stmems target BOARD with
LIS2DW12 connected to EC i2c master bus and motion
sense task running.

To build firmware for discovery_stmems target with
LIS2DW12 sensor connected, simply uncomment
CONFIG_ACCEL_LIS2DW12 define in board.h target file
and make with target BOARD=discovery_stmems.

Commands used to test LIS2DW12 device are:
   - accelinit 0 (to configure accel. device)

All basic features tested, including changing
in ODR:
   - accelrate 0 10000 (set ODR to 10 Hz)
   - accelrate 0 12500 (set ODR to 12.5 Hz)
   - accelrate 0 25000 (set ODR to 25 Hz)
   - accelrate 0 50000 (set ODR to 50 Hz)
   - accelrate 0 100000 (set ODR to 100 Hz)
   - accelrate 0 200000 (set ODR to 200 Hz)
   - accelrate 0 400000 (set ODR to 400 Hz)
   - accelrate 0 800000 (set ODR to 800 Hz)
   - accelrate 0 1600000 (set ODR to 1.6 kHz)

Full Scale Range:
   - accelrange 0 2 (set Full Scale Range to 2g)
   - accelrange 0 4 (set Full Scale Range to 4g)
   - accelrange 0 8 (set Full Scale Range to 8g)
   - accelrange 0 16 (set Full Scale Range to 16g)

FIFO features and interrupt management:
   - accelinfo on 1000 (motion info task with
                        refresh rate 1 s)

and polling data read:
   - accelread 0 (to read data from accelerometer)

Change-Id: I0b9861a71e81052e7ee8eb235a1a5b2a57d4c6f5
Signed-off-by: mario tesi <mario.tesi@st.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/515302
Reviewed-by: Yuval Peress <peress@chromium.org>
Reviewed-by: Paul Ma <magf@bitland.corp-partner.google.com>
Tested-by: Paul Ma <magf@bitland.corp-partner.google.com>
Commit-Queue: Tim Wawrzynczak <twawrzynczak@chromium.org>

1 files changed, 547 insertions, 0 deletions

diff --git a/driver/accel_lis2dw12.c b/driver/accel_lis2dw12.c
new file mode 100644
index 0000000000..7faed478c2
--- /dev/null
+++ b/driver/accel_lis2dw12.c
@@ -0,0 +1,547 @@
+/* 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.
+ */
+
+/*
+ * LIS2DW12 accelerometer module for Chrome EC 3D digital accelerometer.
+ * For more details on LIS2DW12 device please refers to www.st.com.
+ */
+#include "accelgyro.h"
+#include "common.h"
+#include "console.h"
+#include "driver/accel_lis2dw12.h"
+#include "hooks.h"
+#include "hwtimer.h"
+#include "math_util.h"
+#include "task.h"
+#include "util.h"
+
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+
+#ifdef CONFIG_ACCEL_FIFO
+static volatile uint32_t last_interrupt_timestamp;
+
+/**
+ * lis2dw12_enable_fifo - Enable/Disable FIFO in LIS2DW12
+ * @s: Motion sensor pointer
+ * @mode: fifo_modes
+ */
+static int lis2dw12_enable_fifo(const struct motion_sensor_t *s,
+				enum lis2dw12_fmode mode)
+{
+	return st_write_data_with_mask(s, LIS2DW12_FIFO_CTRL_ADDR,
+				       LIS2DW12_FIFO_MODE_MASK, mode);
+}
+
+/**
+ * Load data from internal sensor FIFO.
+ * @s: Motion sensor pointer
+ */
+static int lis2dw12_load_fifo(struct motion_sensor_t *s, int nsamples,
+			      uint32_t *last_fifo_read_ts)
+{
+	int ret, left, length, i;
+	struct ec_response_motion_sensor_data vect;
+	uint32_t interrupt_timestamp = last_interrupt_timestamp;
+	int *axis = s->raw_xyz;
+	uint8_t fifo[FIFO_READ_LEN];
+
+	/* Each sample are OUT_XYZ_SIZE bytes. */
+	left = nsamples * OUT_XYZ_SIZE;
+
+	do {
+		/*
+		 * Limit FIFO read data to burst of FIFO_READ_LEN size because
+		 * read operatios in under i2c mutex lock.
+		 */
+		if (left > FIFO_READ_LEN)
+			length = FIFO_READ_LEN;
+		else
+			length = left;
+
+		ret = st_raw_read_n(s->port, s->i2c_spi_addr_flags,
+				    LIS2DW12_OUT_X_L_ADDR, fifo, length);
+		*last_fifo_read_ts = __hw_clock_source_read();
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		for (i = 0; i < length; i += OUT_XYZ_SIZE) {
+			/* Apply precision, sensitivity and rotation vector. */
+			st_normalize(s, axis, &fifo[i]);
+
+			/* Fill vector array. */
+			vect.data[X] = axis[X];
+			vect.data[Y] = axis[Y];
+			vect.data[Z] = axis[Z];
+			vect.flags = 0;
+			vect.sensor_num = 0;
+			motion_sense_fifo_stage_data(&vect, s, 3,
+						     interrupt_timestamp);
+		}
+		left -= length;
+	} while (left > 0);
+
+	motion_sense_fifo_commit_data();
+
+	return EC_SUCCESS;
+}
+
+/**
+ * lis2dw12_get_fifo_samples - check for stored FIFO samples.
+ */
+static int lis2dw12_get_fifo_samples(struct motion_sensor_t *s, int *nsamples)
+{
+	int ret, tmp;
+
+	ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
+			   LIS2DW12_FIFO_SAMPLES_ADDR, &tmp);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	*nsamples = tmp & LIS2DW12_FIFO_DIFF_MASK;
+
+	return EC_SUCCESS;
+}
+
+static int fifo_data_avail(struct motion_sensor_t *s)
+{
+	int ret, nsamples;
+
+	if (s->flags & MOTIONSENSE_FLAG_INT_SIGNAL)
+		return gpio_get_level(s->int_signal);
+
+	ret = lis2dw12_get_fifo_samples(s, &nsamples);
+	/* If we failed to read the FIFO size assume empty. */
+	if (ret != EC_SUCCESS)
+		return 0;
+	return nsamples;
+}
+#endif  /* CONFIG_ACCEL_FIFO */
+
+/**
+ * lis2dw12_config_interrupt- Configure interrupt for supported features.
+ * @s: Motion sensor pointer
+ *
+ * Must works with interface mutex locked
+ */
+static int lis2dw12_config_interrupt(const struct motion_sensor_t *s)
+{
+	int ret = EC_SUCCESS;
+
+#ifdef CONFIG_ACCEL_FIFO_THRES
+	/* Configure FIFO watermark level. */
+	ret = st_write_data_with_mask(s, LIS2DW12_FIFO_CTRL_ADDR,
+				      LIS2DW12_FIFO_THRESHOLD_MASK, 1);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* Enable interrupt on FIFO watermask and route to int1. */
+	ret = st_write_data_with_mask(s, LIS2DW12_INT1_FTH_ADDR,
+				      LIS2DW12_INT1_FTH_MASK, LIS2DW12_EN_BIT);
+	if (ret != EC_SUCCESS)
+		return ret;
+#endif /* CONFIG_ACCEL_FIFO */
+
+#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
+	/*
+	 * Configure D-TAP event detection on 3 axis.
+	 * For more details please refer to AN5038.
+	 */
+	ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
+			    LIS2DW12_TAP_THS_X_ADDR, 0x09);
+	if (ret != EC_SUCCESS)
+		return ret;
+	ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
+			    LIS2DW12_TAP_THS_Y_ADDR, 0x09);
+	if (ret != EC_SUCCESS)
+		return ret;
+	ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
+			    LIS2DW12_TAP_THS_Z_ADDR, 0xE9);
+	if (ret != EC_SUCCESS)
+		return ret;
+	ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
+			    LIS2DW12_INT_DUR_ADDR, 0x7F);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* Enable D-TAP event detection. */
+	ret = st_write_data_with_mask(s, LIS2DW12_WAKE_UP_THS_ADDR,
+				      LIS2DW12_SINGLE_DOUBLE_TAP,
+				      LIS2DW12_EN_BIT);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/*
+	 * Enable D-TAP detection on int_1 pad. In any case D-TAP event
+	 * can be detected only if ODR is over 200 Hz.
+	 */
+	ret = st_write_data_with_mask(s, LIS2DW12_INT1_TAP_ADDR,
+				      LIS2DW12_INT1_DTAP_MASK,
+				      LIS2DW12_EN_BIT);
+#endif /* CONFIG_GESTURE_SENSOR_BATTERY_TAP */
+	return ret;
+}
+
+static void lis2dw12_handle_interrupt_for_fifo(uint32_t ts)
+{
+#ifdef CONFIG_ACCEL_FIFO
+	if (time_after(ts, last_interrupt_timestamp))
+		last_interrupt_timestamp = ts;
+#endif
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCEL_LIS2DW12_INT_EVENT, 0);
+}
+
+/**
+ * lis2dw12_interrupt - interrupt from int pin of sensor
+ * Schedule Motion Sense Task to manage Interrupts.
+ */
+void lis2dw12_interrupt(enum gpio_signal signal)
+{
+	lis2dw12_handle_interrupt_for_fifo(__hw_clock_source_read());
+}
+
+/**
+ * lis2dw12_irq_handler - bottom half of the interrupt stack.
+ */
+static int lis2dw12_irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	int ret = EC_SUCCESS;
+
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+	    (!(*event & CONFIG_ACCEL_LIS2DW12_INT_EVENT))) {
+		return EC_ERROR_NOT_HANDLED;
+	}
+
+#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
+	{
+		int status = 0;
+
+		/* Read Status register to check TAP events. */
+		st_raw_read8(s->port, s->i2c_spi_addr_flags,
+			     LIS2DW12_STATUS_TAP, &status);
+		if (status & LIS2DW12_DOUBLE_TAP)
+			*event |= CONFIG_GESTURE_TAP_EVENT;
+	}
+#endif /* CONFIG_GESTURE_SENSOR_BATTERY_TAP */
+
+#ifdef CONFIG_ACCEL_FIFO
+	{
+		int nsamples;
+		uint32_t last_fifo_read_ts;
+		uint32_t triggering_interrupt_timestamp =
+			last_interrupt_timestamp;
+
+		ret = lis2dw12_get_fifo_samples(s, &nsamples);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		last_fifo_read_ts = __hw_clock_source_read();
+		if (nsamples == 0)
+			return EC_SUCCESS;
+
+		ret = lis2dw12_load_fifo(s, nsamples, &last_fifo_read_ts);
+
+		/*
+		 * Check if FIFO isn't empty and we never got an interrupt.
+		 * This can happen if new entries were added to the FIFO after
+		 * the count was read, but before the FIFO was cleared out.
+		 * In the long term it might be better to use the last
+		 * spread timestamp instead.
+		 */
+		if (fifo_data_avail(s) &&
+		    triggering_interrupt_timestamp == last_interrupt_timestamp)
+			lis2dw12_handle_interrupt_for_fifo(last_fifo_read_ts);
+	}
+#endif /* CONFIG_ACCEL_FIFO */
+
+	return ret;
+}
+
+/**
+ * set_power_mode - set sensor power mode
+ * @s: Motion sensor pointer
+ * @mode: LIS2DW12_LOW_POWER, LIS2DW12_HIGH_PERF
+ * @lpmode: LIS2DW12_LOW_POWER_MODE_2,
+ *          LIS2DW12_LOW_POWER_MODE_3,
+ *          LIS2DW12_LOW_POWER_MODE_4
+ *
+ * TODO: LIS2DW12_LOW_POWER_MODE_1 not implemented because output differ
+ *       in resolution
+ */
+static int set_power_mode(const struct motion_sensor_t *s,
+			  enum lis2sw12_mode mode,
+			  enum lis2sw12_lpmode lpmode)
+{
+	int ret = EC_SUCCESS;
+
+	if (mode == LIS2DW12_LOW_POWER &&
+	    lpmode == LIS2DW12_LOW_POWER_MODE_1)
+		return EC_ERROR_UNIMPLEMENTED;
+
+	/* Set Mode and Low Power Mode. */
+	ret = st_write_data_with_mask(s, LIS2DW12_ACC_MODE_ADDR,
+				      LIS2DW12_ACC_MODE_MASK, mode);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = st_write_data_with_mask(s, LIS2DW12_ACC_LPMODE_ADDR,
+				      LIS2DW12_ACC_LPMODE_MASK, lpmode);
+	return ret;
+}
+
+/**
+ * set_range - set full scale range
+ * @s: Motion sensor pointer
+ * @range: Range
+ * @rnd: Round up/down flag
+ */
+static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+{
+	int err = EC_SUCCESS;
+	uint8_t reg_val;
+	struct stprivate_data *data = s->drv_data;
+	int newrange = range;
+
+	/* Adjust and check rounded value. */
+	if (rnd && (newrange < LIS2DW12_NORMALIZE_FS(newrange)))
+		newrange <<= 1;
+
+	if (newrange > LIS2DW12_ACCEL_FS_MAX_VAL)
+		newrange = LIS2DW12_ACCEL_FS_MAX_VAL;
+
+	reg_val = LIS2DW12_FS_REG(newrange);
+
+	mutex_lock(s->mutex);
+#ifdef CONFIG_ACCEL_FIFO
+	/*
+	 * FIFO stop collecting events. Restart FIFO in Bypass mode.
+	 * If Range is changed all samples in FIFO must be discharged because
+	 * with a different sensitivity.
+	 */
+	err = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_BYPASS_MODE);
+	if (err != EC_SUCCESS)
+		goto unlock_rate;
+#endif /* CONFIG_ACCEL_FIFO */
+
+	err = st_write_data_with_mask(s, LIS2DW12_FS_ADDR, LIS2DW12_FS_MASK,
+				      reg_val);
+	if (err == EC_SUCCESS)
+		data->base.range = newrange;
+#ifdef CONFIG_ACCEL_FIFO
+	/* FIFO restart collecting events in Cont. mode. */
+	err = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_CONT_MODE);
+#endif /* CONFIG_ACCEL_FIFO */
+
+unlock_rate:
+	mutex_unlock(s->mutex);
+
+	return err;
+}
+
+static int get_range(const struct motion_sensor_t *s)
+{
+	struct stprivate_data *data = s->drv_data;
+
+	return data->base.range;
+}
+
+static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
+{
+	int ret, normalized_rate;
+	struct stprivate_data *data = s->drv_data;
+	uint8_t reg_val;
+
+	mutex_lock(s->mutex);
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* FIFO stop collecting events. Restart FIFO in Bypass mode. */
+	ret = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_BYPASS_MODE);
+	if (ret != EC_SUCCESS)
+		goto unlock_rate;
+#endif /* CONFIG_ACCEL_FIFO */
+
+	if (rate == 0) {
+		ret = st_write_data_with_mask(s, LIS2DW12_ACC_ODR_ADDR,
+					      LIS2DW12_ACC_ODR_MASK,
+					      LIS2DW12_ODR_POWER_OFF_VAL);
+		if (ret == EC_SUCCESS)
+			data->base.odr = LIS2DW12_ODR_POWER_OFF_VAL;
+
+		goto unlock_rate;
+	}
+
+	reg_val = LIS2DW12_ODR_TO_REG(rate);
+	normalized_rate = LIS2DW12_ODR_TO_NORMALIZE(rate);
+
+	if (rnd && (normalized_rate < rate)) {
+		reg_val++;
+		normalized_rate <<= 1;
+	}
+
+	if (reg_val > LIS2DW12_ODR_1_6kHZ_VAL) {
+		reg_val = LIS2DW12_ODR_1_6kHZ_VAL;
+		normalized_rate = LIS2DW12_ODR_MAX_VAL;
+	} else if (reg_val < LIS2DW12_ODR_12HZ_VAL) {
+		reg_val = LIS2DW12_ODR_12HZ_VAL;
+		normalized_rate = LIS2DW12_ODR_MIN_VAL;
+	}
+	if (reg_val > LIS2DW12_ODR_200HZ_VAL)
+		ret = set_power_mode(s, LIS2DW12_HIGH_PERF, 0);
+	else
+		ret = set_power_mode(s, LIS2DW12_LOW_POWER,
+				     LIS2DW12_LOW_POWER_MODE_2);
+
+	ret = st_write_data_with_mask(s, LIS2DW12_ACC_ODR_ADDR,
+				      LIS2DW12_ACC_ODR_MASK, reg_val);
+	if (ret == EC_SUCCESS)
+		data->base.odr = normalized_rate;
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* FIFO restart collecting events in continuous mode. */
+	ret = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_CONT_MODE);
+#endif /* CONFIG_ACCEL_FIFO */
+
+unlock_rate:
+	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->i2c_spi_addr_flags,
+			   LIS2DW12_STATUS_REG, &tmp);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	*ready = (LIS2DW12_STS_DRDY_UP == (tmp & LIS2DW12_STS_DRDY_UP));
+
+	return EC_SUCCESS;
+}
+
+static int read(const struct motion_sensor_t *s, intv3_t v)
+{
+	uint8_t raw[OUT_XYZ_SIZE];
+	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;
+	}
+
+	/* Read 6 bytes starting at xyz_reg. */
+	ret = st_raw_read_n_noinc(s->port, s->i2c_spi_addr_flags,
+				  LIS2DW12_OUT_X_L_ADDR, raw,
+				  OUT_XYZ_SIZE);
+	if (ret != EC_SUCCESS) {
+		CPRINTF("[%T %s type:0x%X RD XYZ Error]", s->name, s->type);
+		return ret;
+	}
+
+	/* Transform from LSB to real data with rotation and gain. */
+	st_normalize(s, v, raw);
+
+	return EC_SUCCESS;
+}
+
+static int init(const struct motion_sensor_t *s)
+{
+	int ret = 0, tmp, timeout = 0, status;
+	struct stprivate_data *data = s->drv_data;
+
+	ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
+			   LIS2DW12_WHO_AM_I_REG, &tmp);
+	if (ret != EC_SUCCESS)
+		return EC_ERROR_UNKNOWN;
+
+	if (tmp != LIS2DW12_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. Initiate software reset to restore
+	 * sensor to default.
+	 */
+	mutex_lock(s->mutex);
+	ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
+			    LIS2DW12_SOFT_RESET_ADDR, LIS2DW12_SOFT_RESET_MASK);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	/* Wait End of Reset. */
+	do {
+		if (timeout > 10) {
+			ret = EC_RES_TIMEOUT;
+			goto err_unlock;
+		}
+
+		msleep(1);
+		timeout += 1;
+		ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
+				  LIS2DW12_SOFT_RESET_ADDR, &status);
+		if (ret != EC_SUCCESS)
+			continue;
+	} while ((status & LIS2DW12_SOFT_RESET_MASK) != 0);
+
+	/* Enable BDU. */
+	ret = st_write_data_with_mask(s, LIS2DW12_BDU_ADDR, LIS2DW12_BDU_MASK,
+				      LIS2DW12_EN_BIT);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	ret = st_write_data_with_mask(s, LIS2DW12_LIR_ADDR, LIS2DW12_LIR_MASK,
+				      LIS2DW12_EN_BIT);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	/* Set default Mode and Low Power Mode. */
+	ret = set_power_mode(s, LIS2DW12_LOW_POWER, LIS2DW12_LOW_POWER_MODE_2);
+	if (ret != EC_SUCCESS)
+		goto err_unlock;
+
+	if (IS_ENABLED(CONFIG_ACCEL_INTERRUPTS)) {
+		ret = lis2dw12_config_interrupt(s);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+	}
+	mutex_unlock(s->mutex);
+
+	/* Set default resolution. */
+	data->resol = LIS2DW12_RESOLUTION;
+	return sensor_init_done(s);
+
+err_unlock:
+	mutex_unlock(s->mutex);
+	CPRINTF("[%T %s: MS Init type:0x%X Error]\n", s->name, s->type);
+
+	return EC_ERROR_UNKNOWN;
+}
+
+const struct accelgyro_drv lis2dw12_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 = lis2dw12_irq_handler,
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+};