phaser: enable phaser motion sensor drivers

This patch add phaser base and lid accel sensor support.
Lid sensor type is lis2de, it has the same register interface
as lis2dh, so they share the same driver. Since it has a very
small fifo, use it in forced mode.

Signed-off-by: Paul Ma <magf@bitland.corp-partner.google.com>

BRANCH=none
BUG=b:110013316
TEST=boot phaser board, base and lid sensor can be inititalized
     successfully. use console command "accelinfo on", both
     sensors has valid output.

Change-Id: Ie8514ea449fec41c6b1e0b6be1f2ae88458d119c
Reviewed-on: https://chromium-review.googlesource.com/1105688
Commit-Ready: Jett Rink <jettrink@chromium.org>
Tested-by: Jett Rink <jettrink@chromium.org>
Reviewed-by: Jett Rink <jettrink@chromium.org>

3 files changed, 31 insertions, 204 deletions

diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c
index 3bba013b71..a8e8827acd 100644
--- a/driver/accel_lis2dh.c
+++ b/driver/accel_lis2dh.c
@@ -22,29 +22,6 @@
 #define CPUTS(outstr) cputs(CC_ACCEL, outstr)
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
 
-#ifdef CONFIG_ACCEL_FIFO
-/**
- * enable_fifo - Enable/Disable FIFO in LIS2DH
- * @s: Motion sensor pointer
- * @mode: fifo_modes
- * @en_dis: LIS2DH_EN_BIT/LIS2DH_DIS_BIT
- */
-static int enable_fifo(const struct motion_sensor_t *s, int mode, int en_dis)
-{
-	int ret;
-
-	ret = st_write_data_with_mask(s, LIS2DH_FIFO_CTRL_REG,
-				      LIS2DH_FIFO_MODE_MASK, mode);
-	if (ret != EC_SUCCESS)
-		return ret;
-
-	ret = st_write_data_with_mask(s, LIS2DH_CTRL5_ADDR, LIS2DH_FIFO_EN_MASK,
-				      en_dis);
-
-	return ret;
-}
-#endif /* CONFIG_ACCEL_FIFO */
-
 /**
  * set_range - set full scale range
  * @s: Motion sensor pointer
@@ -92,7 +69,7 @@ static int get_range(const struct motion_sensor_t *s)
 {
 	struct stprivate_data *data = s->drv_data;
 
-	return LIS2DH_GAIN_TO_FS(data->base.range);
+	return data->base.range;
 }
 
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
@@ -103,13 +80,6 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 
 	mutex_lock(s->mutex);
 
-#ifdef CONFIG_ACCEL_FIFO
-	/* FIFO stop collecting events. Restart FIFO in Bypass mode */
-	ret = enable_fifo(s, LIS2DH_FIFO_BYPASS_MODE, LIS2DH_DIS_BIT);
-	if (ret != EC_SUCCESS)
-		goto unlock_rate;
-#endif /* CONFIG_ACCEL_FIFO */
-
 	if (rate == 0) {
 		/* Power Off device */
 		ret = st_write_data_with_mask(
@@ -144,139 +114,16 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 	if (ret == EC_SUCCESS)
 		data->base.odr = normalized_rate;
 
-#ifdef CONFIG_ACCEL_FIFO
-	/* FIFO restart collecting events */
-	ret = enable_fifo(s, LIS2DH_FIFO_STREAM_MODE, LIS2DH_EN_BIT);
-#endif /* CONFIG_ACCEL_FIFO */
-
 unlock_rate:
 	mutex_unlock(s->mutex);
 	return ret;
 }
 
-#ifdef CONFIG_ACCEL_FIFO
-/*
- * Load data from internal sensor FIFO (deep 32 byte)
- */
-static int load_fifo(struct motion_sensor_t *s)
-{
-	int ret, tmp, nsamples, i;
-	struct ec_response_motion_sensor_data vect;
-	int done = 0;
-	int *axis = s->raw_xyz;
-	uint8_t fifo[FIFO_READ_LEN];
-
-	/* Try to Empty FIFO */
-	do {
-		/* Read samples number in status register */
-		ret = raw_read8(s->port, s->addr, LIS2DH_FIFO_SRC_REG, &tmp);
-		if (ret != EC_SUCCESS)
-			return ret;
-
-		/* Check FIFO empty flag */
-		if (tmp & LIS2DH_FIFO_EMPTY_FLAG)
-			return EC_SUCCESS;
-
-		nsamples = (tmp & LIS2DH_FIFO_UNREAD_MASK) * OUT_XYZ_SIZE;
-
-		/* Limit FIFO read data to burst of FIFO_READ_LEN size because
-		 * read operatios in under i2c mutex lock */
-		if (nsamples > FIFO_READ_LEN)
-			nsamples = FIFO_READ_LEN;
-		else
-			done = 1;
-
-		ret = st_raw_read_n(s->port, s->addr, LIS2DH_OUT_X_L_ADDR, fifo,
-				 nsamples);
-		if (ret != EC_SUCCESS)
-			return ret;
-
-		for (i = 0; i < nsamples; i += OUT_XYZ_SIZE) {
-			/* Apply precision, sensitivity and rotation vector */
-			st_normalize(s, axis, &fifo[i]);
-
-			/* Fill vector array */
-			vect.data[0] = axis[0];
-			vect.data[1] = axis[1];
-			vect.data[2] = axis[2];
-			vect.flags = 0;
-			vect.sensor_num = 0;
-			motion_sense_fifo_add_data(&vect, s, 3,
-						   __hw_clock_source_read());
-			/*
-			 * TODO: get time at a more accurate spot.
-			 * Like in lis2dh_interrupt
-			 */
-		}
-	} while(!done);
-
-	return EC_SUCCESS;
-}
-#endif  /* CONFIG_ACCEL_FIFO */
-
-#ifdef CONFIG_ACCEL_INTERRUPTS
-static int config_interrupt(const struct motion_sensor_t *s)
-{
-	int ret;
-
-#ifdef CONFIG_ACCEL_FIFO_THRES
-	/* configure FIFO watermark level */
-	ret = st_write_data_with_mask(s, LIS2DH_FIFO_CTRL_REG,
-				   LIS2DH_FIFO_THR_MASK,
-				   CONFIG_ACCEL_FIFO_THRES);
-	if (ret != EC_SUCCESS)
-		return ret;
-	/* enable interrupt on FIFO watermask and route to int1 */
-	ret = st_write_data_with_mask(s, LIS2DH_CTRL3_ADDR,
-				   LIS2DH_FIFO_WTM_INT_MASK, 1);
-#endif /* CONFIG_ACCEL_FIFO */
-
-	return ret;
-}
-
-/**
- * lis2dh_interrupt - interrupt from int1/2 pin of sensor
- */
-void lis2dh_interrupt(enum gpio_signal signal)
-{
-	task_set_event(TASK_ID_MOTIONSENSE,
-		       CONFIG_ACCEL_LIS2DH_INT_EVENT, 0);
-}
-
-/**
- * irq_handler - bottom half of the interrupt stack.
- */
-static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
-{
-	int interrupt;
-
-	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
-	    (!(*event & CONFIG_ACCEL_LIS2DH_INT_EVENT))) {
-		return EC_ERROR_NOT_HANDLED;
-	}
-
-	/* read interrupt status register to reset source */
-	raw_read8(s->port, s->addr, LIS2DH_INT1_SRC_REG, &interrupt);
-
-#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
-	*event |= CONFIG_GESTURE_TAP_EVENT;
-#endif
-#ifdef CONFIG_GESTURE_SIGMO
-	*event |= CONFIG_GESTURE_SIGMO_EVENT;
-#endif
-	/*
-	 * No need to read the FIFO here, motion sense task is
-	 * doing it on every interrupt.
-	 */
-	return EC_SUCCESS;
-}
-#endif  /* CONFIG_ACCEL_INTERRUPTS */
-
 static int is_data_ready(const struct motion_sensor_t *s, int *ready)
 {
 	int ret, tmp;
 
-	ret = raw_read8(s->port, s->addr, LIS2DH_STATUS_REG, &tmp);
+	ret = st_raw_read8(s->port, s->addr, LIS2DH_STATUS_REG, &tmp);
 	if (ret != EC_SUCCESS) {
 		CPRINTF("[%T %s type:0x%X RS Error]", s->name, s->type);
 		return ret;
@@ -290,8 +137,7 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready)
 static int read(const struct motion_sensor_t *s, vector_3_t v)
 {
 	uint8_t raw[OUT_XYZ_SIZE];
-	int ret, i, tmp = 0;
-	struct stprivate_data *data = s->drv_data;
+	int ret, tmp = 0;
 
 	ret = is_data_ready(s, &tmp);
 	if (ret != EC_SUCCESS)
@@ -327,8 +173,24 @@ static int init(const struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
 	struct stprivate_data *data = s->drv_data;
+	int count = 10;
+
+	/*
+	 * lis2de need 5 milliseconds to complete boot procedure after
+	 * device power-up. When sensor is powered on, it can't be
+	 * accessed immediately. We need wait serval loops to let sensor
+	 * complete boot procedure.
+	 */
+	do {
+		ret = st_raw_read8(s->port, s->addr, LIS2DH_WHO_AM_I_REG, &tmp);
+		if (ret != EC_SUCCESS) {
+			udelay(10);
+			count--;
+		} else {
+			break;
+		}
+	} while (count > 0);
 
-	ret = raw_read8(s->port, s->addr, LIS2DH_WHO_AM_I_REG, &tmp);
 	if (ret != EC_SUCCESS)
 		return ret;
 
@@ -340,33 +202,33 @@ static int init(const struct motion_sensor_t *s)
 	 * register must be restored to it's default
 	 */
 	/* Enable all accel axes data and clear old settings */
-	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL1_ADDR,
+	ret = st_raw_write8(s->port, s->addr, LIS2DH_CTRL1_ADDR,
 			 LIS2DH_ENABLE_ALL_AXES);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
-	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL2_ADDR,
+	ret = st_raw_write8(s->port, s->addr, LIS2DH_CTRL2_ADDR,
 			 LIS2DH_CTRL2_RESET_VAL);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
-	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL3_ADDR,
+	ret = st_raw_write8(s->port, s->addr, LIS2DH_CTRL3_ADDR,
 			 LIS2DH_CTRL3_RESET_VAL);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
 	/* Enable BDU */
-	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL4_ADDR,
+	ret = st_raw_write8(s->port, s->addr, LIS2DH_CTRL4_ADDR,
 			 LIS2DH_BDU_MASK);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
-	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL5_ADDR,
+	ret = st_raw_write8(s->port, s->addr, LIS2DH_CTRL5_ADDR,
 			 LIS2DH_CTRL5_RESET_VAL);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
-	ret = raw_write8(s->port, s->addr, LIS2DH_CTRL6_ADDR,
+	ret = st_raw_write8(s->port, s->addr, LIS2DH_CTRL6_ADDR,
 			 LIS2DH_CTRL6_RESET_VAL);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
@@ -376,12 +238,6 @@ static int init(const struct motion_sensor_t *s)
 	/* Set default resolution */
 	data->resol = LIS2DH_RESOLUTION;
 
-#ifdef CONFIG_ACCEL_INTERRUPTS
-	ret = config_interrupt(s);
-	if (ret != EC_SUCCESS)
-		return ret;
-#endif
-
 	return sensor_init_done(s);
 
 err_unlock:
@@ -402,7 +258,4 @@ const struct accelgyro_drv lis2dh_drv = {
 	.set_offset = st_set_offset,
 	.get_offset = st_get_offset,
 	.perform_calib = NULL,
-#ifdef CONFIG_ACCEL_INTERRUPTS
-	.irq_handler = irq_handler,
-#endif /* CONFIG_ACCEL_INTERRUPTS */
 };
diff --git a/driver/accel_lis2dh.h b/driver/accel_lis2dh.h
index 4a01cc995c..0421f79503 100644
--- a/driver/accel_lis2dh.h
+++ b/driver/accel_lis2dh.h
@@ -51,34 +51,10 @@
 #define LIS2DH_STATUS_REG	0x27
 #define LIS2DH_STS_XLDA_UP	0x80
 
-#ifdef CONFIG_ACCEL_FIFO
-
-/* FIFO regs, masks and define */
-#define LIS2DH_FIFO_WTM_INT_MASK	0x04
-#define LIS2DH_FIFO_CTRL_REG	0x2e
-#define LIS2DH_FIFO_MODE_MASK	0xc0
-#define LIS2DH_FIFO_THR_MASK	0x1f
-
-/* Select FIFO supported mode:
- * BYPASS - Bypass FIFO
- * FIFO - FIFO mode collect data
- * STREAM - FIFO older data is replaced by new data
- * SFIFO - Stream-to-FIFO mode. Mix FIFO & STREAM
- */
-enum lis2dh_fifo_modes {
-	LIS2DH_FIFO_BYPASS_MODE = 0x00,
-	LIS2DH_FIFO_MODE,
-	LIS2DH_FIFO_STREAM_MODE,
-	LIS2DH_FIFO_SFIFO_MODE
-};
-
-/* Defines for LIS2DH_CTRL5_ADDR FIFO register */
-#define LIS2DH_FIFO_EN_MASK	0x40
-
-#define LIS2DH_FIFO_SRC_REG	0x2f
-#define LIS2DH_FIFO_EMPTY_FLAG	0x20
-#define LIS2DH_FIFO_UNREAD_MASK	0x1f
-#endif /* CONFIG_ACCEL_FIFO */
+#define LIS2DH_FS_2G_VAL         0x00
+#define LIS2DH_FS_4G_VAL         0x01
+#define LIS2DH_FS_8G_VAL         0x02
+#define LIS2DH_FS_16G_VAL        0x03
 
 /* Interrupt source status register */
 #define LIS2DH_INT1_SRC_REG	0x31
@@ -134,6 +110,4 @@ enum lis2dh_odr {
 
 extern const struct accelgyro_drv lis2dh_drv;
 
-void lis2dh_interrupt(enum gpio_signal signal);
-
 #endif /* __CROS_EC_ACCEL_LIS2DH_H */
diff --git a/driver/stm_mems_common.h b/driver/stm_mems_common.h
index a06973c08c..147b9f3e82 100644
--- a/driver/stm_mems_common.h
+++ b/driver/stm_mems_common.h
@@ -17,7 +17,7 @@
 /* X, Y, Z axis data len */
 #define OUT_XYZ_SIZE			6
 
-#define ST_NORMALIZE_RATE(_fS)    (1 << __fls(_fs))
+#define ST_NORMALIZE_RATE(_fs)    (1 << __fls(_fs))
 
 #ifdef CONFIG_ACCEL_FIFO
 #define FIFO_BUFFER_NUM_PATTERN		32