| /* Copyright 2020 The ChromiumOS Authors |
| * Use of this source code is governed by a BSD-style license that can be |
| * found in the LICENSE file. |
| */ |
| |
| /** |
| * BMI accelerometer and gyro module for Chrome EC |
| * 3D digital accelerometer & 3D digital gyroscope |
| */ |
| |
| #include "accelgyro.h" |
| #include "accelgyro_bmi_common.h" |
| #include "console.h" |
| #include "i2c.h" |
| #include "mag_bmm150.h" |
| #include "mag_lis2mdl.h" |
| #include "math_util.h" |
| #include "motion_sense_fifo.h" |
| #include "spi.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) |
| |
| #if !defined(CONFIG_ACCELGYRO_BMI160) && !defined(CONFIG_ACCELGYRO_BMI220) && \ |
| !defined(CONFIG_ACCELGYRO_BMI260) && !defined(CONFIG_ACCELGYRO_BMI3XX) |
| #error "Must use following sensors BMI160 BMI220 BMI260 BMI3XX" |
| #endif |
| |
| #if (defined(CONFIG_ACCELGYRO_BMI260) || defined(CONFIG_ACCELGYRO_BMI220)) && \ |
| !defined(CONFIG_ACCELGYRO_BMI160) |
| #define V(s_) 1 |
| #elif defined(CONFIG_ACCELGYRO_BMI160) && \ |
| !(defined(CONFIG_ACCELGYRO_BMI260) || \ |
| defined(CONFIG_ACCELGYRO_BMI220)) |
| #define V(s_) 0 |
| #else |
| #define V(s_) \ |
| ((s_)->chip == MOTIONSENSE_CHIP_BMI260 || \ |
| (s_)->chip == MOTIONSENSE_CHIP_BMI220) |
| #endif |
| /* Index for which table to use. */ |
| #if defined(CONFIG_ACCELGYRO_BMI160) && \ |
| (defined(CONFIG_ACCELGYRO_BMI220) || defined(CONFIG_ACCELGYRO_BMI260)) |
| #define T(s_) V(s_) |
| #else |
| #define T(s_) 0 |
| #endif |
| |
| /* List of range values in +/-G's and their associated register values. */ |
| const struct bmi_accel_param_pair g_ranges[][4] = { |
| #ifdef CONFIG_ACCELGYRO_BMI160 |
| { { 2, BMI160_GSEL_2G }, |
| { 4, BMI160_GSEL_4G }, |
| { 8, BMI160_GSEL_8G }, |
| { 16, BMI160_GSEL_16G } }, |
| #endif |
| #if defined(CONFIG_ACCELGYRO_BMI220) || defined(CONFIG_ACCELGYRO_BMI260) |
| { { 2, BMI260_GSEL_2G }, |
| { 4, BMI260_GSEL_4G }, |
| { 8, BMI260_GSEL_8G }, |
| { 16, BMI260_GSEL_16G } }, |
| #endif |
| }; |
| |
| /* |
| * List of angular rate range values in +/-dps's |
| * and their associated register values. |
| */ |
| const struct bmi_accel_param_pair dps_ranges[][5] = { |
| #ifdef CONFIG_ACCELGYRO_BMI160 |
| { { 125, BMI160_DPS_SEL_125 }, |
| { 250, BMI160_DPS_SEL_250 }, |
| { 500, BMI160_DPS_SEL_500 }, |
| { 1000, BMI160_DPS_SEL_1000 }, |
| { 2000, BMI160_DPS_SEL_2000 } }, |
| #endif |
| #if defined(CONFIG_ACCELGYRO_BMI220) || defined(CONFIG_ACCELGYRO_BMI260) |
| { { 125, BMI260_DPS_SEL_125 }, |
| { 250, BMI260_DPS_SEL_250 }, |
| { 500, BMI260_DPS_SEL_500 }, |
| { 1000, BMI260_DPS_SEL_1000 }, |
| { 2000, BMI260_DPS_SEL_2000 } }, |
| #endif |
| }; |
| |
| int bmi_get_xyz_reg(const struct motion_sensor_t *s) |
| { |
| switch (s->type) { |
| case MOTIONSENSE_TYPE_ACCEL: |
| return BMI_ACC_DATA(V(s)); |
| case MOTIONSENSE_TYPE_GYRO: |
| return BMI_GYR_DATA(V(s)); |
| case MOTIONSENSE_TYPE_MAG: |
| return BMI_AUX_DATA(V(s)); |
| default: |
| return -1; |
| } |
| } |
| |
| const struct bmi_accel_param_pair * |
| bmi_get_range_table(const struct motion_sensor_t *s, int *psize) |
| { |
| if (s->type == MOTIONSENSE_TYPE_ACCEL) { |
| if (psize) |
| *psize = ARRAY_SIZE(g_ranges[T(s)]); |
| return g_ranges[T(s)]; |
| } |
| if (psize) |
| *psize = ARRAY_SIZE(dps_ranges[T(s)]); |
| return dps_ranges[T(s)]; |
| } |
| |
| /** |
| * @return reg value that matches the given engineering value passed in. |
| * The round_up flag is used to specify whether to round up or down. |
| * Note, this function always returns a valid reg value. If the request is |
| * outside the range of values, it returns the closest valid reg value. |
| */ |
| int bmi_get_reg_val(const int eng_val, const int round_up, |
| const struct bmi_accel_param_pair *pairs, const int size) |
| { |
| int i; |
| |
| for (i = 0; i < size - 1; i++) { |
| if (eng_val <= pairs[i].val) |
| break; |
| |
| if (eng_val < pairs[i + 1].val) { |
| if (round_up) |
| i += 1; |
| break; |
| } |
| } |
| return pairs[i].reg_val; |
| } |
| |
| /** |
| * @return engineering value that matches the given reg val |
| */ |
| int bmi_get_engineering_val(const int reg_val, |
| const struct bmi_accel_param_pair *pairs, |
| const int size) |
| { |
| int i; |
| |
| for (i = 0; i < size; i++) { |
| if (reg_val == pairs[i].reg_val) |
| break; |
| } |
| return pairs[i].val; |
| } |
| |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| static int bmi_spi_raw_read(const int addr, const uint8_t reg, uint8_t *data, |
| const int len) |
| { |
| uint8_t cmd = 0x80 | reg; |
| |
| return spi_transaction(&spi_devices[addr], &cmd, 1, data, len); |
| } |
| #endif |
| |
| /** |
| * Read 8bit register from accelerometer. |
| */ |
| int bmi_read8(const int port, const uint16_t i2c_spi_addr_flags, const int reg, |
| int *data_ptr) |
| { |
| int rv; |
| |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| { |
| uint8_t val; |
| |
| rv = bmi_spi_raw_read(ACCEL_GET_SPI_ADDR(i2c_spi_addr_flags), |
| reg, &val, 1); |
| if (rv == EC_SUCCESS) |
| *data_ptr = val; |
| } |
| #else |
| rv = i2c_read8(port, i2c_spi_addr_flags, reg, data_ptr); |
| #endif |
| return rv; |
| } |
| |
| /** |
| * Write 8bit register from accelerometer. |
| */ |
| int bmi_write8(const int port, const uint16_t i2c_spi_addr_flags, const int reg, |
| int data) |
| { |
| int rv; |
| |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| { |
| uint8_t cmd[2] = { reg, data }; |
| |
| rv = spi_transaction( |
| &spi_devices[ACCEL_GET_SPI_ADDR(i2c_spi_addr_flags)], |
| cmd, 2, NULL, 0); |
| } |
| #else |
| rv = i2c_write8(port, i2c_spi_addr_flags, reg, data); |
| #endif |
| /* |
| * From Bosch: BMI needs a delay of 450us after each write if it |
| * is in suspend mode, otherwise the operation may be ignored by |
| * the sensor. Given we are only doing write during init, add |
| * the delay unconditionally. |
| */ |
| msleep(1); |
| |
| return rv; |
| } |
| |
| /** |
| * Read 16bit register from accelerometer. |
| */ |
| int bmi_read16(const int port, const uint16_t i2c_spi_addr_flags, |
| const uint8_t reg, int *data_ptr) |
| { |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| return bmi_spi_raw_read(ACCEL_GET_SPI_ADDR(i2c_spi_addr_flags), reg, |
| (uint8_t *)data_ptr, 2); |
| #else |
| return i2c_read16(port, i2c_spi_addr_flags, reg, data_ptr); |
| #endif |
| } |
| |
| /** |
| * Write 16bit register from accelerometer. |
| */ |
| int bmi_write16(const int port, const uint16_t i2c_spi_addr_flags, |
| const int reg, int data) |
| { |
| int rv = -EC_ERROR_PARAM1; |
| |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| CPRINTS("%s() spi part is not implemented", __func__); |
| #else |
| rv = i2c_write16(port, i2c_spi_addr_flags, reg, data); |
| #endif |
| /* |
| * From Bosch: BMI needs a delay of 450us after each write if it |
| * is in suspend mode, otherwise the operation may be ignored by |
| * the sensor. Given we are only doing write during init, add |
| * the delay unconditionally. |
| */ |
| msleep(1); |
| return rv; |
| } |
| |
| /** |
| * Read 32bit register from accelerometer. |
| */ |
| int bmi_read32(const int port, const uint16_t i2c_spi_addr_flags, |
| const uint8_t reg, int *data_ptr) |
| { |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| return bmi_spi_raw_read(ACCEL_GET_SPI_ADDR(i2c_spi_addr_flags), reg, |
| (uint8_t *)data_ptr, 4); |
| #else |
| return i2c_read32(port, i2c_spi_addr_flags, reg, data_ptr); |
| #endif |
| } |
| |
| /** |
| * Read n bytes from accelerometer. |
| */ |
| int bmi_read_n(const int port, const uint16_t i2c_spi_addr_flags, |
| const uint8_t reg, uint8_t *data_ptr, const int len) |
| { |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| return bmi_spi_raw_read(ACCEL_GET_SPI_ADDR(i2c_spi_addr_flags), reg, |
| data_ptr, len); |
| #else |
| return i2c_read_block(port, i2c_spi_addr_flags, reg, data_ptr, len); |
| #endif |
| } |
| |
| /** |
| * Write n bytes from accelerometer. |
| */ |
| int bmi_write_n(const int port, const uint16_t i2c_spi_addr_flags, |
| const uint8_t reg, const uint8_t *data_ptr, const int len) |
| { |
| int rv = -EC_ERROR_PARAM1; |
| |
| #ifdef CONFIG_ACCELGYRO_BMI_COMM_SPI |
| CPRINTS("%s() spi part is not implemented", __func__); |
| #else |
| rv = i2c_write_block(port, i2c_spi_addr_flags, reg, data_ptr, len); |
| #endif |
| /* |
| * From Bosch: BMI needs a delay of 450us after each write if it |
| * is in suspend mode, otherwise the operation may be ignored by |
| * the sensor. Given we are only doing write during init, add |
| * the delay unconditionally. |
| */ |
| msleep(1); |
| |
| return rv; |
| } |
| /* |
| * Enable/Disable specific bit set of a 8-bit reg. |
| */ |
| int bmi_enable_reg8(const struct motion_sensor_t *s, int reg, uint8_t bits, |
| int enable) |
| { |
| if (enable) |
| return bmi_set_reg8(s, reg, bits, 0); |
| return bmi_set_reg8(s, reg, 0, bits); |
| } |
| |
| /* |
| * Set specific bit set to certain value of a 8-bit reg. |
| */ |
| int bmi_set_reg8(const struct motion_sensor_t *s, int reg, uint8_t bits, |
| int mask) |
| { |
| int ret, val; |
| |
| ret = bmi_read8(s->port, s->i2c_spi_addr_flags, reg, &val); |
| if (ret) |
| return ret; |
| val = (val & ~mask) | bits; |
| ret = bmi_write8(s->port, s->i2c_spi_addr_flags, reg, val); |
| return ret; |
| } |
| |
| void bmi_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *input) |
| { |
| int i; |
| struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); |
| |
| if (IS_ENABLED(CONFIG_MAG_BMI_BMM150) && |
| (s->type == MOTIONSENSE_TYPE_MAG)) { |
| bmm150_normalize(s, v, input); |
| } else if (IS_ENABLED(CONFIG_MAG_BMI_LIS2MDL) && |
| (s->type == MOTIONSENSE_TYPE_MAG)) { |
| lis2mdl_normalize(s, v, input); |
| } else { |
| v[0] = ((int16_t)((input[1] << 8) | input[0])); |
| v[1] = ((int16_t)((input[3] << 8) | input[2])); |
| v[2] = ((int16_t)((input[5] << 8) | input[4])); |
| } |
| rotate(v, *s->rot_standard_ref, v); |
| for (i = X; i <= Z; i++) |
| v[i] = SENSOR_APPLY_SCALE(v[i], data->scale[i]); |
| } |
| |
| int bmi_decode_header(struct motion_sensor_t *accel, enum fifo_header hdr, |
| uint32_t last_ts, uint8_t **bp, uint8_t *ep) |
| { |
| if ((hdr & BMI_FH_MODE_MASK) == BMI_FH_EMPTY && |
| (hdr & BMI_FH_PARM_MASK) != 0) { |
| int i, size = 0; |
| /* Check if there is enough space for the data frame */ |
| for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL; |
| i--) { |
| if (hdr & (1 << (i + BMI_FH_PARM_OFFSET))) |
| size += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6); |
| } |
| if (*bp + size > ep) { |
| /* frame is not complete, it will be retransmitted. */ |
| *bp = ep; |
| return 1; |
| } |
| for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL; |
| i--) { |
| struct motion_sensor_t *s = accel + i; |
| |
| if (hdr & (1 << (i + BMI_FH_PARM_OFFSET))) { |
| int *v = s->raw_xyz; |
| |
| bmi_normalize(s, v, *bp); |
| if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE) && |
| s->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE) |
| v = s->spoof_xyz; |
| if (IS_ENABLED(CONFIG_ACCEL_FIFO)) { |
| struct ec_response_motion_sensor_data |
| vector; |
| |
| vector.flags = 0; |
| vector.data[X] = v[X]; |
| vector.data[Y] = v[Y]; |
| vector.data[Z] = v[Z]; |
| vector.sensor_num = s - motion_sensors; |
| motion_sense_fifo_stage_data( |
| &vector, s, 3, last_ts); |
| } else { |
| motion_sense_push_raw_xyz(s); |
| } |
| *bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6); |
| } |
| } |
| |
| return 1; |
| } else { |
| return 0; |
| } |
| } |
| |
| enum fifo_state { |
| FIFO_HEADER, |
| FIFO_DATA_SKIP, |
| FIFO_DATA_TIME, |
| FIFO_DATA_CONFIG, |
| }; |
| |
| #define BMI_FIFO_BUFFER 64 |
| static uint8_t bmi_buffer[BMI_FIFO_BUFFER]; |
| |
| int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts) |
| { |
| struct bmi_drv_data_t *data = BMI_GET_DATA(s); |
| uint16_t length; |
| enum fifo_state state = FIFO_HEADER; |
| uint8_t *bp = bmi_buffer; |
| uint8_t *ep; |
| uint32_t beginning; |
| |
| if (s->type != MOTIONSENSE_TYPE_ACCEL) |
| return EC_SUCCESS; |
| |
| if (!(data->flags & (BMI_FIFO_ALL_MASK << BMI_FIFO_FLAG_OFFSET))) { |
| /* |
| * The FIFO was disabled while we were processing it. |
| * |
| * Flush potential left over: |
| * When sensor is resumed, we won't read old data. |
| */ |
| bmi_write8(s->port, s->i2c_spi_addr_flags, BMI_CMD_REG(V(s)), |
| BMI_CMD_FIFO_FLUSH); |
| return EC_SUCCESS; |
| } |
| |
| bmi_read_n(s->port, s->i2c_spi_addr_flags, BMI_FIFO_LENGTH_0(V(s)), |
| (uint8_t *)&length, sizeof(length)); |
| length &= BMI_FIFO_LENGTH_MASK(V(s)); |
| |
| /* |
| * We have not requested timestamp, no extra frame to read. |
| * if we have too much to read, read the whole buffer. |
| */ |
| if (length == 0) { |
| /* |
| * Disable this message on BMI260, due to this seems to always |
| * happen after we complete to read the data. |
| * TODO(chingkang): check why this happen on BMI260. |
| */ |
| if (V(s) == 0) |
| CPRINTS("unexpected empty FIFO"); |
| return EC_SUCCESS; |
| } |
| |
| /* Add one byte to get an empty FIFO frame.*/ |
| length++; |
| |
| if (length > sizeof(bmi_buffer)) |
| CPRINTS("unexpected large FIFO: %d", length); |
| length = MIN(length, sizeof(bmi_buffer)); |
| |
| bmi_read_n(s->port, s->i2c_spi_addr_flags, BMI_FIFO_DATA(V(s)), |
| bmi_buffer, length); |
| beginning = *(uint32_t *)bmi_buffer; |
| ep = bmi_buffer + length; |
| /* |
| * FIFO is invalid when reading while the sensors are all |
| * suspended. |
| * Instead of returning the empty frame, it can return a |
| * pattern that looks like a valid header: 84 or 40. |
| * If we see those, assume the sensors have been disabled |
| * while this thread was running. |
| */ |
| if (beginning == 0x84848484 || (beginning & 0xdcdcdcdc) == 0x40404040) { |
| CPRINTS("Suspended FIFO: accel ODR/rate: %d/%d: 0x%08x", |
| BASE_ODR(s->config[SENSOR_CONFIG_AP].odr), |
| BMI_GET_SAVED_DATA(s)->odr, beginning); |
| return EC_SUCCESS; |
| } |
| |
| while (bp < ep) { |
| switch (state) { |
| case FIFO_HEADER: { |
| enum fifo_header hdr = *bp++; |
| |
| if (bmi_decode_header(s, hdr, last_ts, &bp, ep)) |
| continue; |
| /* Other cases */ |
| hdr &= 0xdc; |
| switch (hdr) { |
| case BMI_FH_EMPTY: |
| return EC_SUCCESS; |
| case BMI_FH_SKIP: |
| state = FIFO_DATA_SKIP; |
| break; |
| case BMI_FH_TIME: |
| state = FIFO_DATA_TIME; |
| break; |
| case BMI_FH_CONFIG: |
| state = FIFO_DATA_CONFIG; |
| break; |
| default: |
| CPRINTS("Unknown header: 0x%02x @ %zd", hdr, |
| bp - bmi_buffer); |
| bmi_write8(s->port, s->i2c_spi_addr_flags, |
| BMI_CMD_REG(V(s)), |
| BMI_CMD_FIFO_FLUSH); |
| return EC_ERROR_NOT_HANDLED; |
| } |
| break; |
| } |
| case FIFO_DATA_SKIP: |
| CPRINTS("@ %zd - %d, skipped %d frames", |
| bp - bmi_buffer, length, *bp); |
| bp++; |
| state = FIFO_HEADER; |
| break; |
| case FIFO_DATA_CONFIG: |
| CPRINTS("@ %zd - %d, config change: 0x%02x", |
| bp - bmi_buffer, length, *bp); |
| bp++; |
| if (V(s)) |
| state = FIFO_DATA_TIME; |
| else |
| state = FIFO_HEADER; |
| break; |
| case FIFO_DATA_TIME: |
| if (bp + 3 > ep) { |
| bp = ep; |
| continue; |
| } |
| /* We are not requesting timestamp */ |
| CPRINTS("timestamp %d", |
| (bp[2] << 16) | (bp[1] << 8) | bp[0]); |
| state = FIFO_HEADER; |
| bp += 3; |
| break; |
| default: |
| CPRINTS("Unknown data: 0x%02x", *bp++); |
| state = FIFO_HEADER; |
| } |
| } |
| |
| return EC_SUCCESS; |
| } |
| |
| int bmi_set_range(struct motion_sensor_t *s, int range, int rnd) |
| { |
| int ret, range_tbl_size; |
| uint8_t reg_val, ctrl_reg; |
| const struct bmi_accel_param_pair *ranges; |
| |
| if (s->type == MOTIONSENSE_TYPE_MAG) { |
| s->current_range = range; |
| return EC_SUCCESS; |
| } |
| |
| ctrl_reg = BMI_RANGE_REG(s->type); |
| ranges = bmi_get_range_table(s, &range_tbl_size); |
| reg_val = bmi_get_reg_val(range, rnd, ranges, range_tbl_size); |
| |
| ret = bmi_write8(s->port, s->i2c_spi_addr_flags, ctrl_reg, reg_val); |
| /* Now that we have set the range, update the driver's value. */ |
| if (ret == EC_SUCCESS) |
| s->current_range = bmi_get_engineering_val(reg_val, ranges, |
| range_tbl_size); |
| return ret; |
| } |
| |
| int bmi_get_data_rate(const struct motion_sensor_t *s) |
| { |
| struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); |
| |
| return data->odr; |
| } |
| |
| int bmi_get_offset(const struct motion_sensor_t *s, int16_t *offset, |
| int16_t *temp) |
| { |
| int i, ret = EC_SUCCESS; |
| intv3_t v; |
| |
| switch (s->type) { |
| case MOTIONSENSE_TYPE_ACCEL: |
| /* |
| * The offset of the accelerometer off_acc_[xyz] is a 8 bit |
| * two-complement number in units of 3.9 mg independent of the |
| * range selected for the accelerometer. |
| */ |
| ret = bmi_accel_get_offset(s, v); |
| break; |
| case MOTIONSENSE_TYPE_GYRO: |
| /* |
| * The offset of the gyroscope off_gyr_[xyz] is a 10 bit |
| * two-complement number in units of 0.061 °/s. |
| * Therefore a maximum range that can be compensated is |
| * -31.25 °/s to +31.25 °/s |
| */ |
| ret = bmi_gyro_get_offset(s, v); |
| break; |
| #ifdef CONFIG_MAG_BMI_BMM150 |
| case MOTIONSENSE_TYPE_MAG: |
| ret = bmm150_get_offset(s, v); |
| break; |
| #endif /* defined(CONFIG_MAG_BMI_BMM150) */ |
| default: |
| for (i = X; i <= Z; i++) |
| v[i] = 0; |
| } |
| |
| if (ret != EC_SUCCESS) |
| return ret; |
| |
| rotate(v, *s->rot_standard_ref, v); |
| offset[X] = v[X]; |
| offset[Y] = v[Y]; |
| offset[Z] = v[Z]; |
| /* Saving temperature at calibration not supported yet */ |
| *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; |
| return EC_SUCCESS; |
| } |
| |
| #ifdef CONFIG_BODY_DETECTION |
| int bmi_get_rms_noise(const struct motion_sensor_t *accel, |
| int rms_noise_100hz_mg) |
| { |
| fp_t rate, sqrt_rate_ratio; |
| |
| /* change unit of ODR to Hz to prevent INT_TO_FP() overflow */ |
| rate = INT_TO_FP(bmi_get_data_rate(accel) / 1000); |
| /* |
| * Since the noise is proportional to sqrt(ODR) in BMI, and we |
| * have rms noise in 100 Hz, we multiply it with the sqrt(ratio |
| * of ODR to 100Hz) to get current noise. |
| */ |
| sqrt_rate_ratio = fp_sqrtf(fp_div(rate, INT_TO_FP(BMI_ACCEL_100HZ))); |
| return FP_TO_INT( |
| fp_mul(INT_TO_FP(rms_noise_100hz_mg), sqrt_rate_ratio)); |
| } |
| #endif |
| |
| int bmi_get_resolution(const struct motion_sensor_t *s) |
| { |
| return BMI_RESOLUTION; |
| } |
| |
| int bmi_set_scale(const struct motion_sensor_t *s, const uint16_t *scale, |
| int16_t temp) |
| { |
| struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); |
| |
| data->scale[X] = scale[X]; |
| data->scale[Y] = scale[Y]; |
| data->scale[Z] = scale[Z]; |
| return EC_SUCCESS; |
| } |
| |
| int bmi_get_scale(const struct motion_sensor_t *s, uint16_t *scale, |
| int16_t *temp) |
| { |
| struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s); |
| |
| scale[X] = data->scale[X]; |
| scale[Y] = data->scale[Y]; |
| scale[Z] = data->scale[Z]; |
| *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; |
| return EC_SUCCESS; |
| } |
| |
| int bmi_enable_fifo(const struct motion_sensor_t *s, int enable) |
| { |
| struct bmi_drv_data_t *data = BMI_GET_DATA(s); |
| int ret; |
| |
| /* FIFO start/stop collecting events */ |
| ret = bmi_enable_reg8(s, BMI_FIFO_CONFIG_1(V(s)), |
| BMI_FIFO_SENSOR_EN(V(s), s->type), enable); |
| if (ret) |
| return ret; |
| |
| if (enable) |
| data->flags |= 1 << (s->type + BMI_FIFO_FLAG_OFFSET); |
| else |
| data->flags &= ~(1 << (s->type + BMI_FIFO_FLAG_OFFSET)); |
| |
| return ret; |
| } |
| |
| int bmi_read(const struct motion_sensor_t *s, intv3_t v) |
| { |
| uint8_t data[6]; |
| int ret, status = 0; |
| |
| ret = bmi_read8(s->port, s->i2c_spi_addr_flags, BMI_STATUS(V(s)), |
| &status); |
| 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 (!(status & BMI_DRDY_MASK(s->type))) { |
| 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 = bmi_read_n(s->port, s->i2c_spi_addr_flags, bmi_get_xyz_reg(s), |
| data, 6); |
| |
| if (ret != EC_SUCCESS) { |
| CPRINTS("%s: type:0x%X RD XYZ Error %d", s->name, s->type, ret); |
| return ret; |
| } |
| bmi_normalize(s, v, data); |
| return EC_SUCCESS; |
| } |
| |
| int bmi_read_temp(const struct motion_sensor_t *s, int *temp_ptr) |
| { |
| return bmi_get_sensor_temp(s - motion_sensors, temp_ptr); |
| } |
| |
| int bmi_get_sensor_temp(int idx, int *temp_ptr) |
| { |
| struct motion_sensor_t *s = &motion_sensors[idx]; |
| int16_t temp; |
| int ret; |
| |
| ret = bmi_read_n(s->port, s->i2c_spi_addr_flags, |
| BMI_TEMPERATURE_0(V(s)), (uint8_t *)&temp, |
| sizeof(temp)); |
| |
| if (ret || temp == (int16_t)BMI_INVALID_TEMP) |
| return EC_ERROR_NOT_POWERED; |
| |
| *temp_ptr = C_TO_K(23 + ((temp + 256) >> 9)); |
| return 0; |
| } |
| |
| int bmi_get_normalized_rate(const struct motion_sensor_t *s, int rate, int rnd, |
| int *normalized_rate_ptr, uint8_t *reg_val_ptr) |
| { |
| *reg_val_ptr = BMI_ODR_TO_REG(rate); |
| *normalized_rate_ptr = BMI_REG_TO_ODR(*reg_val_ptr); |
| if (rnd && (*normalized_rate_ptr < rate)) { |
| (*reg_val_ptr)++; |
| *normalized_rate_ptr = BMI_REG_TO_ODR(*reg_val_ptr); |
| } |
| |
| switch (s->type) { |
| case MOTIONSENSE_TYPE_ACCEL: |
| if (*normalized_rate_ptr > BMI_ACCEL_MAX_FREQ || |
| *normalized_rate_ptr < BMI_ACCEL_MIN_FREQ) |
| return EC_RES_INVALID_PARAM; |
| break; |
| case MOTIONSENSE_TYPE_GYRO: |
| if (*normalized_rate_ptr > BMI_GYRO_MAX_FREQ || |
| *normalized_rate_ptr < BMI_GYRO_MIN_FREQ) |
| return EC_RES_INVALID_PARAM; |
| break; |
| #ifdef CONFIG_MAG_BMI_BMM150 |
| case MOTIONSENSE_TYPE_MAG: |
| /* We use the regular preset we can go about 100Hz */ |
| if (*reg_val_ptr > BMI_ODR_100HZ || |
| *reg_val_ptr < BMI_ODR_0_78HZ) |
| return EC_RES_INVALID_PARAM; |
| break; |
| #endif |
| |
| default: |
| return EC_RES_INVALID_PARAM; |
| } |
| return EC_SUCCESS; |
| } |
| |
| int bmi_accel_get_offset(const struct motion_sensor_t *accel, intv3_t v) |
| { |
| int i, val, ret; |
| |
| for (i = X; i <= Z; i++) { |
| ret = bmi_read8(accel->port, accel->i2c_spi_addr_flags, |
| BMI_OFFSET_ACC70(V(accel)) + i, &val); |
| if (ret != EC_SUCCESS) |
| return ret; |
| |
| if (val > 0x7f) |
| val = -256 + val; |
| v[i] = round_divide((int64_t)val * BMI_OFFSET_ACC_MULTI_MG, |
| BMI_OFFSET_ACC_DIV_MG); |
| } |
| |
| return EC_SUCCESS; |
| } |
| |
| int bmi_gyro_get_offset(const struct motion_sensor_t *gyro, intv3_t v) |
| { |
| int i, val, val98, ret; |
| |
| /* Read the MSB first */ |
| ret = bmi_read8(gyro->port, gyro->i2c_spi_addr_flags, |
| BMI_OFFSET_EN_GYR98(V(gyro)), &val98); |
| if (ret != EC_SUCCESS) |
| return ret; |
| |
| for (i = X; i <= Z; i++) { |
| ret = bmi_read8(gyro->port, gyro->i2c_spi_addr_flags, |
| BMI_OFFSET_GYR70(V(gyro)) + i, &val); |
| if (ret != EC_SUCCESS) |
| return ret; |
| |
| val |= ((val98 >> (2 * i)) & 0x3) << 8; |
| if (val > 0x1ff) |
| val = -1024 + val; |
| v[i] = round_divide((int64_t)val * BMI_OFFSET_GYRO_MULTI_MDS, |
| BMI_OFFSET_GYRO_DIV_MDS); |
| } |
| |
| return EC_SUCCESS; |
| } |
| |
| int bmi_set_accel_offset(const struct motion_sensor_t *accel, intv3_t v) |
| { |
| int i, val, ret; |
| |
| for (i = X; i <= Z; ++i) { |
| val = round_divide((int64_t)v[i] * BMI_OFFSET_ACC_DIV_MG, |
| BMI_OFFSET_ACC_MULTI_MG); |
| if (val > 127) |
| val = 127; |
| if (val < -128) |
| val = -128; |
| if (val < 0) |
| val = 256 + val; |
| ret = bmi_write8(accel->port, accel->i2c_spi_addr_flags, |
| BMI_OFFSET_ACC70(V(accel)) + i, val); |
| if (ret != EC_SUCCESS) |
| return ret; |
| } |
| |
| return EC_SUCCESS; |
| } |
| |
| int bmi_set_gyro_offset(const struct motion_sensor_t *gyro, intv3_t v, |
| int *val98_ptr) |
| { |
| int i, val, ret; |
| |
| for (i = X; i <= Z; i++) { |
| val = round_divide((int64_t)v[i] * BMI_OFFSET_GYRO_DIV_MDS, |
| BMI_OFFSET_GYRO_MULTI_MDS); |
| if (val > 511) |
| val = 511; |
| if (val < -512) |
| val = -512; |
| if (val < 0) |
| val = 1024 + val; |
| ret = bmi_write8(gyro->port, gyro->i2c_spi_addr_flags, |
| BMI_OFFSET_GYR70(V(gyro)) + i, val & 0xFF); |
| if (ret != EC_SUCCESS) |
| return ret; |
| |
| *val98_ptr &= ~(0x3 << (2 * i)); |
| *val98_ptr |= (val >> 8) << (2 * i); |
| } |
| |
| return EC_SUCCESS; |
| } |
| |
| #ifdef CONFIG_BMI_ORIENTATION_SENSOR |
| bool motion_orientation_changed(const struct motion_sensor_t *s) |
| { |
| return BMI_GET_DATA(s)->orientation != |
| BMI_GET_DATA(s)->last_orientation; |
| } |
| |
| enum motionsensor_orientation * |
| motion_orientation_ptr(const struct motion_sensor_t *s) |
| { |
| return &BMI_GET_DATA(s)->orientation; |
| } |
| |
| void motion_orientation_update(const struct motion_sensor_t *s) |
| { |
| BMI_GET_DATA(s)->last_orientation = BMI_GET_DATA(s)->orientation; |
| } |
| #endif |
| |
| int bmi_list_activities(const struct motion_sensor_t *s, uint32_t *enabled, |
| uint32_t *disabled) |
| { |
| struct bmi_drv_data_t *data = BMI_GET_DATA(s); |
| *enabled = data->enabled_activities; |
| *disabled = data->disabled_activities; |
| return EC_RES_SUCCESS; |
| } |