diff options
Diffstat (limited to 'driver/accelgyro_bmi3xx.c')
| -rw-r--r-- | driver/accelgyro_bmi3xx.c | 1132 |
1 files changed, 0 insertions, 1132 deletions
diff --git a/driver/accelgyro_bmi3xx.c b/driver/accelgyro_bmi3xx.c deleted file mode 100644 index ee9b30a9cf..0000000000 --- a/driver/accelgyro_bmi3xx.c +++ /dev/null @@ -1,1132 +0,0 @@ -/* Copyright 2021 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. - */ - -/** - * BMI3XX accelerometer and gyroscope module for Chrome EC - * 3D digital accelerometer & 3D digital gyroscope - */ - -#include "accelgyro.h" -#include "accelgyro_bmi323.h" -#include "accelgyro_bmi_common.h" -#include "console.h" -#include "hwtimer.h" -#include "i2c.h" -#include "init_rom.h" -#include "math_util.h" -#include "motion_sense_fifo.h" -#include "spi.h" -#include "task.h" -#include "timer.h" -#include "util.h" -#include "watchdog.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) - -/* Sensor definition */ -STATIC_IF(CONFIG_BMI_ORIENTATION_SENSOR) void irq_set_orientation( - struct motion_sensor_t *s); - -STATIC_IF(CONFIG_ACCEL_FIFO) volatile uint32_t last_interrupt_timestamp; - -static inline int bmi3_read_n(const struct motion_sensor_t *s, const int reg, - uint8_t *data_ptr, const int len) -{ - return bmi_read_n(s->port, s->i2c_spi_addr_flags, reg, data_ptr, len); -} - -static inline int bmi3_write_n(const struct motion_sensor_t *s, const int reg, - uint8_t *data_ptr, const int len) -{ - return bmi_write_n(s->port, s->i2c_spi_addr_flags, reg, data_ptr, len); -} - -#ifdef CONFIG_ACCEL_INTERRUPTS - -#ifdef CONFIG_BMI_ORIENTATION_SENSOR - -static void irq_set_orientation(struct motion_sensor_t *s) -{ - int ret; - uint8_t reg_data[4]; - uint8_t orient_data; - - enum motionsensor_orientation orientation = - MOTIONSENSE_ORIENTATION_UNKNOWN; - - RETURN_ERROR(bmi3_read_n(s, BMI3_FEATURE_EVENT_EXT, reg_data, 4)); - - orient_data = reg_data[2] & BMI3_PORTRAIT_LANDSCAPE_MASK; - - if (BMI_GET_DATA(s)->raw_orientation != orient_data) { - BMI_GET_DATA(s)->raw_orientation = orient_data; - - switch (orient_data) { - case BMI3_ORIENT_PORTRAIT: - orientation = MOTIONSENSE_ORIENTATION_PORTRAIT; - break; - case BMI3_PORTRAIT_INVERT: - orientation = - MOTIONSENSE_ORIENTATION_UPSIDE_DOWN_PORTRAIT; - break; - case BMI3_LANDSCAPE: - orientation = MOTIONSENSE_ORIENTATION_LANDSCAPE; - break; - case BMI3_LANDSCAPE_INVERT: - orientation = - MOTIONSENSE_ORIENTATION_UPSIDE_DOWN_LANDSCAPE; - break; - default: - break; - } - - orientation = motion_orientation_remap(s, orientation); - *motion_orientation_ptr(s) = orientation; - } -} - -#endif /* CONFIG_BMI_ORIENTATION_SENSOR */ - -/* - * bmi3xx_interrupt - called when the sensor activates the interrupt line. - * - * This is a "top half" interrupt handler, it just asks motion sense ask - * to schedule the "bottom half", ->irq_handler(). - */ -void bmi3xx_interrupt(enum gpio_signal signal) -{ - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - last_interrupt_timestamp = __hw_clock_source_read(); - - task_set_event(TASK_ID_MOTIONSENSE, CONFIG_ACCELGYRO_BMI3XX_INT_EVENT); -} - -static int enable_fifo(const struct motion_sensor_t *s, int enable) -{ - struct bmi_drv_data_t *data = BMI_GET_DATA(s); - /* Set FIFO config to enable accel gyro data */ - uint8_t reg_data[4]; - - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_FIFO_CONF, reg_data, 4)); - - if (enable) { - if (s->type == MOTIONSENSE_TYPE_ACCEL) - reg_data[3] |= BMI3_FIFO_ACC_EN; - else - reg_data[3] |= BMI3_FIFO_GYR_EN; - - data->flags |= 1 << (s->type + BMI_FIFO_FLAG_OFFSET); - } else { - if (s->type == MOTIONSENSE_TYPE_ACCEL) - reg_data[3] &= ~BMI3_FIFO_ACC_EN; - else - reg_data[3] &= ~BMI3_FIFO_GYR_EN; - - data->flags &= ~(1 << (s->type + BMI_FIFO_FLAG_OFFSET)); - } - - return bmi3_write_n(s, BMI3_REG_FIFO_CONF, ®_data[2], 2); -} - -static int config_interrupt(const struct motion_sensor_t *s) -{ - int ret; - uint8_t reg_data[6] = {0}; - - if (s->type != MOTIONSENSE_TYPE_ACCEL) - return EC_SUCCESS; - - mutex_lock(s->mutex); - - /* Clear the FIFO using Flush command */ - reg_data[0] = BMI3_ENABLE; - reg_data[1] = 0; - ret = bmi3_write_n(s, BMI3_REG_FIFO_CTRL, reg_data, 2); - if (ret) - goto err_unlock; - - /* Map FIFO water-mark and FIFO full to INT1 pin */ - ret = bmi3_read_n(s, BMI3_REG_INT_MAP1, reg_data, 6); - if (ret) - goto err_unlock; - - reg_data[5] = BMI3_SET_BITS(reg_data[5], BMI3_FWM_INT, BMI3_INT1); - reg_data[5] = BMI3_SET_BITS(reg_data[5], BMI3_FFULL_INT, BMI3_INT1); - if (IS_ENABLED(CONFIG_BMI_ORIENTATION_SENSOR)) { - /* Map orientation to INT1 pin */ - reg_data[2] = BMI3_SET_BITS(reg_data[2], BMI3_ORIENT_INT, - BMI3_INT1); - } - - ret = bmi3_write_n(s, BMI3_REG_INT_MAP1, ®_data[2], 4); - if (ret) - goto err_unlock; - - /* Set FIFO water-mark to read data whenever available */ - reg_data[0] = BMI3_FIFO_ENTRY; - reg_data[1] = 0; - - ret = bmi3_write_n(s, BMI3_REG_FIFO_WATERMARK, reg_data, 2); - if (ret) - goto err_unlock; - - /* Get the previous configuration data */ - ret = bmi3_read_n(s, BMI3_REG_IO_INT_CTRL, reg_data, 4); - if (ret) - goto err_unlock; - - reg_data[2] = BMI3_SET_BIT_POS0(reg_data[2], BMI3_INT1_LVL, - BMI3_INT_ACTIVE_LOW); - - reg_data[2] = BMI3_SET_BITS(reg_data[2], BMI3_INT1_OD, - BMI3_INT_PUSH_PULL); - - reg_data[2] = BMI3_SET_BITS(reg_data[2], BMI3_INT1_OUTPUT_EN, - BMI3_INT_OUTPUT_ENABLE); - - /* - * Set the interrupt pin configurations - */ - ret = bmi3_write_n(s, BMI3_REG_IO_INT_CTRL, ®_data[2], 2); - if (ret) - goto err_unlock; - - if (IS_ENABLED(CONFIG_BMI_ORIENTATION_SENSOR)) { - /* Enable the orientation feature in BMI3 */ - ret = bmi3_read_n(s, BMI3_FEATURE_IO_0, reg_data, 4); - if (ret) - goto err_unlock; - - reg_data[2] |= BMI3_ANY_MOTION_X_EN_MASK; - ret = bmi3_write_n(s, BMI3_FEATURE_IO_0, ®_data[2], 2); - if (ret) - goto err_unlock; - - /* Write to feature engine */ - reg_data[0] = 1; - reg_data[1] = 0; - ret = bmi3_write_n(s, BMI3_FEATURE_IO_STATUS, reg_data, 2); - } - -err_unlock: - mutex_unlock(s->mutex); - return ret; -} - -static void bmi3_parse_fifo_data(struct motion_sensor_t *s, - struct bmi3_fifo_frame *fifo_frame, - uint32_t last_ts) -{ - struct bmi_drv_data_t *data = BMI_GET_DATA(s); - struct ec_response_motion_sensor_data vect; - uint16_t reg_data; - intv3_t v; - int i; - - /* Start index for FIFO parsing after I2C sync word removal */ - size_t fifo_index = 1; - - /* Variable to store I2C sync data which will get in FIFO data */ - uint16_t i2c_sync_data, fifo_size; - - 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. - */ - - /* Clear the FIFO using Flush command */ - reg_data = BMI3_ENABLE; - bmi3_write_n(s, BMI3_REG_FIFO_CTRL, (uint8_t *)®_data, 2); - return; - } - - /* Parse the length of data read excluding I2C sync word */ - fifo_size = fifo_frame->available_fifo_len - 1; - - while (fifo_size > 0) { - for (i = 0; i < NUM_OF_PRIMARY_SENSOR; i++) { - struct motion_sensor_t *sens_output = s + i; - - if (data->flags & BIT(i + BMI_FIFO_FLAG_OFFSET)) { - /* - * In-case of FIFO read fail it has only - * 0x8000. - */ - if (fifo_frame->data[fifo_index] == 0x8000) - break; - - /* - * In case the frame has been cut, FIFO was - * greater than our buffer. - */ - if (fifo_size < BMI3_FIFO_ENTRY) - break; - - /* Frame is complete, but may have no data. */ - fifo_size -= BMI3_FIFO_ENTRY; - i2c_sync_data = fifo_frame->data[fifo_index++]; - if (i2c_sync_data == - BMI3_FIFO_ACCEL_I2C_SYNC_FRAME + i) { - fifo_index += 2; - continue; - } - - v[X] = i2c_sync_data; - v[Y] = fifo_frame->data[fifo_index++]; - v[Z] = fifo_frame->data[fifo_index++]; - - rotate(v, *sens_output->rot_standard_ref, v); - - vect.data[X] = v[X]; - vect.data[Y] = v[Y]; - vect.data[Z] = v[Z]; - vect.flags = 0; - vect.sensor_num = sens_output - motion_sensors; - motion_sense_fifo_stage_data(&vect, - sens_output, 3, last_ts); - } - } - } -} - -/* - * irq_handler - bottom half of the interrupt stack. - * Ran from the motion_sense task, finds the events that raised the interrupt. - * - * For now, we just print out. We should set a bitmask motion sense code will - * act upon. - */ -static int irq_handler(struct motion_sensor_t *s, - uint32_t *event) -{ - bool has_read_fifo = false; - uint16_t int_status[2]; - uint16_t reg_data[2]; - struct bmi3_fifo_frame fifo_frame; - - if ((s->type != MOTIONSENSE_TYPE_ACCEL) || - (!(*event & CONFIG_ACCELGYRO_BMI3XX_INT_EVENT))) - return EC_ERROR_NOT_HANDLED; - - /* Get the interrupt status */ - do { - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_INT_STATUS_INT1, - (uint8_t *)int_status, 4)); - - if (IS_ENABLED(CONFIG_BMI_ORIENTATION_SENSOR) && - (BMI3_INT_STATUS_ORIENTATION & int_status[1])) - irq_set_orientation(s); - - if ((int_status[1] & - (BMI3_INT_STATUS_FWM | BMI3_INT_STATUS_FFULL)) == 0) - break; - - /* Get the FIFO fill level in words */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_FIFO_FILL_LVL, - (uint8_t *)reg_data, 4)); - - reg_data[1] = BMI3_GET_BIT_POS0(reg_data[1], - BMI3_FIFO_FILL_LVL); - - /* Add space for the initial 16bit read. */ - fifo_frame.available_fifo_len = reg_data[1] + 1; - - /* - * If fill level is greater than buffer size then wrap it to - * buffer size. - */ - if (fifo_frame.available_fifo_len > ARRAY_SIZE(fifo_frame.data)) - CPRINTS("unexpected large FIFO: %d", - fifo_frame.available_fifo_len); - - fifo_frame.available_fifo_len = - MIN(fifo_frame.available_fifo_len, - ARRAY_SIZE(fifo_frame.data)); - /* Read FIFO data */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_FIFO_DATA, - (uint8_t *)fifo_frame.data, - fifo_frame.available_fifo_len * - sizeof(uint16_t))); - - bmi3_parse_fifo_data(s, &fifo_frame, last_interrupt_timestamp); - has_read_fifo = true; - } while (true); - - if (IS_ENABLED(CONFIG_ACCEL_FIFO) && has_read_fifo) - motion_sense_fifo_commit_data(); - - return EC_SUCCESS; -} -#endif /* CONFIG_ACCEL_INTERRUPTS */ - -static int read_temp(const struct motion_sensor_t *s, int *temp_ptr) -{ - return EC_ERROR_UNIMPLEMENTED; -} - -int get_gyro_offset(const struct motion_sensor_t *s, intv3_t v) -{ - int i; - uint8_t reg_data[14] = { 0 }; - - /* Get the accel offset values */ - RETURN_ERROR(bmi3_read_n(s, GYR_DP_OFF_X, reg_data, 14)); - - v[0] = ((uint16_t)(reg_data[3] << 8) | reg_data[2]) & 0x03FF; - v[1] = ((uint16_t)(reg_data[7] << 8) | reg_data[6]) & 0x03FF; - v[2] = ((uint16_t)(reg_data[11] << 8) | reg_data[10]) & 0x03FF; - - for (i = X; i <= Z; ++i) { - if (v[i] > 0x01FF) - v[i] = -1024 + v[i]; - - v[i] = round_divide((int64_t)v[i] * BMI_OFFSET_GYRO_MULTI_MDS, - BMI_OFFSET_GYRO_DIV_MDS); - } - - return EC_SUCCESS; -} - -int set_gyro_offset(const struct motion_sensor_t *s, intv3_t v) -{ - uint8_t reg_data[6] = { 0 }; - uint8_t base_addr[2] = { BMI3_GYRO_OFFSET_ADDR, 0 }; - int i, val[3]; - - for (i = X; i <= Z; ++i) { - val[i] = round_divide((int64_t)v[i] * BMI_OFFSET_GYRO_DIV_MDS, - BMI_OFFSET_GYRO_MULTI_MDS); - if (val[i] > 511) - val[i] = 511; - if (val[i] < -512) - val[i] = -512; - if (val[i] < 0) - val[i] = 1024 + val[i]; - } - - /* - * Set the user accel offset base address to feature engine - * transmission address to start DMA transaction - */ - RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, base_addr, 2)); - - reg_data[0] = (uint8_t)(val[0] & BMI3_SET_LOW_BYTE); - reg_data[1] = (uint8_t)((val[0] & 0x0300) >> 8); - reg_data[2] = (uint8_t)(val[1] & BMI3_SET_LOW_BYTE); - reg_data[3] = (uint8_t)((val[1] & 0x0300) >> 8); - reg_data[4] = (uint8_t)(val[2] & BMI3_SET_LOW_BYTE); - reg_data[5] = (uint8_t)((val[2] & 0x0300) >> 8); - - /* Set the configuration to the feature engine register */ - RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, - 6)); - - /* Update the offset to the sensor engine */ - reg_data[0] = (uint8_t)(BMI3_CMD_USR_GAIN_OFFS_UPDATE & - BMI3_SET_LOW_BYTE); - reg_data[1] = (uint8_t)((BMI3_CMD_USR_GAIN_OFFS_UPDATE & - BMI3_SET_HIGH_BYTE) >> 8); - RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2)); - - return EC_SUCCESS; -} - -int get_accel_offset(const struct motion_sensor_t *s, intv3_t v) -{ - int i; - uint8_t reg_data[14] = { 0 }; - - /* Get the accel offset values from user registers */ - RETURN_ERROR(bmi3_read_n(s, ACC_DP_OFF_X, reg_data, 14)); - - v[0] = ((uint16_t)(reg_data[3] << 8) | reg_data[2]) & 0x1FFF; - v[1] = ((uint16_t)(reg_data[7] << 8) | reg_data[6]) & 0x1FFF; - v[2] = ((uint16_t)(reg_data[11] << 8) | reg_data[10]) & 0x1FFF; - - for (i = X; i <= Z; ++i) { - if (v[i] > 0x0FFF) - v[i] = -8192 + v[i]; - - v[i] = round_divide((int64_t)v[i] * BMI3_OFFSET_ACC_MULTI_MG, - BMI_OFFSET_ACC_DIV_MG); - } - - return EC_SUCCESS; -} - -int set_accel_offset(const struct motion_sensor_t *s, intv3_t v) -{ - uint8_t reg_data[6] = { 0 }; - uint8_t base_addr[2] = { BMI3_ACC_OFFSET_ADDR, 0 }; - uint8_t saved_conf[6] = { 0 }; - int i, val[3]; - - for (i = X; i <= Z; ++i) { - val[i] = round_divide((int64_t)v[i] * BMI_OFFSET_ACC_DIV_MG, - BMI3_OFFSET_ACC_MULTI_MG); - if (val[i] > 4095) - val[i] = 4095; - if (val[i] < -4096) - val[i] = -4096; - if (val[i] < 0) - val[i] += 8192; - } - - /* Set the power mode as suspend */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_CONF, saved_conf, 6)); - - /* Ignore two i2c sync bytes and store consecutive bytes in reg_data */ - reg_data[0] = saved_conf[2]; - reg_data[1] = 0x00; - reg_data[2] = saved_conf[4]; - reg_data[3] = 0x00; - RETURN_ERROR(bmi3_write_n(s, BMI3_REG_ACC_CONF, reg_data, 4)); - - /* - * Set the user accel offset base address to feature engine - * transmission address to start DMA transaction - */ - RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, base_addr, 2)); - - reg_data[0] = (uint8_t)(val[0] & BMI3_SET_LOW_BYTE); - reg_data[1] = (uint8_t)((val[0] & 0x1F00) >> 8); - reg_data[2] = (uint8_t)(val[1] & BMI3_SET_LOW_BYTE); - reg_data[3] = (uint8_t)((val[1] & 0x1F00) >> 8); - reg_data[4] = (uint8_t)(val[2] & BMI3_SET_LOW_BYTE); - reg_data[5] = (uint8_t)((val[2] & 0x1F00) >> 8); - - /* Set the configuration to the feature engine register */ - RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, - 6)); - - /* Restore ACC_CONF by storing saved_conf data */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_CONF, saved_conf, 6)); - - /* Update the offset to the sensor engine */ - reg_data[0] = (uint8_t)(BMI3_CMD_USR_GAIN_OFFS_UPDATE & - BMI3_SET_LOW_BYTE); - - reg_data[1] = (uint8_t)((BMI3_CMD_USR_GAIN_OFFS_UPDATE & - BMI3_SET_HIGH_BYTE) >> 8); - - RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2)); - - return EC_SUCCESS; -} - -static int wait_and_read_data(const struct motion_sensor_t *s, - intv3_t accel_data) -{ - uint8_t reg_data[8] = {0}; - - /* Retry 5 times */ - uint8_t try_cnt = FOC_TRY_COUNT; - - /* Check if data is ready */ - while (try_cnt && (!(reg_data[2] & BMI3_STAT_DATA_RDY_ACCEL_MSK))) { - /* 20ms delay for 50Hz ODR */ - msleep(FOC_DELAY); - - /* Read the status register */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_STATUS, reg_data, 4)); - try_cnt--; - } - - if (!(reg_data[2] & BMI3_STAT_DATA_RDY_ACCEL_MSK)) - return EC_ERROR_TIMEOUT; - - /* Read the sensor data */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_DATA_X, reg_data, 8)); - - accel_data[0] = ((int16_t)((reg_data[3] << 8) | reg_data[2])); - accel_data[1] = ((int16_t)((reg_data[5] << 8) | reg_data[4])); - accel_data[2] = ((int16_t)((reg_data[7] << 8) | reg_data[6])); - - rotate(accel_data, *s->rot_standard_ref, accel_data); - - return EC_SUCCESS; -} - -/*! - * @brief This internal API performs Fast Offset Compensation for accelerometer. - */ -static int8_t perform_accel_foc(struct motion_sensor_t *s, int *target, - int sens_range) -{ - intv3_t accel_data, offset; - int32_t delta_value[3] = {0, 0, 0}; - - /* Variable to define count */ - uint8_t i, loop, sample_count = 0; - - for (loop = 0; loop < BMI3_FOC_SAMPLE_LIMIT; loop++) { - - RETURN_ERROR(wait_and_read_data(s, accel_data)); - - sample_count++; - - /* Store the data in a temporary structure */ - delta_value[0] += accel_data[0] - target[X]; - delta_value[1] += accel_data[1] - target[Y]; - delta_value[2] += accel_data[2] - target[Z]; - } - - /* The data is in LSB so -> [(LSB)*1000*range/2^15] (mdps | mg) */ - for (i = X; i <= Z; ++i) { - offset[i] = (((int64_t)(delta_value[i] * 1000 * sens_range - / sample_count) >> 15) * -1); - } - - rotate_inv(offset, *s->rot_standard_ref, offset); - - RETURN_ERROR(set_accel_offset(s, offset)); - - return EC_SUCCESS; -} - -static int set_gyro_foc_config(struct motion_sensor_t *s) -{ - uint8_t reg_data[4] = { 0 }; - uint8_t base_addr[2] = { BMI3_BASE_ADDR_SC, 0 }; - - /* - * Set the user accel offset base address to feature engine - * transmission address to start DMA transaction - */ - RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, base_addr, 2)); - - /* Read the configuration from the feature engine register */ - RETURN_ERROR(bmi3_read_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, - 4)); - /* Enable self calibration */ - reg_data[2] |= 0x07; - - RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, base_addr, 2)); - - /* Set the configuration to the feature engine register */ - RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, - ®_data[2], 2)); - - /* Trigger bmi3 gyro self calibration */ - reg_data[0] = (uint8_t)(BMI3_CMD_SELF_CALIB & BMI3_SET_LOW_BYTE); - reg_data[1] = (uint8_t)((BMI3_CMD_SELF_CALIB & BMI3_SET_HIGH_BYTE) - >> 8); - - RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2)); - - return EC_SUCCESS; -} - -static int get_calib_result(struct motion_sensor_t *s) -{ - uint8_t i, reg_data[4]; - - for (i = 0; i < 25; i++) { - /* A delay of 120ms is required to read this status register */ - msleep(120); - - /* Read the configuration from the feature engine register */ - RETURN_ERROR(bmi3_read_n(s, BMI3_FEATURE_IO_1, reg_data, 4)); - - switch (s->type) { - case MOTIONSENSE_TYPE_ACCEL: - if ((reg_data[3] & BMI3_UGAIN_OFFS_UPD_COMPLETE) - && ((reg_data[2] & BMI3_FEATURE_IO_1_ERROR_MASK) - == BMI3_FEATURE_IO_1_NO_ERROR)) { - return EC_SUCCESS; - } - break; - case MOTIONSENSE_TYPE_GYRO: - if (reg_data[2] & BMI3_SC_ST_STATUS_MASK) { - /* Check calibration result */ - if (reg_data[2] & BMI3_SC_RESULT_MASK) - return EC_SUCCESS; - } - break; - default: - return EC_ERROR_UNIMPLEMENTED; - } - } - - return EC_ERROR_NOT_CALIBRATED; -} - -static int perform_calib(struct motion_sensor_t *s, int enable) -{ - int ret; - intv3_t target = {0, 0, 0}; - uint8_t saved_conf[6] = {0}; - - /* Sensor is configured to be in 16G range */ - int sens_range = 16; - - /* Variable to set the accelerometer configuration value 50Hz for FOC */ - uint8_t acc_conf_data[2] = {BMI3_FOC_ACC_CONF_VAL_LSB, - BMI3_FOC_ACC_CONF_VAL_MSB}; - - if (!enable) - return EC_SUCCESS; - - /* Get default configurations for the type of feature selected. */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_CONF, saved_conf, - 6)); - - ret = bmi3_write_n(s, BMI3_REG_ACC_CONF, acc_conf_data, 2); - if (ret) - goto end_calib; - - msleep(FOC_DELAY); - - switch (s->type) { - case MOTIONSENSE_TYPE_ACCEL: - target[Z] = BMI3_ACC_DATA_PLUS_1G(sens_range); - - /* Perform accel calibration */ - ret = perform_accel_foc(s, target, sens_range); - if (ret) - goto end_calib; - - /* Get caliration results */ - ret = get_calib_result(s); - if (ret) - goto end_calib; - - break; - case MOTIONSENSE_TYPE_GYRO: - ret = set_gyro_foc_config(s); - if (ret) - goto end_calib; - - ret = get_calib_result(s); - if (ret) - goto end_calib; - - break; - default: - /* Not supported on Magnetometer */ - ret = EC_RES_INVALID_PARAM; - goto end_calib; - } - - -end_calib: - /* Restore ACC_CONF before exiting */ - RETURN_ERROR(bmi3_write_n(s, BMI3_REG_ACC_CONF, &saved_conf[2], 4)); - - return ret; -} - -static int get_offset(const struct motion_sensor_t *s, int16_t *offset, - int16_t *temp) -{ - int i; - intv3_t v; - - switch (s->type) { - case MOTIONSENSE_TYPE_ACCEL: - /* - * The offset of the accelerometer is a 8 bit - * two-complement number in units of 3.9 mg independent of the - * range selected for the accelerometer. - */ - RETURN_ERROR(get_accel_offset(s, v)); - break; - case MOTIONSENSE_TYPE_GYRO: - /* Gyro offset is in milli-dps */ - RETURN_ERROR(get_gyro_offset(s, v)); - break; - default: - for (i = X; i <= Z; i++) - v[i] = 0; - } - - 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 = (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP; - - return EC_SUCCESS; -} - -static int set_offset(const struct motion_sensor_t *s, - const int16_t *offset, - int16_t temp) -{ - intv3_t v = { offset[X], offset[Y], offset[Z] }; - (void)temp; - - rotate_inv(v, *s->rot_standard_ref, v); - - switch (s->type) { - case MOTIONSENSE_TYPE_ACCEL: - /* Offset should be in units of mg */ - RETURN_ERROR(set_accel_offset(s, v)); - break; - case MOTIONSENSE_TYPE_GYRO: - /* Offset should be in units of mdps */ - RETURN_ERROR(set_gyro_offset(s, v)); - break; - default: - return EC_RES_INVALID_PARAM; - } - - return EC_SUCCESS; -} - -#ifdef CONFIG_BODY_DETECTION -int get_rms_noise(const struct motion_sensor_t *s) -{ - return EC_ERROR_UNIMPLEMENTED; -} -#endif - -static int set_scale(const struct motion_sensor_t *s, const uint16_t *scale, - int16_t temp) -{ - struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s); - - saved_data->scale[X] = scale[X]; - saved_data->scale[Y] = scale[Y]; - saved_data->scale[Z] = scale[Z]; - - return EC_SUCCESS; -} - -static int get_scale(const struct motion_sensor_t *s, uint16_t *scale, - int16_t *temp) -{ - struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s); - - scale[X] = saved_data->scale[X]; - scale[Y] = saved_data->scale[Y]; - scale[Z] = saved_data->scale[Z]; - - *temp = (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP; - - return EC_SUCCESS; -} - - -static int get_data_rate(const struct motion_sensor_t *s) -{ - struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s); - - return saved_data->odr; -} - -static int set_data_rate(const struct motion_sensor_t *s, - int rate, int rnd) -{ - int ret; - int normalized_rate = 0; - uint8_t reg_data[4]; - uint8_t reg_val = 0; - - struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s); - - if (rate > 0) - RETURN_ERROR(bmi_get_normalized_rate(s, rate, rnd, - &normalized_rate, ®_val)); - - /* - * Lock accel resource to prevent another task from attempting - * to write accel parameters until we are done. - */ - mutex_lock(s->mutex); - - /* - * Get default configurations for the type of feature selected. - */ - ret = bmi3_read_n(s, BMI3_REG_ACC_CONF + s->type, reg_data, 4); - if (ret) { - mutex_unlock(s->mutex); - return ret; - } - - if (s->type == MOTIONSENSE_TYPE_ACCEL) { - if (rate == 0) { - /* FIFO stop collecting events */ - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - ret = enable_fifo(s, 0); - - /* - * Disable accel to set rate equal to zero. - * Accel does not have suspend mode. - */ - reg_data[3] = BMI3_SET_BITS(reg_data[3], - BMI3_POWER_MODE, - BMI3_ACC_MODE_DISABLE); - - saved_data->odr = 0; - } else if (saved_data->odr == 0) { - /* - * Power mode changed from suspend to - * normal - */ - reg_data[3] = BMI3_SET_BITS(reg_data[3], - BMI3_POWER_MODE, - BMI3_ACC_MODE_NORMAL); - } - } else if (s->type == MOTIONSENSE_TYPE_GYRO) { - if (rate == 0) { - /* FIFO stop collecting events */ - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - ret = enable_fifo(s, 0); - - /* - * Set gyro to suspend mode to disable gyro - * however keep internal driver enabled - */ - reg_data[3] = BMI3_SET_BITS(reg_data[3], - BMI3_POWER_MODE, - BMI3_GYR_MODE_SUSPEND); - - saved_data->odr = 0; - } else if (saved_data->odr == 0) { - /* Power mode changed from suspend to - * normal - */ - reg_data[3] = BMI3_SET_BITS(reg_data[3], - BMI3_POWER_MODE, - BMI3_GYR_MODE_NORMAL); - } - } - - /* Set accelerometer ODR */ - reg_data[2] = BMI3_SET_BIT_POS0(reg_data[2], BMI3_SENS_ODR, reg_val); - - /* Set the accel/gyro configurations. */ - ret = bmi3_write_n(s, BMI3_REG_ACC_CONF + s->type, ®_data[2], 2); - if (ret) { - mutex_unlock(s->mutex); - return ret; - } - - saved_data->odr = normalized_rate; - - /* - * If rate is non zero, FIFO start collecting events. - * They will be discarded if AP does not want them. - */ - if (rate > 0) - ret = enable_fifo(s, 1); - - mutex_unlock(s->mutex); - return ret; -} - -static int get_resolution(const struct motion_sensor_t *s) -{ - return BMI3_16_BIT_RESOLUTION; -} - -static int set_range(struct motion_sensor_t *s, int range, int rnd) -{ - int ret; - uint8_t index, sens_size = 0; - uint8_t reg_data[4] = { 0 }; - int (*sensor_range)[2]; - - int acc_sensor_range[4][2] = { - { 2, BMI3_ACC_RANGE_2G }, - { 4, BMI3_ACC_RANGE_4G }, - { 8, BMI3_ACC_RANGE_8G }, - { 16, BMI3_ACC_RANGE_16G }, - }; - - int gyr_sensor_range[5][2] = { - { 125, BMI3_GYR_RANGE_125DPS }, - { 250, BMI3_GYR_RANGE_250DPS }, - { 500, BMI3_GYR_RANGE_500DPS }, - { 1000, BMI3_GYR_RANGE_1000DPS }, - { 2000, BMI3_GYR_RANGE_2000DPS }, - }; - - if (s->type == MOTIONSENSE_TYPE_ACCEL) { - sens_size = ARRAY_SIZE(acc_sensor_range); - sensor_range = acc_sensor_range; - } else { - sens_size = ARRAY_SIZE(gyr_sensor_range); - sensor_range = gyr_sensor_range; - } - - for (index = 0; index < sens_size - 1; index++) { - if (range <= sensor_range[index][0]) - break; - - if (range < sensor_range[index + 1][0] && rnd) { - index++; - break; - } - } - - mutex_lock(s->mutex); - - /* - * Read the range register from sensor for accelerometer/gyroscope - * s->type should have MOTIONSENSE_TYPE_ACCEL = 0 ; - * MOTIONSENSE_TYPE_GYRO = 1 - */ - ret = bmi3_read_n(s, BMI3_REG_ACC_CONF + s->type, reg_data, 4); - - if (ret == EC_SUCCESS) { - /* Set accelerometer/Gyroscope range */ - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - reg_data[2] = BMI3_SET_BITS(reg_data[2], BMI3_SENS_RANGE, - sensor_range[index][1]); - - /* Set the accel/gyro configurations. */ - ret = bmi3_write_n(s, BMI3_REG_ACC_CONF + s->type, - ®_data[2], 2); - - /* Now that we have set the range, update the driver's value. */ - if (ret == EC_SUCCESS) - s->current_range = sensor_range[index][0]; - } - - mutex_unlock(s->mutex); - - return ret; -} - -static int read(const struct motion_sensor_t *s, intv3_t v) -{ - int ret; - uint8_t reg_data[8] = { 0 }; - uint16_t status_val = 0; - - mutex_lock(s->mutex); - - /* Read the status register */ - ret = bmi3_read_n(s, BMI3_REG_STATUS, reg_data, 4); - - if (ret == EC_SUCCESS) { - status_val = (reg_data[2] | ((uint16_t)reg_data[3] << 8)); - /* - * If sensor data is not ready, return the previous read data. - * Note: return success so that motion sensor task can read - * again to get the latest updated sensor data quickly. - */ - if (!(status_val & BMI3_DRDY_MASK(s->type))) { - if (v != s->raw_xyz) - memcpy(v, s->raw_xyz, sizeof(s->raw_xyz)); - - mutex_unlock(s->mutex); - - return EC_SUCCESS; - } - - if (s->type == MOTIONSENSE_TYPE_ACCEL) { - /* Read the sensor data */ - ret = bmi3_read_n(s, BMI3_REG_ACC_DATA_X, reg_data, 8); - } else if (s->type == MOTIONSENSE_TYPE_GYRO) { - /* Read the sensor data */ - ret = bmi3_read_n(s, BMI3_REG_GYR_DATA_X, reg_data, 8); - } - - if (ret == EC_SUCCESS) { - v[0] = ((int16_t)((reg_data[3] << 8) | reg_data[2])); - v[1] = ((int16_t)((reg_data[5] << 8) | reg_data[4])); - v[2] = ((int16_t)((reg_data[7] << 8) | reg_data[6])); - - rotate(v, *s->rot_standard_ref, v); - } - } - - mutex_unlock(s->mutex); - - return ret; -} - -static int init(struct motion_sensor_t *s) -{ - /* Status of communication result */ - uint8_t i; - uint8_t reg_data[4] = { 0 }; - - /* Store the sensor configurations */ - struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s); - struct bmi_drv_data_t *data = BMI_GET_DATA(s); - - /* This driver requires a mutex */ - ASSERT(s->mutex); - - /* - * BMI3xx driver only supports MOTIONSENSE_TYPE_ACCEL and - * MOTIONSENSE_TYPE_GYR0 - */ - if (s->type != MOTIONSENSE_TYPE_ACCEL - && s->type != MOTIONSENSE_TYPE_GYRO) - return EC_ERROR_UNIMPLEMENTED; - - /* Read chip id */ - RETURN_ERROR(bmi3_read_n(s, BMI3_REG_CHIP_ID, reg_data, 4)); - - if (reg_data[2] != BMI323_CHIP_ID) - return EC_ERROR_HW_INTERNAL; - - if (s->type == MOTIONSENSE_TYPE_ACCEL) { - /* Reset bmi3 device */ - reg_data[0] = (uint8_t)(BMI3_CMD_SOFT_RESET - & BMI3_SET_LOW_BYTE); - reg_data[1] = (uint8_t)((BMI3_CMD_SOFT_RESET - & BMI3_SET_HIGH_BYTE) >> 8); - - RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2)); - - /* Delay of 2ms after soft reset*/ - msleep(2); - - /* Enable feature engine bit */ - reg_data[0] = BMI3_ENABLE; - reg_data[1] = 0; - - RETURN_ERROR(bmi3_write_n(s, BMI3_REG_FEATURE_ENGINE_GLOB_CTRL, - reg_data, 2)); - - if (IS_ENABLED(CONFIG_ACCEL_INTERRUPTS)) - RETURN_ERROR(config_interrupt(s)); - } - - for (i = X; i <= Z; i++) - saved_data->scale[i] = MOTION_SENSE_DEFAULT_SCALE; - - /* The sensor is in Suspend mode at init, so set data rate to 0*/ - saved_data->odr = 0; - - /* Flags used in FIFO parsing */ - data->flags &= ~(BMI_FLAG_SEC_I2C_ENABLED - | (BMI_FIFO_ALL_MASK << BMI_FIFO_FLAG_OFFSET)); - - return sensor_init_done(s); -} - -/* Accelerometer/Gyroscope base driver structure */ -const struct accelgyro_drv bmi3xx_drv = { - .init = init, - .read = read, - .set_range = set_range, - .get_resolution = get_resolution, - .set_data_rate = set_data_rate, - .get_data_rate = get_data_rate, - .get_scale = get_scale, - .set_scale = set_scale, - .set_offset = set_offset, - .get_offset = get_offset, - .perform_calib = perform_calib, - .read_temp = read_temp, -#ifdef CONFIG_ACCEL_INTERRUPTS - .irq_handler = irq_handler, -#endif -#ifdef CONFIG_BODY_DETECTION - .get_rms_noise = get_rms_noise, -#endif -}; |