diff options
Diffstat (limited to 'driver/accelgyro_bmi_common.c')
| -rw-r--r-- | driver/accelgyro_bmi_common.c | 902 |
1 files changed, 0 insertions, 902 deletions
diff --git a/driver/accelgyro_bmi_common.c b/driver/accelgyro_bmi_common.c deleted file mode 100644 index 2da407427e..0000000000 --- a/driver/accelgyro_bmi_common.c +++ /dev/null @@ -1,902 +0,0 @@ -/* Copyright 2020 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. - */ - -/** - * BMI accelerometer and gyro module for Chrome EC - * 3D digital accelerometer & 3D digital gyroscope - */ - - -#include "accelgyro.h" -#include "console.h" -#include "accelgyro_bmi_common.h" -#include "mag_bmm150.h" -#include "mag_lis2mdl.h" -#include "i2c.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_BMI260) \ -&& !defined(CONFIG_ACCELGYRO_BMI3XX) -#error "Must use following sensors BMI160 BMI260 BMI3XX" -#endif - -#if defined(CONFIG_ACCELGYRO_BMI260) && !defined(CONFIG_ACCELGYRO_BMI160) -#define V(s_) 1 -#elif defined(CONFIG_ACCELGYRO_BMI160) && !defined(CONFIG_ACCELGYRO_BMI260) -#define V(s_) 0 -#else -#define V(s_) ((s_)->chip == MOTIONSENSE_CHIP_BMI260) -#endif -/* Index for which table to use. */ -#if !defined(CONFIG_ACCELGYRO_BMI160) || !defined(CONFIG_ACCELGYRO_BMI260) -#define T(s_) 0 -#else -#define T(s_) V(s_) -#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 -#ifdef 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 -#ifdef 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))) { - struct ec_response_motion_sensor_data vector; - int *v = s->raw_xyz; - - vector.flags = 0; - bmi_normalize(s, v, *bp); - if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE) && - s->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE) - v = s->spoof_xyz; - 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); - *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 *s) -{ - int ret; - fp_t noise_100hz, rate, sqrt_rate_ratio; - - switch (s->type) { - case MOTIONSENSE_TYPE_ACCEL: - /* change unit of ODR to Hz to prevent INT_TO_FP() overflow */ - rate = INT_TO_FP(bmi_get_data_rate(s) / 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. - */ - noise_100hz = INT_TO_FP(BMI_ACCEL_RMS_NOISE_100HZ(V(s))); - sqrt_rate_ratio = - fp_sqrtf(fp_div(rate, INT_TO_FP(BMI_ACCEL_100HZ))); - ret = FP_TO_INT(fp_mul(noise_100hz, sqrt_rate_ratio)); - break; - default: - CPRINTS("%s with gyro/mag is not implemented", __func__); - return 0; - } - return ret; -} -#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; -} |