581 lines
15 KiB
C
581 lines
15 KiB
C
/* Copyright 2019 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.
|
|
*/
|
|
|
|
/*
|
|
* LIS2DW12 accelerometer module for Chrome EC 3D digital accelerometer.
|
|
* For more details on LIS2DW12 device please refers to www.st.com.
|
|
*/
|
|
#include "accelgyro.h"
|
|
#include "common.h"
|
|
#include "console.h"
|
|
#include "driver/accel_lis2dw12.h"
|
|
#include "hooks.h"
|
|
#include "hwtimer.h"
|
|
#include "math_util.h"
|
|
#include "task.h"
|
|
#include "util.h"
|
|
|
|
#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
|
|
|
|
/* Only when configured as base accel sensor, fifo and interrupt
|
|
* are supported.
|
|
*/
|
|
#ifdef CONFIG_ACCEL_LIS2DW_AS_BASE
|
|
|
|
#ifdef CONFIG_ACCEL_FIFO
|
|
static volatile uint32_t last_interrupt_timestamp;
|
|
|
|
/**
|
|
* lis2dw12_enable_fifo - Enable/Disable FIFO in LIS2DW12
|
|
* @s: Motion sensor pointer
|
|
* @mode: fifo_modes
|
|
*/
|
|
static int lis2dw12_enable_fifo(const struct motion_sensor_t *s,
|
|
enum lis2dw12_fmode mode)
|
|
{
|
|
return st_write_data_with_mask(s, LIS2DW12_FIFO_CTRL_ADDR,
|
|
LIS2DW12_FIFO_MODE_MASK, mode);
|
|
}
|
|
|
|
/**
|
|
* Load data from internal sensor FIFO.
|
|
* @s: Motion sensor pointer
|
|
*/
|
|
static int lis2dw12_load_fifo(struct motion_sensor_t *s, int nsamples,
|
|
uint32_t *last_fifo_read_ts)
|
|
{
|
|
int ret, left, length, i;
|
|
struct ec_response_motion_sensor_data vect;
|
|
uint32_t interrupt_timestamp = last_interrupt_timestamp;
|
|
int *axis = s->raw_xyz;
|
|
uint8_t fifo[FIFO_READ_LEN];
|
|
|
|
/* Each sample are OUT_XYZ_SIZE bytes. */
|
|
left = nsamples * OUT_XYZ_SIZE;
|
|
|
|
do {
|
|
/*
|
|
* Limit FIFO read data to burst of FIFO_READ_LEN size because
|
|
* read operatios in under i2c mutex lock.
|
|
*/
|
|
if (left > FIFO_READ_LEN)
|
|
length = FIFO_READ_LEN;
|
|
else
|
|
length = left;
|
|
|
|
ret = st_raw_read_n(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_OUT_X_L_ADDR, fifo, length);
|
|
*last_fifo_read_ts = __hw_clock_source_read();
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
for (i = 0; i < length; i += OUT_XYZ_SIZE) {
|
|
/* Apply precision, sensitivity and rotation vector. */
|
|
st_normalize(s, axis, &fifo[i]);
|
|
|
|
/* Fill vector array. */
|
|
vect.data[X] = axis[X];
|
|
vect.data[Y] = axis[Y];
|
|
vect.data[Z] = axis[Z];
|
|
vect.flags = 0;
|
|
vect.sensor_num = 0;
|
|
motion_sense_fifo_stage_data(&vect, s, 3,
|
|
interrupt_timestamp);
|
|
}
|
|
left -= length;
|
|
} while (left > 0);
|
|
|
|
motion_sense_fifo_commit_data();
|
|
|
|
return EC_SUCCESS;
|
|
}
|
|
|
|
/**
|
|
* lis2dw12_get_fifo_samples - check for stored FIFO samples.
|
|
*/
|
|
static int lis2dw12_get_fifo_samples(struct motion_sensor_t *s, int *nsamples)
|
|
{
|
|
int ret, tmp;
|
|
|
|
ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_FIFO_SAMPLES_ADDR, &tmp);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
*nsamples = tmp & LIS2DW12_FIFO_DIFF_MASK;
|
|
|
|
return EC_SUCCESS;
|
|
}
|
|
|
|
static int fifo_data_avail(struct motion_sensor_t *s)
|
|
{
|
|
int ret, nsamples;
|
|
|
|
if (s->flags & MOTIONSENSE_FLAG_INT_SIGNAL)
|
|
return gpio_get_level(s->int_signal) ==
|
|
!!(MOTIONSENSE_FLAG_INT_ACTIVE_HIGH & s->flags);
|
|
|
|
ret = lis2dw12_get_fifo_samples(s, &nsamples);
|
|
/* If we failed to read the FIFO size assume empty. */
|
|
if (ret != EC_SUCCESS)
|
|
return 0;
|
|
return nsamples;
|
|
}
|
|
#endif /* CONFIG_ACCEL_FIFO */
|
|
|
|
/**
|
|
* lis2dw12_config_interrupt- Configure interrupt for supported features.
|
|
* @s: Motion sensor pointer
|
|
*
|
|
* Must works with interface mutex locked
|
|
*/
|
|
static int lis2dw12_config_interrupt(const struct motion_sensor_t *s)
|
|
{
|
|
int ret = EC_SUCCESS;
|
|
|
|
#ifdef CONFIG_ACCEL_FIFO_THRES
|
|
/* Configure FIFO watermark level. */
|
|
ret = st_write_data_with_mask(s, LIS2DW12_FIFO_CTRL_ADDR,
|
|
LIS2DW12_FIFO_THRESHOLD_MASK, 1);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
/* Enable interrupt on FIFO watermask and route to int1. */
|
|
ret = st_write_data_with_mask(s, LIS2DW12_INT1_FTH_ADDR,
|
|
LIS2DW12_INT1_FTH_MASK, LIS2DW12_EN_BIT);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
#endif /* CONFIG_ACCEL_FIFO */
|
|
|
|
#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
|
|
/*
|
|
* Configure D-TAP event detection on 3 axis.
|
|
* For more details please refer to AN5038.
|
|
*/
|
|
ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_TAP_THS_X_ADDR, 0x09);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_TAP_THS_Y_ADDR, 0x09);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_TAP_THS_Z_ADDR, 0xE9);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_INT_DUR_ADDR, 0x7F);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
/* Enable D-TAP event detection. */
|
|
ret = st_write_data_with_mask(s, LIS2DW12_WAKE_UP_THS_ADDR,
|
|
LIS2DW12_SINGLE_DOUBLE_TAP,
|
|
LIS2DW12_EN_BIT);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
/*
|
|
* Enable D-TAP detection on int_1 pad. In any case D-TAP event
|
|
* can be detected only if ODR is over 200 Hz.
|
|
*/
|
|
ret = st_write_data_with_mask(s, LIS2DW12_INT1_TAP_ADDR,
|
|
LIS2DW12_INT1_DTAP_MASK,
|
|
LIS2DW12_EN_BIT);
|
|
#endif /* CONFIG_GESTURE_SENSOR_BATTERY_TAP */
|
|
return ret;
|
|
}
|
|
|
|
static void lis2dw12_handle_interrupt_for_fifo(uint32_t ts)
|
|
{
|
|
#ifdef CONFIG_ACCEL_FIFO
|
|
if (time_after(ts, last_interrupt_timestamp))
|
|
last_interrupt_timestamp = ts;
|
|
#endif
|
|
task_set_event(TASK_ID_MOTIONSENSE,
|
|
CONFIG_ACCEL_LIS2DW12_INT_EVENT, 0);
|
|
}
|
|
|
|
/**
|
|
* lis2dw12_interrupt - interrupt from int pin of sensor
|
|
* Schedule Motion Sense Task to manage Interrupts.
|
|
*/
|
|
void lis2dw12_interrupt(enum gpio_signal signal)
|
|
{
|
|
lis2dw12_handle_interrupt_for_fifo(__hw_clock_source_read());
|
|
}
|
|
|
|
/**
|
|
* lis2dw12_irq_handler - bottom half of the interrupt stack.
|
|
*/
|
|
static int lis2dw12_irq_handler(struct motion_sensor_t *s, uint32_t *event)
|
|
{
|
|
int ret = EC_SUCCESS;
|
|
|
|
if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
|
|
(!(*event & CONFIG_ACCEL_LIS2DW12_INT_EVENT))) {
|
|
return EC_ERROR_NOT_HANDLED;
|
|
}
|
|
|
|
#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
|
|
{
|
|
int status = 0;
|
|
|
|
/* Read Status register to check TAP events. */
|
|
st_raw_read8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_STATUS_TAP, &status);
|
|
if (status & LIS2DW12_DOUBLE_TAP)
|
|
*event |= CONFIG_GESTURE_TAP_EVENT;
|
|
}
|
|
#endif /* CONFIG_GESTURE_SENSOR_BATTERY_TAP */
|
|
|
|
#ifdef CONFIG_ACCEL_FIFO
|
|
{
|
|
int nsamples;
|
|
uint32_t last_fifo_read_ts;
|
|
uint32_t triggering_interrupt_timestamp =
|
|
last_interrupt_timestamp;
|
|
|
|
ret = lis2dw12_get_fifo_samples(s, &nsamples);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
last_fifo_read_ts = __hw_clock_source_read();
|
|
if (nsamples == 0)
|
|
return EC_SUCCESS;
|
|
|
|
ret = lis2dw12_load_fifo(s, nsamples, &last_fifo_read_ts);
|
|
|
|
/*
|
|
* Check if FIFO isn't empty and we never got an interrupt.
|
|
* This can happen if new entries were added to the FIFO after
|
|
* the count was read, but before the FIFO was cleared out.
|
|
* In the long term it might be better to use the last
|
|
* spread timestamp instead.
|
|
*/
|
|
if (fifo_data_avail(s) &&
|
|
triggering_interrupt_timestamp == last_interrupt_timestamp)
|
|
lis2dw12_handle_interrupt_for_fifo(last_fifo_read_ts);
|
|
}
|
|
#endif /* CONFIG_ACCEL_FIFO */
|
|
|
|
return ret;
|
|
}
|
|
|
|
#endif /* CONFIG_ACCEL_LIS2DW_AS_BASE */
|
|
|
|
/**
|
|
* set_power_mode - set sensor power mode
|
|
* @s: Motion sensor pointer
|
|
* @mode: LIS2DW12_LOW_POWER, LIS2DW12_HIGH_PERF
|
|
* @lpmode: LIS2DW12_LOW_POWER_MODE_2,
|
|
* LIS2DW12_LOW_POWER_MODE_3,
|
|
* LIS2DW12_LOW_POWER_MODE_4
|
|
*
|
|
* TODO: LIS2DW12_LOW_POWER_MODE_1 not implemented because output differ
|
|
* in resolution
|
|
*/
|
|
static int set_power_mode(const struct motion_sensor_t *s,
|
|
enum lis2sw12_mode mode,
|
|
enum lis2sw12_lpmode lpmode)
|
|
{
|
|
int ret = EC_SUCCESS;
|
|
|
|
if (mode == LIS2DW12_LOW_POWER &&
|
|
lpmode == LIS2DW12_LOW_POWER_MODE_1)
|
|
return EC_ERROR_UNIMPLEMENTED;
|
|
|
|
/* Set Mode and Low Power Mode. */
|
|
ret = st_write_data_with_mask(s, LIS2DW12_ACC_MODE_ADDR,
|
|
LIS2DW12_ACC_MODE_MASK, mode);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
ret = st_write_data_with_mask(s, LIS2DW12_ACC_LPMODE_ADDR,
|
|
LIS2DW12_ACC_LPMODE_MASK, lpmode);
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* set_range - set full scale range
|
|
* @s: Motion sensor pointer
|
|
* @range: Range
|
|
* @rnd: Round up/down flag
|
|
*/
|
|
static int set_range(const struct motion_sensor_t *s, int range, int rnd)
|
|
{
|
|
int err = EC_SUCCESS;
|
|
uint8_t reg_val;
|
|
struct stprivate_data *data = s->drv_data;
|
|
int newrange = range;
|
|
|
|
/* Adjust and check rounded value. */
|
|
if (rnd && (newrange < LIS2DW12_NORMALIZE_FS(newrange)))
|
|
newrange <<= 1;
|
|
|
|
if (newrange > LIS2DW12_ACCEL_FS_MAX_VAL)
|
|
newrange = LIS2DW12_ACCEL_FS_MAX_VAL;
|
|
|
|
reg_val = LIS2DW12_FS_REG(newrange);
|
|
|
|
mutex_lock(s->mutex);
|
|
#if defined(CONFIG_ACCEL_FIFO) && defined(CONFIG_ACCEL_LIS2DW_AS_BASE)
|
|
/*
|
|
* FIFO stop collecting events. Restart FIFO in Bypass mode.
|
|
* If Range is changed all samples in FIFO must be discharged because
|
|
* with a different sensitivity.
|
|
*/
|
|
err = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_BYPASS_MODE);
|
|
if (err != EC_SUCCESS)
|
|
goto unlock_rate;
|
|
#endif /* CONFIG_ACCEL_FIFO && CONFIG_ACCEL_LIS2DW_AS_BASE */
|
|
|
|
err = st_write_data_with_mask(s, LIS2DW12_FS_ADDR, LIS2DW12_FS_MASK,
|
|
reg_val);
|
|
if (err == EC_SUCCESS)
|
|
data->base.range = newrange;
|
|
#if defined(CONFIG_ACCEL_FIFO) && defined(CONFIG_ACCEL_LIS2DW_AS_BASE)
|
|
/* FIFO restart collecting events in Cont. mode. */
|
|
err = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_CONT_MODE);
|
|
|
|
unlock_rate:
|
|
#endif /* CONFIG_ACCEL_FIFO && CONFIG_ACCEL_LIS2DW_AS_BASE */
|
|
mutex_unlock(s->mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
static int get_range(const struct motion_sensor_t *s)
|
|
{
|
|
struct stprivate_data *data = s->drv_data;
|
|
|
|
return data->base.range;
|
|
}
|
|
|
|
static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
|
|
{
|
|
int ret, normalized_rate;
|
|
struct stprivate_data *data = s->drv_data;
|
|
uint8_t reg_val;
|
|
|
|
mutex_lock(s->mutex);
|
|
|
|
#if defined(CONFIG_ACCEL_FIFO) && defined(CONFIG_ACCEL_LIS2DW_AS_BASE)
|
|
/* FIFO stop collecting events. Restart FIFO in Bypass mode. */
|
|
ret = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_BYPASS_MODE);
|
|
if (ret != EC_SUCCESS)
|
|
goto unlock_rate;
|
|
#endif /* CONFIG_ACCEL_FIFO && CONFIG_ACCEL_LIS2DW_AS_BASE */
|
|
|
|
if (rate == 0) {
|
|
ret = st_write_data_with_mask(s, LIS2DW12_ACC_ODR_ADDR,
|
|
LIS2DW12_ACC_ODR_MASK,
|
|
LIS2DW12_ODR_POWER_OFF_VAL);
|
|
if (ret == EC_SUCCESS)
|
|
data->base.odr = LIS2DW12_ODR_POWER_OFF_VAL;
|
|
|
|
goto unlock_rate;
|
|
}
|
|
|
|
reg_val = LIS2DW12_ODR_TO_REG(rate);
|
|
normalized_rate = LIS2DW12_ODR_TO_NORMALIZE(rate);
|
|
|
|
if (rnd && (normalized_rate < rate)) {
|
|
reg_val++;
|
|
normalized_rate <<= 1;
|
|
}
|
|
|
|
if (reg_val > LIS2DW12_ODR_1_6kHZ_VAL) {
|
|
reg_val = LIS2DW12_ODR_1_6kHZ_VAL;
|
|
normalized_rate = LIS2DW12_ODR_MAX_VAL;
|
|
} else if (reg_val < LIS2DW12_ODR_12HZ_VAL) {
|
|
reg_val = LIS2DW12_ODR_12HZ_VAL;
|
|
normalized_rate = LIS2DW12_ODR_MIN_VAL;
|
|
}
|
|
|
|
/* lis2dwl supports 14 bit resolution only at high performance mode,
|
|
* and it will always stay at high performance mode from initialization.
|
|
* But lis2dw12 needs switch low power mode according to odr value.
|
|
*/
|
|
#ifndef CONFIG_ACCEL_LIS2DWL
|
|
if (reg_val > LIS2DW12_ODR_200HZ_VAL)
|
|
ret = set_power_mode(s, LIS2DW12_HIGH_PERF, 0);
|
|
else
|
|
ret = set_power_mode(s, LIS2DW12_LOW_POWER,
|
|
LIS2DW12_LOW_POWER_MODE_2);
|
|
#endif
|
|
|
|
ret = st_write_data_with_mask(s, LIS2DW12_ACC_ODR_ADDR,
|
|
LIS2DW12_ACC_ODR_MASK, reg_val);
|
|
if (ret == EC_SUCCESS)
|
|
data->base.odr = normalized_rate;
|
|
|
|
#if defined(CONFIG_ACCEL_FIFO) && defined(CONFIG_ACCEL_LIS2DW_AS_BASE)
|
|
/* FIFO restart collecting events in continuous mode. */
|
|
ret = lis2dw12_enable_fifo(s, LIS2DW12_FIFO_CONT_MODE);
|
|
#endif /* CONFIG_ACCEL_FIFO && CONFIG_ACCEL_LIS2DW_AS_BASE */
|
|
|
|
unlock_rate:
|
|
mutex_unlock(s->mutex);
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int is_data_ready(const struct motion_sensor_t *s, int *ready)
|
|
{
|
|
int ret, tmp;
|
|
|
|
ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_STATUS_REG, &tmp);
|
|
if (ret != EC_SUCCESS)
|
|
return ret;
|
|
|
|
*ready = (LIS2DW12_STS_DRDY_UP == (tmp & LIS2DW12_STS_DRDY_UP));
|
|
|
|
return EC_SUCCESS;
|
|
}
|
|
|
|
static int read(const struct motion_sensor_t *s, intv3_t v)
|
|
{
|
|
uint8_t raw[OUT_XYZ_SIZE];
|
|
int ret, tmp = 0;
|
|
|
|
ret = is_data_ready(s, &tmp);
|
|
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 (!tmp) {
|
|
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 = st_raw_read_n_noinc(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_OUT_X_L_ADDR, raw,
|
|
OUT_XYZ_SIZE);
|
|
if (ret != EC_SUCCESS) {
|
|
CPRINTF("[%T %s type:0x%X RD XYZ Error]", s->name, s->type);
|
|
return ret;
|
|
}
|
|
|
|
/* Transform from LSB to real data with rotation and gain. */
|
|
st_normalize(s, v, raw);
|
|
|
|
return EC_SUCCESS;
|
|
}
|
|
|
|
static int init(const struct motion_sensor_t *s)
|
|
{
|
|
int ret = 0, tmp, timeout = 0, status;
|
|
struct stprivate_data *data = s->drv_data;
|
|
|
|
ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_WHO_AM_I_REG, &tmp);
|
|
if (ret != EC_SUCCESS)
|
|
return EC_ERROR_UNKNOWN;
|
|
|
|
if (tmp != LIS2DW12_WHO_AM_I)
|
|
return EC_ERROR_ACCESS_DENIED;
|
|
|
|
/*
|
|
* This sensor can be powered through an EC reboot, so the state of
|
|
* the sensor is unknown here. Initiate software reset to restore
|
|
* sensor to default.
|
|
*/
|
|
mutex_lock(s->mutex);
|
|
ret = st_raw_write8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_SOFT_RESET_ADDR, LIS2DW12_SOFT_RESET_MASK);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
|
|
/* Wait End of Reset. */
|
|
do {
|
|
if (timeout > 10) {
|
|
ret = EC_RES_TIMEOUT;
|
|
goto err_unlock;
|
|
}
|
|
|
|
msleep(1);
|
|
timeout += 1;
|
|
ret = st_raw_read8(s->port, s->i2c_spi_addr_flags,
|
|
LIS2DW12_SOFT_RESET_ADDR, &status);
|
|
if (ret != EC_SUCCESS)
|
|
continue;
|
|
} while ((status & LIS2DW12_SOFT_RESET_MASK) != 0);
|
|
|
|
/* Enable BDU. */
|
|
ret = st_write_data_with_mask(s, LIS2DW12_BDU_ADDR, LIS2DW12_BDU_MASK,
|
|
LIS2DW12_EN_BIT);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
|
|
ret = st_write_data_with_mask(s, LIS2DW12_LIR_ADDR, LIS2DW12_LIR_MASK,
|
|
LIS2DW12_EN_BIT);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
|
|
#if defined(CONFIG_ACCEL_INTERRUPTS) && defined(CONFIG_ACCEL_LIS2DW_AS_BASE)
|
|
/* Interrupt trigger level of power-on-reset is HIGH */
|
|
if (!(MOTIONSENSE_FLAG_INT_ACTIVE_HIGH & s->flags)) {
|
|
ret = st_write_data_with_mask(s, LIS2DW12_H_ACTIVE_ADDR,
|
|
LIS2DW12_H_ACTIVE_MASK,
|
|
LIS2DW12_EN_BIT);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
}
|
|
#endif
|
|
|
|
#ifdef CONFIG_ACCEL_LIS2DWL
|
|
/* lis2dwl supports 14 bit resolution only at high perfomance mode */
|
|
ret = set_power_mode(s, LIS2DW12_HIGH_PERF, 0);
|
|
#else
|
|
/* Set default Mode and Low Power Mode. */
|
|
ret = set_power_mode(s, LIS2DW12_LOW_POWER, LIS2DW12_LOW_POWER_MODE_2);
|
|
#endif
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
|
|
#ifdef CONFIG_ACCEL_LIS2DW_AS_BASE
|
|
if (IS_ENABLED(CONFIG_ACCEL_INTERRUPTS)) {
|
|
ret = lis2dw12_config_interrupt(s);
|
|
if (ret != EC_SUCCESS)
|
|
goto err_unlock;
|
|
}
|
|
#endif
|
|
mutex_unlock(s->mutex);
|
|
|
|
/* Set default resolution. */
|
|
data->resol = LIS2DW12_RESOLUTION;
|
|
return sensor_init_done(s);
|
|
|
|
err_unlock:
|
|
mutex_unlock(s->mutex);
|
|
CPRINTF("[%T %s: MS Init type:0x%X Error]\n", s->name, s->type);
|
|
|
|
return EC_ERROR_UNKNOWN;
|
|
}
|
|
|
|
const struct accelgyro_drv lis2dw12_drv = {
|
|
.init = init,
|
|
.read = read,
|
|
.set_range = set_range,
|
|
.get_range = get_range,
|
|
.get_resolution = st_get_resolution,
|
|
.set_data_rate = set_data_rate,
|
|
.get_data_rate = st_get_data_rate,
|
|
.set_offset = st_set_offset,
|
|
.get_offset = st_get_offset,
|
|
#if defined(CONFIG_ACCEL_INTERRUPTS) && defined(CONFIG_ACCEL_LIS2DW_AS_BASE)
|
|
.irq_handler = lis2dw12_irq_handler,
|
|
#endif /* CONFIG_ACCEL_INTERRUPTS && CONFIG_ACCEL_LIS2DW_AS_BASE */
|
|
};
|