diff options
-rw-r--r-- | driver/accelgyro_lsm6dsm.c | 384 | ||||
-rw-r--r-- | driver/accelgyro_lsm6dsm.h | 122 | ||||
-rw-r--r-- | driver/stm_mems_common.c | 5 |
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; } |