diff options
Diffstat (limited to 'driver/accel_bma4xx.c')
| -rw-r--r-- | driver/accel_bma4xx.c | 434 |
1 files changed, 0 insertions, 434 deletions
diff --git a/driver/accel_bma4xx.c b/driver/accel_bma4xx.c deleted file mode 100644 index e9502c20b4..0000000000 --- a/driver/accel_bma4xx.c +++ /dev/null @@ -1,434 +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. - */ - -/* - * Bosch Accelerometer driver for Chrome EC - * - * Supported: BMA422 - */ - -#include "accelgyro.h" -#include "accel_bma422.h" -#include "common.h" -#include "console.h" -#include "i2c.h" -#include "math_util.h" -#include "spi.h" -#include "task.h" -#include "util.h" - -#define CPUTS(outstr) cputs(CC_ACCEL, outstr) -#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) - -/** - * Read 8bit register from accelerometer. - */ -static inline int bma4_read8(const struct motion_sensor_t *s, const int reg, - int *data_ptr) -{ - return i2c_read8(s->port, s->i2c_spi_addr_flags, reg, data_ptr); -} - -/** - * Write 8bit register from accelerometer. - */ -static inline int bma4_write8(const struct motion_sensor_t *s, const int reg, - int data) -{ - int ret; - - ret = i2c_write8(s->port, s->i2c_spi_addr_flags, reg, data); - - /* - * From Bosch: BMA 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. - */ - usleep(450); - - return ret; -} - -/* - * Set specific bit set to certain value of a 8-bit reg. - */ -static inline int bma4_set_reg8(const struct motion_sensor_t *s, int reg, - uint8_t bits, int mask) -{ - int val; - - RETURN_ERROR(bma4_read8(s, reg, &val)); - - val = (val & ~mask) | bits; - - return bma4_write8(s, reg, val); -} - -static int write_accel_offset(const struct motion_sensor_t *s, intv3_t v) -{ - int i, val; - - rotate_inv(v, *s->rot_standard_ref, v); - - for (i = X; i <= Z; i++) { - val = round_divide((int64_t)v[i] * BMA4_OFFSET_ACC_DIV_MG, - BMA4_OFFSET_ACC_MULTI_MG); - if (val > 127) - val = 127; - if (val < -128) - val = -128; - if (val < 0) - val += 256; - - RETURN_ERROR(bma4_write8(s, BMA4_OFFSET_0_ADDR + i, val)); - } - - return EC_SUCCESS; -} - -static int set_foc_config(struct motion_sensor_t *s) -{ - /* Disabling offset compensation */ - RETURN_ERROR(bma4_set_reg8(s, BMA4_NV_CONFIG_ADDR, - (BMA4_DISABLE << BMA4_NV_ACCEL_OFFSET_POS), - BMA4_NV_ACCEL_OFFSET_MSK)); - - /* Set accelerometer configurations to 50Hz,CIC, continuous mode */ - RETURN_ERROR(bma4_write8(s, BMA4_ACCEL_CONFIG_ADDR, - BMA4_FOC_ACC_CONF_VAL)); - - - /* Set accelerometer to normal mode by enabling it */ - RETURN_ERROR(bma4_set_reg8(s, BMA4_POWER_CTRL_ADDR, - (BMA4_ENABLE << BMA4_ACCEL_ENABLE_POS), - BMA4_ACCEL_ENABLE_MSK)); - - /* Disable advance power save mode */ - RETURN_ERROR(bma4_set_reg8(s, BMA4_POWER_CONF_ADDR, - (BMA4_DISABLE - << BMA4_ADVANCE_POWER_SAVE_POS), - BMA4_ADVANCE_POWER_SAVE_MSK)); - - return EC_SUCCESS; -} - -static int wait_and_read_data(struct motion_sensor_t *s, intv3_t v) -{ - int i; - - /* Retry 5 times */ - uint8_t reg_data[6] = {0}, try_cnt = 5; - - /* Check if data is ready */ - while (try_cnt && (!(reg_data[0] & BMA4_STAT_DATA_RDY_ACCEL_MSK))) { - /* 20ms delay for 50Hz ODR */ - msleep(20); - - /* Read the status register */ - RETURN_ERROR(i2c_read_block(s->port, s->i2c_spi_addr_flags, - BMA4_STATUS_ADDR, reg_data, 1)); - - try_cnt--; - } - - if (!(reg_data[0] & 0x80)) - return EC_ERROR_TIMEOUT; - - /* Read the sensor data */ - RETURN_ERROR(i2c_read_block(s->port, s->i2c_spi_addr_flags, - BMA4_DATA_8_ADDR, reg_data, 6)); - - for (i = X; i <= Z; i++) { - v[i] = (((int8_t)reg_data[i * 2 + 1]) << 8) - | (reg_data[i * 2] & 0xf0); - - /* Since the resolution is only 12 bits*/ - v[i] = (v[i] / 0x10); - } - - rotate(v, *s->rot_standard_ref, v); - - return EC_SUCCESS; -} - -static int8_t perform_accel_foc(struct motion_sensor_t *s, int *target, - int sens_range) -{ - intv3_t accel_data, offset; - - /* Structure to store accelerometer data temporarily */ - int32_t delta_value[3] = {0, 0, 0}; - - /* Variable to define count */ - uint8_t i, loop, sample_count = 0; - - for (loop = 0; loop < BMA4_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^11*-1] - * (unit of offset:mg) - */ - for (i = X; i <= Z; ++i) { - offset[i] = ((((delta_value[i] * 1000 * sens_range) - / sample_count) / 2048) * -1); - } - - RETURN_ERROR(write_accel_offset(s, offset)); - - /* Enable the offsets and backup to NVM */ - RETURN_ERROR(bma4_set_reg8(s, BMA4_NV_CONFIG_ADDR, - (BMA4_ENABLE << BMA4_NV_ACCEL_OFFSET_POS), - BMA4_NV_ACCEL_OFFSET_MSK)); - - return EC_SUCCESS; -} - -static int perform_calib(struct motion_sensor_t *s, int enable) -{ - uint8_t config[2]; - int pwr_ctrl, pwr_conf; - intv3_t target = {0, 0, 0}; - int sens_range = s->current_range; - - if (!enable) - return EC_SUCCESS; - - /* Read accelerometer configurations */ - RETURN_ERROR(i2c_read_block(s->port, s->i2c_spi_addr_flags, - BMA4_ACCEL_CONFIG_ADDR, config, 2)); - - /* Get accelerometer enable status to be saved */ - RETURN_ERROR(bma4_read8(s, BMA4_POWER_CTRL_ADDR, &pwr_ctrl)); - - /* Get advance power save mode to be saved */ - RETURN_ERROR(bma4_read8(s, BMA4_POWER_CONF_ADDR, &pwr_conf)); - - /* Perform calibration */ - RETURN_ERROR(set_foc_config(s)); - - /* We calibrate considering Z axis is laid flat on the surface */ - target[Z] = BMA4_ACC_DATA_PLUS_1G(sens_range); - - RETURN_ERROR(perform_accel_foc(s, target, sens_range)); - - /* Set the saved sensor configuration */ - RETURN_ERROR(i2c_write_block(s->port, s->i2c_spi_addr_flags, - BMA4_ACCEL_CONFIG_ADDR, config, 2)); - - RETURN_ERROR(bma4_write8(s, BMA4_POWER_CTRL_ADDR, pwr_ctrl)); - - RETURN_ERROR(bma4_write8(s, BMA4_POWER_CONF_ADDR, pwr_conf)); - - return EC_SUCCESS; -} - -static int set_range(struct motion_sensor_t *s, int range, int round) -{ - int ret, range_reg_val; - - range_reg_val = BMA4_RANGE_TO_REG(range); - - /* - * If rounding flag is set then set the range_val to nearest - * valid value. - */ - if ((BMA4_REG_TO_RANGE(range_reg_val) < range) && round) - range_reg_val = BMA4_RANGE_TO_REG(range * 2); - - mutex_lock(s->mutex); - - /* Determine the new value of control reg and attempt to write it. */ - ret = bma4_set_reg8(s, BMA4_ACCEL_RANGE_ADDR, - range_reg_val << BMA4_ACCEL_RANGE_POS, - BMA4_ACCEL_RANGE_MSK); - - /* If successfully written, then save the range. */ - if (ret == EC_SUCCESS) - s->current_range = BMA4_REG_TO_RANGE(range_reg_val); - - mutex_unlock(s->mutex); - - return ret; -} - -static int get_resolution(const struct motion_sensor_t *s) -{ - return BMA4_12_BIT_RESOLUTION; -} - -static int set_data_rate(const struct motion_sensor_t *s, int rate, int round) -{ - int ret, odr_reg_val; - struct accelgyro_saved_data_t *data = s->drv_data; - - odr_reg_val = BMA4_ODR_TO_REG(rate); - - if ((BMA4_REG_TO_ODR(odr_reg_val) < rate) && round) - odr_reg_val = BMA4_ODR_TO_REG(rate * 2); - - mutex_lock(s->mutex); - - /* Determine the new value of control reg and attempt to write it. */ - ret = bma4_set_reg8(s, BMA4_ACCEL_CONFIG_ADDR, - odr_reg_val << BMA4_ACCEL_ODR_POS, - BMA4_ACCEL_ODR_MSK); - - /* If successfully written, then save the new data rate. */ - if (ret == EC_SUCCESS) - data->odr = BMA4_REG_TO_ODR(odr_reg_val); - - mutex_unlock(s->mutex); - - return ret; -} - -static int get_data_rate(const struct motion_sensor_t *s) -{ - struct accelgyro_saved_data_t *data = s->drv_data; - - return data->odr; -} - -static int set_offset(const struct motion_sensor_t *s, const int16_t *offset, - int16_t temp) -{ - int ret; - intv3_t v = { offset[X], offset[Y], offset[Z] }; - - mutex_lock(s->mutex); - - ret = write_accel_offset(s, v); - - if (ret == EC_SUCCESS) { - /* Enable the offsets and backup to NVM */ - ret = bma4_set_reg8(s, BMA4_NV_CONFIG_ADDR, - (BMA4_ENABLE << BMA4_NV_ACCEL_OFFSET_POS), - BMA4_NV_ACCEL_OFFSET_MSK); - } - - mutex_unlock(s->mutex); - - return ret; -} - -static int get_offset(const struct motion_sensor_t *s, int16_t *offset, - int16_t *temp) -{ - int i, val, ret; - intv3_t v; - - mutex_lock(s->mutex); - - for (i = X; i <= Z; i++) { - ret = bma4_read8(s, BMA4_OFFSET_0_ADDR + i, &val); - if (ret) { - mutex_unlock(s->mutex); - return ret; - } - - if (val > 0x7f) - val -= -256; - - v[i] = round_divide((int64_t)val * BMA4_OFFSET_ACC_MULTI_MG, - BMA4_OFFSET_ACC_DIV_MG); - } - - mutex_unlock(s->mutex); - - /* Offset is in milli-g */ - rotate(v, *s->rot_standard_ref, v); - offset[X] = v[X]; - offset[Y] = v[Y]; - offset[Z] = v[Z]; - - *temp = (int16_t)EC_MOTION_SENSE_INVALID_CALIB_TEMP; - - return EC_SUCCESS; -} - -static int read(const struct motion_sensor_t *s, intv3_t v) -{ - uint8_t acc[6]; - int ret, i; - - mutex_lock(s->mutex); - - /* Read 6 bytes starting at X_AXIS_LSB. */ - ret = i2c_read_block(s->port, s->i2c_spi_addr_flags, - BMA4_DATA_8_ADDR, acc, 6); - - mutex_unlock(s->mutex); - - if (ret) - return ret; - - /* - * Convert acceleration to a signed 16-bit number. Note, based on - * the order of the registers: - * - * acc[0] = X_AXIS_LSB -> bit 7~4 for value, bit 0 for new data bit - * acc[1] = X_AXIS_MSB - * acc[2] = Y_AXIS_LSB -> bit 7~4 for value, bit 0 for new data bit - * acc[3] = Y_AXIS_MSB - * acc[4] = Z_AXIS_LSB -> bit 7~4 for value, bit 0 for new data bit - * acc[5] = Z_AXIS_MSB - */ - for (i = X; i <= Z; i++) - v[i] = (((int8_t)acc[i * 2 + 1]) << 8) | (acc[i * 2] & 0xf0); - - rotate(v, *s->rot_standard_ref, v); - - return EC_SUCCESS; -} - -static int init(struct motion_sensor_t *s) -{ - int ret = 0, reg_val; - - /* This driver requires a mutex. Assert if mutex is not supplied. */ - ASSERT(s->mutex); - - /* Read accelerometer's CHID ID */ - RETURN_ERROR(bma4_read8(s, BMA4_CHIP_ID_ADDR, ®_val)); - - if (s->chip != MOTIONSENSE_CHIP_BMA422 || reg_val != BMA422_CHIP_ID) - return EC_ERROR_HW_INTERNAL; - - mutex_lock(s->mutex); - - /* Enable accelerometer */ - ret = bma4_set_reg8(s, BMA4_POWER_CTRL_ADDR, - BMA4_ENABLE << BMA4_ACCEL_ENABLE_POS, - BMA4_ACCEL_ENABLE_MSK); - - mutex_unlock(s->mutex); - - if (ret) - return ret; - - return sensor_init_done(s); -} - -const struct accelgyro_drv bma4_accel_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, - .set_offset = set_offset, - .get_offset = get_offset, - .perform_calib = perform_calib, -}; |