621 lines
14 KiB
C
621 lines
14 KiB
C
// SPDX-License-Identifier: GPL-2.0+
|
|
/*
|
|
* STMicroelectronics st_lsm6dsr embedded function sensor driver
|
|
*
|
|
* Copyright 2019 STMicroelectronics Inc.
|
|
*
|
|
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
|
|
*/
|
|
|
|
#include <linux/kernel.h>
|
|
#include <linux/module.h>
|
|
#include <linux/iio/iio.h>
|
|
|
|
#include "st_lsm6dsr.h"
|
|
|
|
#define ST_LSM6DSR_REG_PAGE_SEL_ADDR 0x02
|
|
#define ST_LSM6DSR_REG_PAGE_SEL_RST_MASK BIT(0)
|
|
|
|
#define ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR 0x04
|
|
#define ST_LSM6DSR_REG_PAGE_ADDRESS 0x08
|
|
#define ST_LSM6DSR_REG_PAGE_VALUE 0x09
|
|
#define ST_LSM6DSR_FSM_BASE_ADDRESS 0x2da
|
|
|
|
#define ST_LSM6DSR_REG_PEDO_EN_MASK BIT(3)
|
|
#define ST_LSM6DSR_REG_TILT_EN_MASK BIT(4)
|
|
#define ST_LSM6DSR_REG_SIGN_MOTION_EN_MASK BIT(5)
|
|
|
|
#define ST_LSM6DSR_REG_INT_DTAP_MASK BIT(3)
|
|
#define ST_LSM6DSR_REG_INT_STAP_MASK BIT(6)
|
|
|
|
#define ST_LSM6DSR_REG_EMB_FUNC_EN_B_ADDR 0x05
|
|
#define ST_LSM6DSR_REG_FSM_EN_MASK BIT(0)
|
|
#define ST_LSM6DSR_REG_INT_STEP_DET_MASK BIT(3)
|
|
#define ST_LSM6DSR_REG_INT_TILT_MASK BIT(4)
|
|
#define ST_LSM6DSR_REG_INT_SIGMOT_MASK BIT(5)
|
|
|
|
#define ST_LSM6DSR_PAGE_RW_ADDR 0x17
|
|
#define ST_LSM6DSR_REG_WR_MASK GENMASK(6, 5)
|
|
#define ST_LSM6DSR_REG_EMB_FUNC_LIR_MASK BIT(7)
|
|
|
|
#define ST_LSM6DSR_REG_EMB_FUNC_FIFO_CFG_ADDR 0x44
|
|
#define ST_LSM6DSR_REG_PEDO_FIFO_EN_MASK BIT(6)
|
|
|
|
#define ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR 0x46
|
|
#define ST_LSM6DSR_REG_FSM_OUTS6_ADDR 0x51
|
|
#define ST_LSM6DSR_REG_ORIENTATION_0_MASK BIT(5)
|
|
#define ST_LSM6DSR_REG_ORIENTATION_90_MASK BIT(7)
|
|
#define ST_LSM6DSR_REG_ORIENTATION_180_MASK BIT(4)
|
|
#define ST_LSM6DSR_REG_ORIENTATION_270_MASK BIT(6)
|
|
|
|
/* Finite State Machine ODR configuration */
|
|
#define ST_LSM6DSR_REG_EMB_FUNC_ODR_CFG_B_ADDR 0x5f
|
|
#define ST_LSM6DSR_REG_FSM_ODR_MASK GENMASK(5, 3)
|
|
#define ST_LSM6DSR_FSM_ODR_12_5 0
|
|
#define ST_LSM6DSR_FSM_ODR_26 1
|
|
#define ST_LSM6DSR_FSM_ODR_52 2
|
|
#define ST_LSM6DSR_FSM_ODR_104 3
|
|
#define ST_LSM6DSR_FSM_ODR_208 4
|
|
#define ST_LSM6DSR_FSM_ODR_416 5
|
|
|
|
#define ST_LSM6DSR_REG_STEP_COUNTER_L_ADDR 0x62
|
|
#define ST_LSM6DSR_REG_EMB_FUNC_SRC_ADDR 0x64
|
|
#define ST_LSM6DSR_REG_PEDO_RST_STEP_MASK BIT(7)
|
|
|
|
#define ST_LSM6DSR_FSM_MAX_SIZE 255
|
|
|
|
/**
|
|
* @struct st_lsm6dsr_fsm_sensor
|
|
* @brief Single FSM description entry
|
|
*
|
|
* Implements #595543 Feature
|
|
*
|
|
* The following FSM state machine LSM6DSR features listed in EX_FUN_FSM_SENSOR:
|
|
*
|
|
* SENSOR_TYPE_GLANCE_GESTURE
|
|
* SENSOR_TYPE_MOTION_DETECT
|
|
* SENSOR_TYPE_STATIONARY_DETECT
|
|
* SENSOR_TYPE_WAKE_GESTURE
|
|
* SENSOR_TYPE_PICK_UP_GESTURE
|
|
* SENSOR_TYPE_WRIST_TILT_GESTURE
|
|
*
|
|
* will be managed as event sensors
|
|
*
|
|
* data: FSM binary data block.
|
|
* id: Sensor Identifier.
|
|
* FSM binary data block len.
|
|
*/
|
|
struct st_lsm6dsr_fsm_sensor {
|
|
u8 data[ST_LSM6DSR_FSM_MAX_SIZE];
|
|
enum st_lsm6dsr_sensor_id id;
|
|
u16 len;
|
|
};
|
|
|
|
static const struct st_lsm6dsr_fsm_sensor st_lsm6dsr_fsm_sensor_list[] = {
|
|
/* glance */
|
|
{
|
|
.id = ST_LSM6DSR_ID_GLANCE,
|
|
.data = {
|
|
0xb2, 0x10, 0x24, 0x20, 0x17, 0x17, 0x66, 0x32,
|
|
0x66, 0x3c, 0x20, 0x20, 0x02, 0x02, 0x08, 0x08,
|
|
0x00, 0x04, 0x0c, 0x00, 0xc7, 0x66, 0x33, 0x73,
|
|
0x77, 0x64, 0x88, 0x75, 0x99, 0x66, 0x33, 0x53,
|
|
0x44, 0xf5, 0x22, 0x00,
|
|
},
|
|
.len = 36,
|
|
},
|
|
/* motion */
|
|
{
|
|
.id = ST_LSM6DSR_ID_MOTION,
|
|
.data = {
|
|
0x51, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3c,
|
|
0x02, 0x00, 0x00, 0x7d, 0x00, 0xc7, 0x05, 0x99,
|
|
0x33, 0x53, 0x44, 0xf5, 0x22, 0x00,
|
|
},
|
|
.len = 22,
|
|
},
|
|
/* no motion */
|
|
{
|
|
.id = ST_LSM6DSR_ID_NO_MOTION,
|
|
.data = {
|
|
0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x66, 0x3c,
|
|
0x02, 0x00, 0x00, 0x7d, 0xff, 0x53, 0x99, 0x50,
|
|
},
|
|
.len = 16,
|
|
},
|
|
/* wakeup */
|
|
{
|
|
.id = ST_LSM6DSR_ID_WAKEUP,
|
|
.data = {
|
|
0xe2, 0x00, 0x1e, 0x20, 0x13, 0x15, 0x66, 0x3e,
|
|
0x66, 0xbe, 0xcd, 0x3c, 0xc0, 0xc0, 0x02, 0x02,
|
|
0x0b, 0x10, 0x05, 0x66, 0xcc, 0x35, 0x38, 0x35,
|
|
0x77, 0xdd, 0x03, 0x54, 0x22, 0x00,
|
|
},
|
|
.len = 30,
|
|
},
|
|
/* pickup */
|
|
{
|
|
.id = ST_LSM6DSR_ID_PICKUP,
|
|
.data = {
|
|
0x51, 0x00, 0x10, 0x00, 0x00, 0x00, 0x33, 0x3c,
|
|
0x02, 0x00, 0x00, 0x05, 0x05, 0x99, 0x30, 0x00,
|
|
},
|
|
.len = 16,
|
|
},
|
|
/* orientation */
|
|
{
|
|
.id = ST_LSM6DSR_ID_ORIENTATION,
|
|
.data = {
|
|
0x91, 0x10, 0x16, 0x00, 0x00, 0x00, 0x66, 0x3a,
|
|
0x66, 0x32, 0xf0, 0x00, 0x00, 0x0d, 0x00, 0xc7,
|
|
0x05, 0x73, 0x99, 0x08, 0xf5, 0x22,
|
|
},
|
|
.len = 22,
|
|
},
|
|
/* wrist tilt */
|
|
{
|
|
.id = ST_LSM6DSR_ID_WRIST_TILT,
|
|
.data = {
|
|
0x52, 0x00, 0x14, 0x00, 0x00, 0x00, 0xae, 0xb7,
|
|
0x80, 0x00, 0x00, 0x06, 0x0f, 0x05, 0x73, 0x33,
|
|
0x07, 0x54, 0x44, 0x22,
|
|
},
|
|
.len = 20,
|
|
},
|
|
};
|
|
|
|
struct st_lsm6dsr_fsm_fs {
|
|
u32 gain;
|
|
__le16 val;
|
|
};
|
|
|
|
static const struct st_lsm6dsr_fsm_fs st_lsm6dsr_fsm_fs_table[] = {
|
|
{ ST_LSM6DSR_ACC_FS_2G_GAIN, 0x03ff },
|
|
{ ST_LSM6DSR_ACC_FS_4G_GAIN, 0x07fe },
|
|
{ ST_LSM6DSR_ACC_FS_8G_GAIN, 0x0bfe },
|
|
{ ST_LSM6DSR_ACC_FS_16G_GAIN, 0x0ffe },
|
|
};
|
|
|
|
static inline
|
|
int st_lsm6dsr_fsm_set_access(struct st_lsm6dsr_hw *hw, bool enable)
|
|
{
|
|
u8 val = enable ? 2 : 0;
|
|
|
|
return __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_PAGE_RW_ADDR,
|
|
ST_LSM6DSR_REG_WR_MASK,
|
|
val);
|
|
}
|
|
|
|
static int st_lsm6dsr_fsm_write(struct st_lsm6dsr_hw *hw, u16 base_addr,
|
|
int len, const u8 *data)
|
|
{
|
|
u8 msb, lsb;
|
|
int i, err;
|
|
|
|
msb = (((base_addr >> 8) & 0xf) << 4) | 1;
|
|
lsb = base_addr & 0xff;
|
|
|
|
err = hw->tf->write(hw->dev,
|
|
ST_LSM6DSR_REG_PAGE_ADDRESS,
|
|
sizeof(lsb),
|
|
&lsb);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
err = hw->tf->write(hw->dev,
|
|
ST_LSM6DSR_REG_PAGE_SEL_ADDR,
|
|
sizeof(msb),
|
|
&msb);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
for (i = 0; i < len; i++) {
|
|
err = hw->tf->write(hw->dev,
|
|
ST_LSM6DSR_REG_PAGE_VALUE,
|
|
sizeof(u8),
|
|
&data[i]);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
if (++lsb == 0) {
|
|
msb += (1 << 4);
|
|
err = hw->tf->write(hw->dev,
|
|
ST_LSM6DSR_REG_PAGE_SEL_ADDR,
|
|
sizeof(msb),
|
|
&msb);
|
|
if (err < 0)
|
|
return err;
|
|
}
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
static int st_lsm6dsr_ef_pg1_sensor_set_enable(struct st_lsm6dsr_sensor *sensor,
|
|
u8 mask, u8 irq_mask,
|
|
bool enable)
|
|
{
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
int err;
|
|
|
|
err = st_lsm6dsr_sensor_set_enable(sensor, enable);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto unlock;
|
|
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR,
|
|
mask, enable);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
err = __st_lsm6dsr_write_with_mask(hw, hw->embfunc_irq_reg, irq_mask,
|
|
enable);
|
|
reset_page:
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
|
|
unlock:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* FSM Function sensor [FSM_FUN]
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param enable: Enable/Disable sensor
|
|
* @return < 0 if error, 0 otherwise
|
|
*/
|
|
static int st_lsm6dsr_fsm_set_enable(struct st_lsm6dsr_sensor *sensor,
|
|
bool enable)
|
|
{
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
u16 enable_mask = hw->fsm_enable_mask;
|
|
int err, i;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list); i++)
|
|
if (st_lsm6dsr_fsm_sensor_list[i].id == sensor->id)
|
|
break;
|
|
|
|
if (i == ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list))
|
|
return -EINVAL;
|
|
|
|
err = st_lsm6dsr_sensor_set_enable(sensor, enable);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto unlock;
|
|
|
|
if (enable)
|
|
enable_mask |= BIT(i);
|
|
else
|
|
enable_mask &= ~BIT(i);
|
|
|
|
err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR,
|
|
sizeof(enable_mask), (u8 *)&enable_mask);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
hw->fsm_enable_mask = enable_mask;
|
|
|
|
reset_page:
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
|
|
unlock:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Enable Embedded Function sensor [EMB_FUN]
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param enable: Enable/Disable sensor
|
|
* @return < 0 if error, 0 otherwise
|
|
*/
|
|
int st_lsm6dsr_embfunc_sensor_set_enable(struct st_lsm6dsr_sensor *sensor,
|
|
bool enable)
|
|
{
|
|
int err;
|
|
|
|
switch (sensor->id) {
|
|
case ST_LSM6DSR_ID_STEP_DETECTOR:
|
|
err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor,
|
|
ST_LSM6DSR_REG_PEDO_EN_MASK,
|
|
ST_LSM6DSR_REG_INT_STEP_DET_MASK,
|
|
enable);
|
|
break;
|
|
case ST_LSM6DSR_ID_SIGN_MOTION:
|
|
err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor,
|
|
ST_LSM6DSR_REG_SIGN_MOTION_EN_MASK,
|
|
ST_LSM6DSR_REG_INT_SIGMOT_MASK,
|
|
enable);
|
|
break;
|
|
case ST_LSM6DSR_ID_TILT:
|
|
err = st_lsm6dsr_ef_pg1_sensor_set_enable(sensor,
|
|
ST_LSM6DSR_REG_TILT_EN_MASK,
|
|
ST_LSM6DSR_REG_TILT_EN_MASK,
|
|
enable);
|
|
break;
|
|
case ST_LSM6DSR_ID_NO_MOTION:
|
|
case ST_LSM6DSR_ID_MOTION:
|
|
case ST_LSM6DSR_ID_WAKEUP:
|
|
case ST_LSM6DSR_ID_PICKUP:
|
|
case ST_LSM6DSR_ID_ORIENTATION:
|
|
case ST_LSM6DSR_ID_WRIST_TILT:
|
|
case ST_LSM6DSR_ID_GLANCE:
|
|
err = st_lsm6dsr_fsm_set_enable(sensor, enable);
|
|
break;
|
|
default:
|
|
err = -EINVAL;
|
|
break;
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Enable Step Counter Sensor [EMB_FUN]
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param enable: Enable/Disable sensor
|
|
* @return < 0 if error, 0 otherwise
|
|
*/
|
|
int st_lsm6dsr_step_counter_set_enable(struct st_lsm6dsr_sensor *sensor,
|
|
bool enable)
|
|
{
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
int err;
|
|
|
|
err = st_lsm6dsr_sensor_set_enable(sensor, enable);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto unlock;
|
|
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_EMB_FUNC_EN_A_ADDR,
|
|
ST_LSM6DSR_REG_PEDO_EN_MASK,
|
|
enable);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_EMB_FUNC_FIFO_CFG_ADDR,
|
|
ST_LSM6DSR_REG_PEDO_FIFO_EN_MASK,
|
|
enable);
|
|
|
|
reset_page:
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
|
|
unlock:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Reset Step Counter value [EMB_FUN]
|
|
*
|
|
* @param iio_dev: IIO device
|
|
* @return < 0 if error, 0 otherwise
|
|
*/
|
|
int st_lsm6dsr_reset_step_counter(struct iio_dev *iio_dev)
|
|
{
|
|
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
u16 prev_val, val = 0;
|
|
__le16 data;
|
|
int err;
|
|
|
|
mutex_lock(&iio_dev->mlock);
|
|
if (iio_buffer_enabled(iio_dev)) {
|
|
err = -EBUSY;
|
|
goto unlock_iio_dev;
|
|
}
|
|
|
|
err = st_lsm6dsr_step_counter_set_enable(sensor, true);
|
|
if (err < 0)
|
|
goto unlock_iio_dev;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto unlock_page;
|
|
|
|
do {
|
|
prev_val = val;
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_EMB_FUNC_SRC_ADDR,
|
|
ST_LSM6DSR_REG_PEDO_RST_STEP_MASK, 1);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
msleep(100);
|
|
|
|
err = hw->tf->read(hw->dev,
|
|
ST_LSM6DSR_REG_STEP_COUNTER_L_ADDR,
|
|
sizeof(data), (u8 *)&data);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
val = le16_to_cpu(data);
|
|
} while (val && val >= prev_val);
|
|
|
|
reset_page:
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
|
|
unlock_page:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
err = st_lsm6dsr_step_counter_set_enable(sensor, false);
|
|
unlock_iio_dev:
|
|
mutex_unlock(&iio_dev->mlock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Read Orientation data sensor [EMB_FUN]
|
|
*
|
|
* @param hw: ST IMU MEMS hw instance.
|
|
* @param out: Out data buffer.
|
|
* @return < 0 if error, 0 otherwise
|
|
*/
|
|
int st_lsm6dsr_fsm_get_orientation(struct st_lsm6dsr_hw *hw, u8 *out)
|
|
{
|
|
int err;
|
|
u8 data;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto unlock;
|
|
|
|
err = hw->tf->read(hw->dev, ST_LSM6DSR_REG_FSM_OUTS6_ADDR,
|
|
sizeof(data), &data);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
switch (data) {
|
|
case ST_LSM6DSR_REG_ORIENTATION_0_MASK:
|
|
*out = 0;
|
|
break;
|
|
case ST_LSM6DSR_REG_ORIENTATION_90_MASK:
|
|
*out = 1;
|
|
break;
|
|
case ST_LSM6DSR_REG_ORIENTATION_180_MASK:
|
|
*out = 2;
|
|
break;
|
|
case ST_LSM6DSR_REG_ORIENTATION_270_MASK:
|
|
*out = 3;
|
|
break;
|
|
default:
|
|
err = -EINVAL;
|
|
break;
|
|
}
|
|
|
|
reset_page:
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
|
|
unlock:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
|
|
/**
|
|
* Initialize Finite State Machine HW block [FSM_FUN]
|
|
*
|
|
* @param hw: ST IMU MEMS hw instance
|
|
* @return < 0 if error, 0 otherwise
|
|
*/
|
|
int st_lsm6dsr_fsm_init(struct st_lsm6dsr_hw *hw)
|
|
{
|
|
u8 nfsm[] = {
|
|
ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list),
|
|
ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list)
|
|
};
|
|
__le16 irq_mask, fsm_addr = ST_LSM6DSR_FSM_BASE_ADDRESS;
|
|
u8 val[2] = {};
|
|
int i, err;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto unlock;
|
|
|
|
/* enable gesture rec */
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_EMB_FUNC_EN_B_ADDR,
|
|
ST_LSM6DSR_REG_FSM_EN_MASK,
|
|
1);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
/* gest rec ODR 52Hz */
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_EMB_FUNC_ODR_CFG_B_ADDR,
|
|
ST_LSM6DSR_REG_FSM_ODR_MASK,
|
|
ST_LSM6DSR_FSM_ODR_52);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
/* disable all fsm sensors */
|
|
err = hw->tf->write(hw->dev, ST_LSM6DSR_REG_FSM_ENABLE_A_ADDR,
|
|
sizeof(val), val);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
/* enable fsm interrupt */
|
|
irq_mask = (1 << ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list)) - 1;
|
|
err = hw->tf->write(hw->dev, hw->embfunc_irq_reg + 1,
|
|
sizeof(irq_mask),
|
|
(u8 *)&irq_mask);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
/* enable latched interrupts */
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_PAGE_RW_ADDR,
|
|
ST_LSM6DSR_REG_EMB_FUNC_LIR_MASK,
|
|
1);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
/* enable access */
|
|
err = st_lsm6dsr_fsm_set_access(hw, true);
|
|
if (err < 0)
|
|
goto reset_page;
|
|
|
|
/* # of configured fsm */
|
|
err = st_lsm6dsr_fsm_write(hw, 0x17c, sizeof(nfsm), nfsm);
|
|
if (err < 0)
|
|
goto reset_access;
|
|
|
|
err = st_lsm6dsr_fsm_write(hw, 0x17e, sizeof(fsm_addr), (u8 *)
|
|
&fsm_addr);
|
|
if (err < 0)
|
|
goto reset_access;
|
|
|
|
/* configure fsm */
|
|
for (i = 0; i < ARRAY_SIZE(st_lsm6dsr_fsm_sensor_list); i++) {
|
|
err = st_lsm6dsr_fsm_write(hw, fsm_addr,
|
|
st_lsm6dsr_fsm_sensor_list[i].len,
|
|
st_lsm6dsr_fsm_sensor_list[i].data);
|
|
if (err < 0)
|
|
goto reset_access;
|
|
|
|
fsm_addr += st_lsm6dsr_fsm_sensor_list[i].len;
|
|
}
|
|
|
|
reset_access:
|
|
st_lsm6dsr_fsm_set_access(hw, false);
|
|
|
|
__st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_PAGE_SEL_ADDR,
|
|
ST_LSM6DSR_REG_PAGE_SEL_RST_MASK, 1);
|
|
reset_page:
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_FUNC_CFG_MASK, false);
|
|
unlock:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|