summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorMario Tesi <mario.tesi@st.com>2017-04-04 11:00:55 +0200
committerCommit Bot <commit-bot@chromium.org>2019-05-24 16:33:27 +0000
commit7bc7d8a2b4bb6b06616a99104346c8801d03e162 (patch)
tree43778df9c6f6decd4aee3a43b4b813f9281a49b5
parent27054ef1d36d8995ba3f98a1541aedbd05b250ae (diff)
downloadchrome-ec-7bc7d8a2b4bb6b06616a99104346c8801d03e162.tar.gz
driver: lsm6dsm/lsm6dsl: Add FIFO support
Added FIFO support to Acc and Gyro with watermark interrupt management. Watermark is configurable setting macro CONFIG_ACCEL_FIFO_THRES properly (board.h) BUG=b:73546254 BRANCH=master TEST=Pass CTS test cheets_CTS_N.7.1_r15.x86.CtsHardwareTestCases on meowth. TEST=Tested on discovery (target stmems) BOARD with LSM6DSM connected to EC i2c master bus. Using motion sense console commands is possible to: - enable sensor (accelinit 0, accelinit 1 for acc and gyro) - set ODR (accelrate 0 x, accelrate 1 y where x, y are mHz) - show sensor data (accelinfo on <time> where time is in ms) Using this procedure is possible to see Green Led of Discovery Board Blinking each time an interrupt from FIFO arrives. To be sure to generate interrupt is better to use high ODR and low time in accelinfo. (cherry picked from 87e769a87 driver: lsm6dsm/lsm6dsl: Add FIFO support) Signed-off-by: Mario Tesi <mario.tesi@st.com> Reviewed-on: https://chromium-review.googlesource.com/467326 Commit-Ready: Gwendal Grignou <gwendal@chromium.org> Tested-by: Gwendal Grignou <gwendal@chromium.org> Reviewed-by: Gwendal Grignou <gwendal@chromium.org> Change-Id: Icf95b0e889dc806206b8ca50e74636e6a2441a18 Signed-off-by: Gwendal Grignou <gwendal@chromium.org> Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1621262 Reviewed-by: Enrico Granata <egranata@chromium.org>
-rw-r--r--driver/accelgyro_lsm6dsm.c384
-rw-r--r--driver/accelgyro_lsm6dsm.h122
-rw-r--r--driver/stm_mems_common.c5
3 files changed, 446 insertions, 65 deletions
diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 9eff6fecc6..eaccda78ca 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -11,13 +11,19 @@
#include "driver/accelgyro_lsm6dsm.h"
#include "hooks.h"
+#include "hwtimer.h"
#include "math_util.h"
#include "task.h"
+#include "timer.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)
+#ifdef CONFIG_ACCEL_FIFO
+static uint32_t last_interrupt_timestamp;
+#endif
+
/**
* @return output base register for sensor
*/
@@ -27,6 +33,298 @@ static inline int get_xyz_reg(enum motionsensor_type type)
(LSM6DSM_ACCEL_OUT_X_L_ADDR - LSM6DSM_GYRO_OUT_X_L_ADDR) * type;
}
+#ifdef CONFIG_ACCEL_INTERRUPTS
+/**
+ * Configure interrupt int 1 to fire handler for:
+ *
+ * FIFO threshold on watermark
+ *
+ * @s: Motion sensor pointer
+ */
+static int config_interrupt(const struct motion_sensor_t *s)
+{
+ int ret = EC_SUCCESS;
+ int int1_ctrl_val;
+
+ ret = raw_read8(s->port, s->addr, LSM6DSM_INT1_CTRL, &int1_ctrl_val);
+ if (ret != EC_SUCCESS)
+ return ret;
+
+#ifdef CONFIG_ACCEL_FIFO
+ /* As soon as one sample is ready, trigger an interrupt. */
+ ret = raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL1_ADDR,
+ OUT_XYZ_SIZE / sizeof(uint16_t));
+ if (ret != EC_SUCCESS)
+ return ret;
+ int1_ctrl_val |= LSM6DSM_INT_FIFO_TH | LSM6DSM_INT_FIFO_OVR |
+ LSM6DSM_INT_FIFO_FULL;
+#endif /* CONFIG_ACCEL_FIFO */
+
+ return raw_write8(s->port, s->addr, LSM6DSM_INT1_CTRL, int1_ctrl_val);
+}
+
+
+#ifdef CONFIG_ACCEL_FIFO
+/**
+ * fifo_disable - set fifo mode
+ * @s: Motion sensor pointer: must be MOTIONSENSE_TYPE_ACCEL.
+ * @fmode: BYPASS or CONTINUOUS
+ */
+static int fifo_disable(const struct motion_sensor_t *s)
+{
+ return raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR, 0x00);
+}
+
+/**
+ * fifo_reset_pattern: called at each new FIFO pattern.
+ */
+static void fifo_reset_pattern(struct lsm6dsm_data *private)
+{
+ /* The fifo is ready to run. */
+ memcpy(&private->current, &private->config,
+ sizeof(struct lsm6dsm_fifo_data));
+ private->next_in_patten = FIFO_DEV_INVALID;
+}
+
+/**
+ * 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 *accel)
+{
+ int err, i, rate;
+ uint8_t decimator[FIFO_DEV_NUM] = { 0 };
+ unsigned int min_odr = LSM6DSM_ODR_MAX_VAL;
+ unsigned int max_odr = 0;
+ struct lsm6dsm_data *private = accel->drv_data;
+ /* In FIFO sensors are mapped in a different way. */
+ uint8_t agm_maps[] = {
+ MOTIONSENSE_TYPE_GYRO,
+ MOTIONSENSE_TYPE_ACCEL,
+ MOTIONSENSE_TYPE_MAG,
+ };
+
+
+ /* Search for min and max odr values for acc, gyro. */
+ for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) {
+ /* Check if sensor enabled with ODR. */
+ rate = st_get_data_rate(accel + agm_maps[i]);
+ if (rate > 0) {
+ min_odr = MIN(min_odr, rate);
+ max_odr = MAX(max_odr, rate);
+ }
+ }
+
+ if (max_odr == 0) {
+ /* Leave FIFO disabled. */
+ return EC_SUCCESS;
+ }
+
+ /* Scan all sensors configuration to calculate FIFO decimator. */
+ private->config.total_samples_in_pattern = 0;
+ for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) {
+ rate = st_get_data_rate(accel + agm_maps[i]);
+ if (rate > 0) {
+ private->config.samples_in_pattern[i] = rate / min_odr;
+ decimator[i] = LSM6DSM_FIFO_DECIMATOR(max_odr / rate);
+ private->config.total_samples_in_pattern +=
+ private->config.samples_in_pattern[i];
+ } else {
+ /* Not in FIFO if sensor disabled. */
+ private->config.samples_in_pattern[i] = 0;
+ }
+ }
+ raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL3_ADDR,
+ (decimator[FIFO_DEV_GYRO] << LSM6DSM_FIFO_DEC_G_OFF) |
+ (decimator[FIFO_DEV_ACCEL] << LSM6DSM_FIFO_DEC_XL_OFF));
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+ raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL4_ADDR,
+ decimator[FIFO_DEV_MAG]);
+#endif /* CONFIG_MAG_LSM6DSM_LIS2MDL */
+ err = raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL5_ADDR,
+ (LSM6DSM_ODR_TO_REG(max_odr) <<
+ LSM6DSM_FIFO_CTRL5_ODR_OFF) |
+ LSM6DSM_FIFO_MODE_CONTINUOUS_VAL);
+ if (err != EC_SUCCESS)
+ return err;
+ fifo_reset_pattern(private);
+ return EC_SUCCESS;
+}
+
+/*
+ * Must order FIFO read based on ODR:
+ * For example Acc @ 52 Hz, Gyro @ 26 Hz Mag @ 13 Hz in FIFO we have
+ * for each pattern this data samples:
+ * ________ _______ _______ _______ ________ _______ _______
+ * | Gyro_0 | Acc_0 | Mag_0 | Acc_1 | Gyro_1 | Acc_2 | Acc_3 |
+ * |________|_______|_______|_______|________|_______|_______|
+ *
+ * Total samples for each pattern: 2 Gyro, 4 Acc, 1 Mag.
+ */
+static int fifo_next(struct lsm6dsm_data *private)
+{
+ int next_id;
+
+ if (private->current.total_samples_in_pattern == 0)
+ fifo_reset_pattern(private);
+
+ if (private->current.total_samples_in_pattern == 0) {
+ /*
+ * Not expected we are supposed to be called to process FIFO
+ * data.
+ */
+ CPRINTF("[%T FIFO empty pattern]\n");
+ return FIFO_DEV_INVALID;
+ }
+
+ for (next_id = private->next_in_patten + 1; 1; next_id++) {
+ if (next_id == FIFO_DEV_NUM)
+ next_id = FIFO_DEV_GYRO;
+ if (private->current.samples_in_pattern[next_id] != 0) {
+ private->current.samples_in_pattern[next_id]--;
+ private->current.total_samples_in_pattern--;
+ private->next_in_patten = next_id;
+ return next_id;
+ }
+ }
+ /* Will never happen. */
+ return FIFO_DEV_INVALID;
+}
+
+/**
+ * push_fifo_data - Scan data pattern and push upside
+ */
+static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo,
+ uint16_t flen)
+{
+ struct lsm6dsm_data *private = accel->drv_data;
+ /* In FIFO sensors are mapped in a different way. */
+ uint8_t agm_maps[] = {
+ MOTIONSENSE_TYPE_GYRO,
+ MOTIONSENSE_TYPE_ACCEL,
+ MOTIONSENSE_TYPE_MAG,
+ };
+
+ while (flen > 0) {
+ struct ec_response_motion_sensor_data vect;
+ int id = agm_maps[fifo_next(private)];
+ int *axis = (accel + id)->raw_xyz;
+
+ /* Apply precision, sensitivity and rotation. */
+ st_normalize(accel + id, axis, fifo);
+ vect.data[X] = axis[X];
+ vect.data[Y] = axis[Y];
+ vect.data[Z] = axis[Z];
+
+ vect.flags = 0;
+ vect.sensor_num = accel - motion_sensors + id;
+ motion_sense_fifo_add_data(&vect, accel + id, 3,
+ last_interrupt_timestamp);
+
+ fifo += OUT_XYZ_SIZE;
+ flen -= OUT_XYZ_SIZE;
+ }
+}
+
+static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts)
+{
+ int err, left, length;
+ uint8_t fifo[FIFO_READ_LEN];
+
+ /*
+ * DIFF[9:0] are number of unread uint16 in FIFO
+ * mask DIFF and compute total byte len to read from FIFO.
+ */
+ left = fsts->len & LSM6DSM_FIFO_DIFF_MASK;
+ left *= sizeof(uint16_t);
+ left = (left / OUT_XYZ_SIZE) * OUT_XYZ_SIZE;
+
+ /* Push all data on upper side. */
+ do {
+ /* Fit len to pre-allocated static buffer. */
+ if (left > FIFO_READ_LEN)
+ length = FIFO_READ_LEN;
+ else
+ length = left;
+
+ /* Read data and copy in buffer. */
+ err = st_raw_read_n_noinc(s->port, s->addr,
+ LSM6DSM_FIFO_DATA_ADDR,
+ fifo, length);
+ if (err != EC_SUCCESS)
+ return err;
+
+ /* Manage patterns and push data. */
+ push_fifo_data(s, fifo, length);
+ left -= length;
+ } while (left > 0);
+
+ return EC_SUCCESS;
+}
+
+/**
+ * config_fifo - update mode and ODR for FIFO decimator
+ */
+static int config_fifo(const struct motion_sensor_t *accel)
+{
+ int err;
+
+ /* Changing in ODR must stop FIFO. */
+ err = fifo_disable(accel);
+ if (err != EC_SUCCESS)
+ return err;
+
+
+ return fifo_enable(accel);
+}
+#endif /* CONFIG_ACCEL_FIFO */
+
+/**
+ * lsm6dsm_interrupt - interrupt from int1/2 pin of sensor
+ */
+void lsm6dsm_interrupt(enum gpio_signal signal)
+{
+#ifdef CONFIG_ACCEL_FIFO
+ last_interrupt_timestamp = __hw_clock_source_read();
+#endif
+ task_set_event(TASK_ID_MOTIONSENSE,
+ CONFIG_ACCEL_LSM6DSM_INT_EVENT, 0);
+}
+
+/**
+ * irq_handler - bottom half of the interrupt stack
+ */
+static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+ int ret = EC_SUCCESS;
+
+ if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+ (!(*event & CONFIG_ACCEL_LSM6DSM_INT_EVENT)))
+ return EC_ERROR_NOT_HANDLED;
+
+#ifdef CONFIG_ACCEL_FIFO
+ {
+ struct fstatus fsts;
+ /* Read how many data pattern on FIFO to read and pattern. */
+ ret = st_raw_read_n_noinc(s->port, s->addr,
+ LSM6DSM_FIFO_STS1_ADDR,
+ (uint8_t *)&fsts, sizeof(fsts));
+ if (ret != EC_SUCCESS)
+ return ret;
+ if (fsts.len & (LSM6DSM_FIFO_DATA_OVR | LSM6DSM_FIFO_FULL)) {
+ CPRINTF("[%T %s FIFO Overrun: %04x]\n",
+ s->name, fsts.len);
+ }
+ if (!(fsts.len & LSM6DSM_FIFO_EMPTY))
+ ret = load_fifo(s, &fsts);
+ }
+#endif
+ return ret;
+}
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+
/**
* set_range - set full scale range
* @s: Motion sensor pointer
@@ -99,50 +397,34 @@ static int get_range(const struct motion_sensor_t *s)
*/
static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
{
- int ret, normalized_rate = LSM6DSM_ODR_MIN_VAL;
+ int ret, normalized_rate;
struct stprivate_data *data = s->drv_data;
uint8_t ctrl_reg, reg_val;
ctrl_reg = LSM6DSM_ODR_REG(s->type);
-
- if (rate == 0) {
- /* 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_REG_TO_ODR(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_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;
+ if (rate > 0) {
+ reg_val = LSM6DSM_ODR_TO_REG(rate);
+ normalized_rate = LSM6DSM_REG_TO_ODR(reg_val);
+
+ if (rnd && (normalized_rate < rate)) {
+ reg_val++;
+ normalized_rate *= 2;
+ }
+ } else {
+ reg_val = 0;
+ normalized_rate = 0;
}
mutex_lock(s->mutex);
ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, reg_val);
- if (ret == EC_SUCCESS)
+ if (ret == EC_SUCCESS) {
data->base.odr = normalized_rate;
+#ifdef CONFIG_ACCEL_FIFO
+ config_fifo(LSM6DSM_MAIN_SENSOR(s));
+#endif
+ }
mutex_unlock(s->mutex);
-
return ret;
}
@@ -165,8 +447,6 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready)
}
/*
- * TODO: Implement FIFO support
- *
* 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).
@@ -228,20 +508,32 @@ static int init(const struct motion_sensor_t *s)
mutex_lock(s->mutex);
/* Software reset. */
- ret = st_write_data_with_mask(s,
- LSM6DSM_RESET_ADDR,
- LSM6DSM_RESET_MASK,
- LSM6DSM_EN_BIT);
+ ret = raw_write8(s->port, s->addr, LSM6DSM_CTRL3_ADDR,
+ LSM6DSM_SW_RESET);
+ if (ret != EC_SUCCESS)
+ goto err_unlock;
+
+ /*
+ * Output data not updated until have been read.
+ * Prefer interrupt to be active low.
+ */
+ ret = raw_write8(s->port, s->addr, LSM6DSM_CTRL3_ADDR,
+ LSM6DSM_BDU | LSM6DSM_H_L_ACTIVE |
+ LSM6DSM_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 */
- /* Output data not updated until have been read. */
- ret = st_write_data_with_mask(s,
- LSM6DSM_BDU_ADDR,
- LSM6DSM_BDU_MASK,
- LSM6DSM_EN_BIT);
+#ifdef CONFIG_ACCEL_INTERRUPTS
+ ret = config_interrupt(s);
if (ret != EC_SUCCESS)
goto err_unlock;
+#endif /* CONFIG_ACCEL_INTERRUPTS */
mutex_unlock(s->mutex);
}
@@ -252,6 +544,7 @@ static int init(const struct motion_sensor_t *s)
err_unlock:
mutex_unlock(s->mutex);
+ CPRINTF("[%T %s: MS Init type:0x%X Error]\n", s->name, s->type);
return ret;
}
@@ -266,4 +559,7 @@ const struct accelgyro_drv lsm6dsm_drv = {
.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_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index 7f02baa02e..bb332f71b5 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -23,16 +23,10 @@
#define LSM6DSM_WHO_AM_I_REG 0x0f
#define LSM6DSM_WHO_AM_I 0x6a
-/* Sensor Software Reset Bit */
-#define LSM6DSM_RESET_ADDR 0x12
-#define LSM6DSM_RESET_MASK 0x01
-
/* COMMON DEFINE FOR ACCEL-GYRO SENSORS */
#define LSM6DSM_EN_BIT 0x01
#define LSM6DSM_DIS_BIT 0x00
-#define LSM6DSM_BDU_ADDR 0x12
-#define LSM6DSM_BDU_MASK 0x40
#define LSM6DSM_GYRO_OUT_X_L_ADDR 0x22
#define LSM6DSM_ACCEL_OUT_X_L_ADDR 0x28
@@ -40,6 +34,15 @@
#define LSM6DSM_CTRL1_ADDR 0x10
#define LSM6DSM_CTRL2_ADDR 0x11
#define LSM6DSM_CTRL3_ADDR 0x12
+#define LSM6DSM_SW_RESET 0x01
+#define LSM6DSM_IF_INC 0x04
+#define LSM6DSM_PP_OD 0x10
+#define LSM6DSM_H_L_ACTIVE 0x20
+#define LSM6DSM_BDU 0x40
+
+#define LSM6DSM_CTRL4_ADDR 0x13
+#define LSM6DSM_INT2_ON_INT1_MASK 0x20
+
#define LSM6DSM_CTRL6_ADDR 0x15
#define LSM6DSM_CTRL7_ADDR 0x16
@@ -50,16 +53,70 @@
(LSM6DSM_CTRL1_ADDR + _sensor)
#define LSM6DSM_ODR_MASK 0xf0
-/* Common Acc/Gyro data rate */
-enum lsm6dsm_odr {
- LSM6DSM_ODR_0HZ_VAL = 0,
- LSM6DSM_ODR_13HZ_VAL,
- LSM6DSM_ODR_26HZ_VAL,
- LSM6DSM_ODR_52HZ_VAL,
- LSM6DSM_ODR_104HZ_VAL,
- LSM6DSM_ODR_208HZ_VAL,
- LSM6DSM_ODR_416HZ_VAL,
- LSM6DSM_ODR_LIST_NUM
+/* Hardware FIFO size in byte */
+#define LSM6DSM_MAX_FIFO_SIZE 4096
+#define LSM6DSM_MAX_FIFO_LENGTH (LSM6DSM_MAX_FIFO_SIZE / OUT_XYZ_SIZE)
+
+/* FIFO decimator registers and bitmask */
+#define LSM6DSM_FIFO_CTRL1_ADDR 0x06
+#define LSM6DSM_FIFO_CTRL2_ADDR 0x07
+
+#define LSM6DSM_FIFO_CTRL3_ADDR 0x08
+#define LSM6DSM_FIFO_DEC_XL_OFF 0
+#define LSM6DSM_FIFO_DEC_G_OFF 3
+
+#define LSM6DSM_FIFO_CTRL4_ADDR 0x09
+
+#define LSM6DSM_FIFO_DECIMATOR(_dec) \
+ (_dec < 8 ? _dec : (2 + __builtin_ctz(_dec)))
+
+#define LSM6DSM_INT1_CTRL 0x0d
+#define LSM6DSM_INT_FIFO_TH 0x08
+#define LSM6DSM_INT_FIFO_OVR 0x10
+#define LSM6DSM_INT_FIFO_FULL 0x20
+#define LSM6DSM_INT_SIGMO 0x40
+
+#define LSM6DSM_FIFO_STS1_ADDR 0x3a
+#define LSM6DSM_FIFO_STS2_ADDR 0x3b
+#define LSM6DSM_FIFO_DIFF_MASK 0x07ff
+#define LSM6DSM_FIFO_EMPTY 0x1000
+#define LSM6DSM_FIFO_FULL 0x2000
+#define LSM6DSM_FIFO_DATA_OVR 0x4000
+#define LSM6DSM_FIFO_WATERMARK 0x8000
+#define LSM6DSM_FIFO_NODECIM 0x01
+
+/* Out data register */
+#define LSM6DSM_FIFO_DATA_ADDR 0x3e
+
+/* Registers value for supported FIFO mode */
+#define LSM6DSM_FIFO_MODE_BYPASS_VAL 0x00
+#define LSM6DSM_FIFO_MODE_CONTINUOUS_VAL 0x06
+
+#define LSM6DSM_FIFO_CTRL5_ADDR 0x0a
+#define LSM6DSM_FIFO_CTRL5_ODR_OFF 3
+#define LSM6DSM_FIFO_CTRL5_ODR_MASK \
+ (0xf << LSM6DSM_FIFO_CTRL5_ODR_OFF)
+#define LSM6DSM_FIFO_CTRL5_MODE_MASK 0x07
+
+/* Define ODR FIFO values. Max value is max ODR for sensors
+ * Value is limited to 416 Hz
+ */
+
+/* Registers value for sensor Hub */
+#define LSM6DSM_FUNC_SRC1 0x53
+#define LSM6DSM_SENSORHUB_END_OP 0x01
+
+/* Define device available in FIFO pattern */
+enum dev_fifo {
+ FIFO_DEV_INVALID = -1,
+ FIFO_DEV_GYRO = 0,
+ FIFO_DEV_ACCEL,
+ FIFO_DEV_NUM,
+};
+
+struct fstatus {
+ uint16_t len;
+ uint16_t pattern;
};
/* Absolute maximum rate for acc and gyro sensors */
@@ -146,4 +203,37 @@ enum lsm6dsm_status {
extern const struct accelgyro_drv lsm6dsm_drv;
+void lsm6dsm_interrupt(enum gpio_signal signal);
+
+struct lsm6dsm_fifo_data {
+ /*
+ * FIFO data order is based on the ODR of each sensors.
+ * For example Acc @ 52 Hz, Gyro @ 26 Hz Mag @ 13 Hz in FIFO we have
+ * for each pattern this data samples:
+ * ________ _______ _______ _______ ________ _______ _______
+ * | Gyro_0 | Acc_0 | Mag_0 | Acc_1 | Gyro_1 | Acc_2 | Acc_3 |
+ * |________|_______|_______|_______|________|_______|_______|
+ *
+ * Total samples for each pattern: 2 Gyro, 4 Acc, 1 Mag
+ */
+ /* Calculated samples in a pattern, based on ODR. */
+ int samples_in_pattern[FIFO_DEV_NUM];
+
+ /* Sum of all samples_in_pattern. */
+ int total_samples_in_pattern;
+};
+
+
+struct lsm6dsm_data {
+ /* Must be first: ST generic accelerometer data. */
+ struct stprivate_data a_data;
+#ifdef CONFIG_ACCEL_FIFO
+ struct lsm6dsm_fifo_data config;
+ struct lsm6dsm_fifo_data current;
+ int next_in_patten;
+#endif
+};
+
+#define LSM6DSM_MAIN_SENSOR(_s) ((_s) - (_s)->type)
+
#endif /* __CROS_EC_ACCELGYRO_LSM6DSM_H */
diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c
index c2939ae714..4af0cb4a34 100644
--- a/driver/stm_mems_common.c
+++ b/driver/stm_mems_common.c
@@ -163,10 +163,5 @@ void st_normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
/* apply offset in the device coordinates */
range = s->drv->get_range(s);
for (i = X; i <= Z; i++)
- /*
- * TODO(gwendal): only work for accel:
- * offsest is in 1/1024th g/LSB, so shift left and divide by
- * range to normalize the value.
- */
v[i] += (drvdata->offset[i] << 5) / range;
}