1002 lines
25 KiB
C
1002 lines
25 KiB
C
// SPDX-License-Identifier: GPL-2.0+
|
|
/*
|
|
* STMicroelectronics st_lsm6dsr sensor hub library driver
|
|
*
|
|
* Copyright 2020 STMicroelectronics Inc.
|
|
*
|
|
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
|
|
*/
|
|
|
|
#include <linux/module.h>
|
|
#include <linux/delay.h>
|
|
#include <linux/iio/iio.h>
|
|
#include <linux/iio/sysfs.h>
|
|
#include <asm/unaligned.h>
|
|
|
|
#include "st_lsm6dsr.h"
|
|
|
|
#define ST_LSM6DSR_REG_MASTER_CONFIG_ADDR 0x14
|
|
#define ST_LSM6DSR_REG_WRITE_ONCE_MASK BIT(6)
|
|
#define ST_LSM6DSR_REG_MASTER_ON_MASK BIT(2)
|
|
|
|
#define ST_LSM6DSR_REG_SLV0_ADDR 0x15
|
|
#define ST_LSM6DSR_REG_SLV0_CFG 0x17
|
|
#define ST_LSM6DSR_REG_SLV1_ADDR 0x18
|
|
#define ST_LSM6DSR_REG_SLV2_ADDR 0x1b
|
|
#define ST_LSM6DSR_REG_SLV3_ADDR 0x1e
|
|
#define ST_LSM6DSR_REG_DATAWRITE_SLV0_ADDR 0x21
|
|
#define ST_LSM6DSR_REG_BATCH_EXT_SENS_EN_MASK BIT(3)
|
|
#define ST_LSM6DSR_REG_SLAVE_NUMOP_MASK GENMASK(2, 0)
|
|
|
|
#define ST_LSM6DSR_REG_SLV0_OUT_ADDR 0x02
|
|
#define ST_LSM6DSR_MAX_SLV_NUM 2
|
|
|
|
/**
|
|
* @struct st_lsm6dsr_ext_pwr
|
|
* @brief External device Power Management description
|
|
* reg: Generic sensor register description.
|
|
* off_val: Value to write into register to power off external sensor.
|
|
* on_val: Value to write into register for power on external sensor.
|
|
*/
|
|
struct st_lsm6dsr_ext_pwr {
|
|
struct st_lsm6dsr_reg reg;
|
|
u8 off_val;
|
|
u8 on_val;
|
|
};
|
|
|
|
/**
|
|
* @struct st_lsm6dsr_ext_dev_settings
|
|
* @brief External sensor descritor entry
|
|
* i2c_addr: External I2C device address (max two).
|
|
* wai_addr: Device ID address.
|
|
* wai_val: Device ID value.
|
|
* odr_table: ODR sensor table.
|
|
* fs_table: Full scale table.
|
|
* temp_comp_reg: Temperature compensation registers.
|
|
* pwr_table: External device Power Management description.
|
|
* off_canc_reg: Offset cancellation registers.
|
|
* bdu_reg: Block Data Update registers.
|
|
* ext_available_scan_masks: IIO device scan mask.
|
|
* ext_channels:IIO device channel specifications.
|
|
* ext_chan_depth: Max number of IIO device channel specifications.
|
|
* data_len: Sensor output data len.
|
|
*/
|
|
struct st_lsm6dsr_ext_dev_settings {
|
|
u8 i2c_addr[2];
|
|
u8 wai_addr;
|
|
u8 wai_val;
|
|
struct st_lsm6dsr_odr_table_entry odr_table;
|
|
struct st_lsm6dsr_fs_table_entry fs_table;
|
|
struct st_lsm6dsr_reg temp_comp_reg;
|
|
struct st_lsm6dsr_ext_pwr pwr_table;
|
|
struct st_lsm6dsr_reg off_canc_reg;
|
|
struct st_lsm6dsr_reg bdu_reg;
|
|
unsigned long ext_available_scan_masks[2];
|
|
const struct iio_chan_spec ext_channels[5];
|
|
u8 ext_chan_depth;
|
|
u8 data_len;
|
|
};
|
|
|
|
static const struct st_lsm6dsr_ext_dev_settings st_lsm6dsr_ext_dev_table[] = {
|
|
/* LIS2MDL */
|
|
{
|
|
.i2c_addr = { 0x1e },
|
|
.wai_addr = 0x4f,
|
|
.wai_val = 0x40,
|
|
.odr_table = {
|
|
.odr_size = 5,
|
|
.reg = {
|
|
.addr = 0x60,
|
|
.mask = GENMASK(3, 2),
|
|
},
|
|
/*
|
|
* added 5Hz for CTS coverage, reg value is the same
|
|
* for 5 and 10 Hz
|
|
*/
|
|
.odr_avl[0] = { 5, 1, 0x0 },
|
|
.odr_avl[1] = { 10, 0, 0x0 },
|
|
.odr_avl[2] = { 20, 0, 0x1 },
|
|
.odr_avl[3] = { 50, 0, 0x2 },
|
|
.odr_avl[4] = { 100, 0, 0x3 },
|
|
},
|
|
.fs_table = {
|
|
.size = 1,
|
|
.fs_avl[0] = {
|
|
.gain = 1500,
|
|
.val = 0x0,
|
|
}, /* 1500 uG/LSB */
|
|
},
|
|
.temp_comp_reg = {
|
|
.addr = 0x60,
|
|
.mask = BIT(7),
|
|
},
|
|
.pwr_table = {
|
|
.reg = {
|
|
.addr = 0x60,
|
|
.mask = GENMASK(1, 0),
|
|
},
|
|
.off_val = 0x2,
|
|
.on_val = 0x0,
|
|
},
|
|
.off_canc_reg = {
|
|
.addr = 0x61,
|
|
.mask = BIT(1),
|
|
},
|
|
.bdu_reg = {
|
|
.addr = 0x62,
|
|
.mask = BIT(4),
|
|
},
|
|
.ext_available_scan_masks = { 0x7, 0x0 },
|
|
.ext_channels[0] = ST_LSM6DSR_DATA_CHANNEL(IIO_MAGN, 0x68,
|
|
1, IIO_MOD_X, 0,
|
|
16, 16, 's'),
|
|
.ext_channels[1] = ST_LSM6DSR_DATA_CHANNEL(IIO_MAGN, 0x6a,
|
|
1, IIO_MOD_Y, 1,
|
|
16, 16, 's'),
|
|
.ext_channels[2] = ST_LSM6DSR_DATA_CHANNEL(IIO_MAGN, 0x6c,
|
|
1, IIO_MOD_Z, 2,
|
|
16, 16, 's'),
|
|
.ext_channels[3] = ST_LSM6DSR_EVENT_CHANNEL(IIO_MAGN, flush),
|
|
.ext_channels[4] = IIO_CHAN_SOFT_TIMESTAMP(3),
|
|
.ext_chan_depth = 5,
|
|
.data_len = 6,
|
|
},
|
|
/* LPS22HB */
|
|
{
|
|
.i2c_addr = { 0x5c, 0x5d },
|
|
.wai_addr = 0x0f,
|
|
.wai_val = 0xb1,
|
|
.odr_table = {
|
|
.odr_size = 4,
|
|
.reg = {
|
|
.addr = 0x10,
|
|
.mask = GENMASK(6, 4),
|
|
},
|
|
.odr_avl[0] = { 1, 0, 0x1 },
|
|
.odr_avl[1] = { 10, 0, 0x2 },
|
|
.odr_avl[2] = { 25, 0, 0x3 },
|
|
.odr_avl[3] = { 50, 0, 0x4 },
|
|
},
|
|
.fs_table = {
|
|
.size = 1,
|
|
/* hPa miscro scale */
|
|
.fs_avl[0] = {
|
|
.gain = 1000000UL/4096UL,
|
|
.val = 0x0,
|
|
},
|
|
},
|
|
.bdu_reg = {
|
|
.addr = 0x10,
|
|
.mask = BIT(1),
|
|
},
|
|
.ext_available_scan_masks = { 0x1, 0x0 },
|
|
.ext_channels[0] = ST_LSM6DSR_DATA_CHANNEL(IIO_PRESSURE, 0x28,
|
|
0, IIO_NO_MOD, 0,
|
|
24, 32, 'u'),
|
|
.ext_channels[1] = ST_LSM6DSR_EVENT_CHANNEL(IIO_PRESSURE,
|
|
flush),
|
|
.ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1),
|
|
.ext_chan_depth = 3,
|
|
.data_len = 3,
|
|
},
|
|
/* LPS22HH */
|
|
{
|
|
.i2c_addr = { 0x5c, 0x5d },
|
|
.wai_addr = 0x0f,
|
|
.wai_val = 0xb3,
|
|
.odr_table = {
|
|
.odr_size = 5,
|
|
.reg = {
|
|
.addr = 0x10,
|
|
.mask = GENMASK(6, 4),
|
|
},
|
|
.odr_avl[0] = { 1, 0, 0x1 },
|
|
.odr_avl[1] = { 10, 0, 0x2 },
|
|
.odr_avl[2] = { 25, 0, 0x3 },
|
|
.odr_avl[3] = { 50, 0, 0x4 },
|
|
.odr_avl[4] = { 100, 0, 0x6 },
|
|
},
|
|
.fs_table = {
|
|
.size = 1,
|
|
/* hPa miscro scale */
|
|
.fs_avl[0] = {
|
|
.gain = 1000000UL/4096UL,
|
|
.val = 0x0,
|
|
},
|
|
},
|
|
.bdu_reg = {
|
|
.addr = 0x10,
|
|
.mask = BIT(1),
|
|
},
|
|
.ext_available_scan_masks = { 0x1, 0x0 },
|
|
.ext_channels[0] = ST_LSM6DSR_DATA_CHANNEL(IIO_PRESSURE, 0x28,
|
|
0, IIO_NO_MOD, 0,
|
|
24, 32, 'u'),
|
|
.ext_channels[1] = ST_LSM6DSR_EVENT_CHANNEL(IIO_PRESSURE,
|
|
flush),
|
|
.ext_channels[2] = IIO_CHAN_SOFT_TIMESTAMP(1),
|
|
.ext_chan_depth = 3,
|
|
.data_len = 3,
|
|
},
|
|
};
|
|
|
|
/**
|
|
* Wait write trigger [SHUB]
|
|
*
|
|
* In write on external device register, each operation is triggered
|
|
* by accel/gyro data ready, this means that wait time depends on ODR
|
|
* plus i2c time
|
|
* NOTE: Be sure to enable Acc or Gyro before this operation
|
|
*
|
|
* @param hw: ST IMU MEMS hw instance.
|
|
*/
|
|
static inline void st_lsm6dsr_shub_wait_complete(struct st_lsm6dsr_hw *hw)
|
|
{
|
|
struct st_lsm6dsr_sensor *sensor;
|
|
u16 odr;
|
|
|
|
sensor = iio_priv(hw->iio_devs[ST_LSM6DSR_ID_ACC]);
|
|
/* check if acc is enabled */
|
|
odr = (hw->enable_mask & BIT(ST_LSM6DSR_ID_ACC)) ? sensor->odr : 13;
|
|
msleep((2000U / odr) + 1);
|
|
}
|
|
|
|
/**
|
|
* Read from sensor hub bank register [SHUB]
|
|
*
|
|
* NOTE: uses page_lock
|
|
*
|
|
* @param hw: ST IMU MEMS hw instance.
|
|
* @param addr: Remote address register.
|
|
* @param data: Data buffer.
|
|
* @param len: Data read len.
|
|
* @return 0 if OK, < 0 if ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_read_reg(struct st_lsm6dsr_hw *hw, u8 addr,
|
|
u8 *data, int len)
|
|
{
|
|
int err;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto out;
|
|
|
|
err = hw->tf->read(hw->dev, addr, len, data);
|
|
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK, false);
|
|
out:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Write to sensor hub bank register [SHUB]
|
|
*
|
|
* NOTE: uses page_lock
|
|
*
|
|
* @param hw: ST IMU MEMS hw instance.
|
|
* @param addr: Remote address register.
|
|
* @param data: Data buffer.
|
|
* @param len: Data read len.
|
|
* @return 0 if OK, < 0 if ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_write_reg(struct st_lsm6dsr_hw *hw, u8 addr,
|
|
u8 *data, int len)
|
|
{
|
|
int err;
|
|
|
|
mutex_lock(&hw->page_lock);
|
|
err = st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto out;
|
|
|
|
err = hw->tf->write(hw->dev, addr, len, data);
|
|
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK, false);
|
|
out:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Enable sensor hub interface [SHUB]
|
|
*
|
|
* NOTE: uses page_lock
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param enable: Master Enable/Disable.
|
|
* @return 0 if OK, < 0 if ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_master_enable(struct st_lsm6dsr_sensor *sensor,
|
|
bool enable)
|
|
{
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
int err;
|
|
|
|
/* enable acc sensor as trigger */
|
|
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_SHUB_REG_MASK,
|
|
true);
|
|
if (err < 0)
|
|
goto out;
|
|
|
|
err = __st_lsm6dsr_write_with_mask(hw,
|
|
ST_LSM6DSR_REG_MASTER_CONFIG_ADDR,
|
|
ST_LSM6DSR_REG_MASTER_ON_MASK,
|
|
enable);
|
|
|
|
st_lsm6dsr_set_page_access(hw, ST_LSM6DSR_REG_SHUB_REG_MASK, false);
|
|
|
|
out:
|
|
mutex_unlock(&hw->page_lock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Read sensor data register from shub interface
|
|
*
|
|
* NOTE: use SLV3 i2c slave for one-shot read operation
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param addr: Remote address register.
|
|
* @param data: Data buffer.
|
|
* @param len: Data read len.
|
|
* @return 0 if OK, < 0 if ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_read(struct st_lsm6dsr_sensor *sensor, u8 addr,
|
|
u8 *data, int len)
|
|
{
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
u8 out_addr = ST_LSM6DSR_REG_SLV0_OUT_ADDR + hw->ext_data_len;
|
|
u8 config[3];
|
|
int err;
|
|
|
|
config[0] = (ext_info->ext_dev_i2c_addr << 1) | 1;
|
|
config[1] = addr;
|
|
config[2] = len & 0x7;
|
|
|
|
err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV3_ADDR,
|
|
config, sizeof(config));
|
|
if (err < 0)
|
|
return err;
|
|
|
|
err = st_lsm6dsr_shub_master_enable(sensor, true);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
st_lsm6dsr_shub_wait_complete(hw);
|
|
|
|
err = st_lsm6dsr_shub_read_reg(hw, out_addr, data, len & 0x7);
|
|
|
|
st_lsm6dsr_shub_master_enable(sensor, false);
|
|
|
|
memset(config, 0, sizeof(config));
|
|
return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV3_ADDR,
|
|
config, sizeof(config));
|
|
}
|
|
|
|
/**
|
|
* Write sensor data register from shub interface
|
|
*
|
|
* NOTE: use SLV0 i2c slave for write operation
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param addr: Remote address register.
|
|
* @param data: Data buffer.
|
|
* @param len: Data read len.
|
|
* @return 0 if OK, < 0 if ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_write(struct st_lsm6dsr_sensor *sensor, u8 addr,
|
|
u8 *data, int len)
|
|
{
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
u8 mconfig = ST_LSM6DSR_REG_WRITE_ONCE_MASK | 3;
|
|
u8 config[3] = {};
|
|
int err, i;
|
|
|
|
/* AuxSens = 3 + wr once */
|
|
err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_MASTER_CONFIG_ADDR,
|
|
&mconfig, sizeof(mconfig));
|
|
if (err < 0)
|
|
return err;
|
|
|
|
config[0] = ext_info->ext_dev_i2c_addr << 1;
|
|
for (i = 0; i < len; i++) {
|
|
config[1] = addr + i;
|
|
|
|
err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV0_ADDR,
|
|
config, sizeof(config));
|
|
if (err < 0)
|
|
return err;
|
|
|
|
err = st_lsm6dsr_shub_write_reg(hw,
|
|
ST_LSM6DSR_REG_DATAWRITE_SLV0_ADDR,
|
|
&data[i], 1);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
err = st_lsm6dsr_shub_master_enable(sensor, true);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
st_lsm6dsr_shub_wait_complete(hw);
|
|
|
|
st_lsm6dsr_shub_master_enable(sensor, false);
|
|
}
|
|
|
|
return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV0_ADDR,
|
|
config, sizeof(config));
|
|
}
|
|
|
|
/**
|
|
* Write sensor data register from shub interface using register bitmask
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param addr: Remote address register.
|
|
* @param mask: Register bitmask.
|
|
* @param val: Data buffer.
|
|
* @return 0 if OK, < 0 if ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_write_with_mask(struct st_lsm6dsr_sensor *sensor,
|
|
u8 addr, u8 mask, u8 val)
|
|
{
|
|
int err;
|
|
u8 data;
|
|
|
|
err = st_lsm6dsr_shub_read(sensor, addr, &data, sizeof(data));
|
|
if (err < 0)
|
|
return err;
|
|
|
|
data = ((data & ~mask) | (val << __ffs(mask) & mask));
|
|
|
|
return st_lsm6dsr_shub_write(sensor, addr, &data, sizeof(data));
|
|
}
|
|
|
|
/**
|
|
* Configure external sensor connected on master I2C interface
|
|
*
|
|
* NOTE: use SLV1/SLV2 i2c slave for FIFO read operation
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param enable: Enable/Disable sensor.
|
|
* @return 0 if OK, < 0 if ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_config_channels(struct st_lsm6dsr_sensor *sensor,
|
|
bool enable)
|
|
{
|
|
struct st_lsm6dsr_ext_dev_info *ext_info;
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
struct st_lsm6dsr_sensor *cur_sensor;
|
|
u8 config[6] = {}, enable_mask;
|
|
int i, j = 0;
|
|
|
|
enable_mask = enable ? hw->enable_mask | BIT(sensor->id)
|
|
: hw->enable_mask & ~BIT(sensor->id);
|
|
|
|
for (i = ST_LSM6DSR_ID_EXT0; i <= ST_LSM6DSR_ID_EXT1; i++) {
|
|
if (!hw->iio_devs[i])
|
|
continue;
|
|
|
|
cur_sensor = iio_priv(hw->iio_devs[i]);
|
|
if (!(enable_mask & BIT(cur_sensor->id)))
|
|
continue;
|
|
|
|
ext_info = &cur_sensor->ext_dev_info;
|
|
config[j] = (ext_info->ext_dev_i2c_addr << 1) | 1;
|
|
config[j + 1] =
|
|
ext_info->ext_dev_settings->ext_channels[0].address;
|
|
config[j + 2] = ST_LSM6DSR_REG_BATCH_EXT_SENS_EN_MASK |
|
|
(ext_info->ext_dev_settings->data_len &
|
|
ST_LSM6DSR_REG_SLAVE_NUMOP_MASK);
|
|
j += 3;
|
|
}
|
|
|
|
return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV1_ADDR,
|
|
config, sizeof(config));
|
|
}
|
|
|
|
/**
|
|
* Get a valid ODR [SHUB]
|
|
*
|
|
* Check a valid ODR closest to the passed value
|
|
*
|
|
* @param sensor: SST IMU sensor instance.
|
|
* @param odr: ODR value (in Hz).
|
|
* @param val: ODR register value data pointer.
|
|
* @return 0 if OK, negative value for ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_get_odr_val(struct st_lsm6dsr_sensor *sensor,
|
|
u16 odr, u8 *val)
|
|
{
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
int i;
|
|
|
|
for (i = 0; i < ext_info->ext_dev_settings->odr_table.odr_size; i++)
|
|
if (ext_info->ext_dev_settings->odr_table.odr_avl[i].hz >= odr)
|
|
break;
|
|
|
|
if (i == ext_info->ext_dev_settings->odr_table.odr_size)
|
|
return -EINVAL;
|
|
|
|
*val = ext_info->ext_dev_settings->odr_table.odr_avl[i].val;
|
|
|
|
/* set decimator for low ODR */
|
|
sensor->decimator =
|
|
ext_info->ext_dev_settings->odr_table.odr_avl[i].uhz;
|
|
sensor->dec_counter = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Set new ODR to sensor [SHUB]
|
|
*
|
|
* Set a valid ODR closest to the passed value
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param odr: ODR value (in Hz).
|
|
* @return 0 if OK, negative value for ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_set_odr(struct st_lsm6dsr_sensor *sensor, u16 odr)
|
|
{
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
struct st_lsm6dsr_hw *hw = sensor->hw;
|
|
u8 odr_val;
|
|
int err;
|
|
|
|
err = st_lsm6dsr_shub_get_odr_val(sensor, odr, &odr_val);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
if (sensor->odr == odr && (hw->enable_mask & BIT(sensor->id)))
|
|
return 0;
|
|
|
|
return st_lsm6dsr_shub_write_with_mask(sensor,
|
|
ext_info->ext_dev_settings->odr_table.reg.addr,
|
|
ext_info->ext_dev_settings->odr_table.reg.mask,
|
|
odr_val);
|
|
}
|
|
|
|
/**
|
|
* Enable or Disable sensor [SHUB]
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param enable: Enable or disable the sensor [true,false].
|
|
* @return 0 if OK, negative value for ERROR
|
|
*/
|
|
int st_lsm6dsr_shub_set_enable(struct st_lsm6dsr_sensor *sensor, bool enable)
|
|
{
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
int err;
|
|
|
|
err = st_lsm6dsr_shub_config_channels(sensor, enable);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
if (enable) {
|
|
err = st_lsm6dsr_shub_set_odr(sensor, sensor->odr);
|
|
if (err < 0)
|
|
return err;
|
|
} else {
|
|
err = st_lsm6dsr_shub_write_with_mask(sensor,
|
|
ext_info->ext_dev_settings->odr_table.reg.addr,
|
|
ext_info->ext_dev_settings->odr_table.reg.mask,
|
|
0);
|
|
if (err < 0)
|
|
return err;
|
|
}
|
|
|
|
if (ext_info->ext_dev_settings->pwr_table.reg.addr) {
|
|
u8 val;
|
|
|
|
val = enable ? ext_info->ext_dev_settings->pwr_table.on_val
|
|
: ext_info->ext_dev_settings->pwr_table.off_val;
|
|
err = st_lsm6dsr_shub_write_with_mask(sensor,
|
|
ext_info->ext_dev_settings->pwr_table.reg.addr,
|
|
ext_info->ext_dev_settings->pwr_table.reg.mask,
|
|
val);
|
|
if (err < 0)
|
|
return err;
|
|
}
|
|
|
|
return st_lsm6dsr_shub_master_enable(sensor, enable);
|
|
}
|
|
|
|
static inline u32 st_lsm6dsr_get_unaligned_le24(const u8 *p)
|
|
{
|
|
return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
|
|
}
|
|
|
|
/**
|
|
* Single sensor read operation [SHUB]
|
|
*
|
|
* @param sensor: ST IMU sensor instance
|
|
* @param ch: IIO Channel.
|
|
* @param val: Output data register value.
|
|
* @return IIO_VAL_INT if OK, negative value for ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_read_oneshot(struct st_lsm6dsr_sensor *sensor,
|
|
struct iio_chan_spec const *ch,
|
|
int *val)
|
|
{
|
|
int err, delay, len = ch->scan_type.realbits >> 3;
|
|
u8 data[ST_LSM6DSR_RX_MAX_LENGTH];
|
|
|
|
err = st_lsm6dsr_shub_set_enable(sensor, true);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
delay = 1000000 / sensor->odr;
|
|
usleep_range(delay, 2 * delay);
|
|
|
|
err = st_lsm6dsr_shub_read(sensor, ch->address, data, len);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
st_lsm6dsr_shub_set_enable(sensor, false);
|
|
|
|
switch (len) {
|
|
case 3:
|
|
*val = (s32)st_lsm6dsr_get_unaligned_le24(data);
|
|
break;
|
|
case 2:
|
|
*val = (s16)get_unaligned_le16(data);
|
|
break;
|
|
default:
|
|
return -EINVAL;
|
|
}
|
|
|
|
return IIO_VAL_INT;
|
|
}
|
|
|
|
/**
|
|
* Read Sensor data configuration [SHUB]
|
|
*
|
|
* @param iio_dev: IIO Device.
|
|
* @param ch: IIO Channel.
|
|
* @param val: Data Buffer (MSB).
|
|
* @param val2: Data Buffer (LSB).
|
|
* @param mask: Data Mask.
|
|
* @return 0 if OK, -EINVAL value for ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_read_raw(struct iio_dev *iio_dev,
|
|
struct iio_chan_spec const *ch,
|
|
int *val, int *val2, long mask)
|
|
{
|
|
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
|
|
int ret;
|
|
|
|
switch (mask) {
|
|
case IIO_CHAN_INFO_RAW:
|
|
mutex_lock(&iio_dev->mlock);
|
|
if (iio_buffer_enabled(iio_dev)) {
|
|
ret = -EBUSY;
|
|
mutex_unlock(&iio_dev->mlock);
|
|
break;
|
|
}
|
|
ret = st_lsm6dsr_shub_read_oneshot(sensor, ch, val);
|
|
mutex_unlock(&iio_dev->mlock);
|
|
break;
|
|
case IIO_CHAN_INFO_SAMP_FREQ:
|
|
*val = sensor->odr;
|
|
ret = IIO_VAL_INT;
|
|
break;
|
|
case IIO_CHAN_INFO_SCALE:
|
|
*val = 0;
|
|
*val2 = sensor->gain;
|
|
ret = IIO_VAL_INT_PLUS_MICRO;
|
|
break;
|
|
default:
|
|
ret = -EINVAL;
|
|
break;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* Write Sensor data configuration [SHUB]
|
|
*
|
|
* @param iio_dev: IIO Device.
|
|
* @param chan: IIO Channel.
|
|
* @param val: Data Buffer (MSB).
|
|
* @param val2: Data Buffer (LSB).
|
|
* @param mask: Data Mask.
|
|
* @return 0 if OK, -EINVAL value for ERROR
|
|
*/
|
|
static int st_lsm6dsr_shub_write_raw(struct iio_dev *iio_dev,
|
|
struct iio_chan_spec const *chan,
|
|
int val, int val2, long mask)
|
|
{
|
|
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
|
|
int err;
|
|
|
|
mutex_lock(&iio_dev->mlock);
|
|
|
|
switch (mask) {
|
|
case IIO_CHAN_INFO_SAMP_FREQ: {
|
|
u8 data;
|
|
|
|
err = st_lsm6dsr_shub_get_odr_val(sensor, val, &data);
|
|
if (!err)
|
|
sensor->odr = val;
|
|
break;
|
|
}
|
|
case IIO_CHAN_INFO_SCALE:
|
|
err = 0;
|
|
break;
|
|
default:
|
|
err = -EINVAL;
|
|
break;
|
|
}
|
|
|
|
mutex_unlock(&iio_dev->mlock);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Get a list of available sensor ODR [SHUB]
|
|
*
|
|
* List of available ODR returned separated by commas
|
|
*
|
|
* @param dev: IIO Device.
|
|
* @param attr: IIO Channel attribute.
|
|
* @param buf: User buffer.
|
|
* @return buffer len
|
|
*/
|
|
static ssize_t
|
|
st_lsm6dsr_sysfs_shub_sampling_freq_avail(struct device *dev,
|
|
struct device_attribute *attr,
|
|
char *buf)
|
|
{
|
|
struct st_lsm6dsr_sensor *sensor = iio_priv(dev_get_drvdata(dev));
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
int i, len = 0;
|
|
|
|
for (i = 0; i < ST_LSM6DSR_ODR_LIST_SIZE; i++) {
|
|
u16 val = ext_info->ext_dev_settings->odr_table.odr_avl[i].hz;
|
|
|
|
if (val > 0)
|
|
len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
|
|
val);
|
|
}
|
|
buf[len - 1] = '\n';
|
|
|
|
return len;
|
|
}
|
|
|
|
/**
|
|
* Get a list of available sensor Full Scale [SHUB]
|
|
*
|
|
* List of available Full Scale returned separated by commas
|
|
*
|
|
* @param dev: IIO Device.
|
|
* @param attr: IIO Channel attribute.
|
|
* @param buf: User buffer.
|
|
* @return buffer len
|
|
*/
|
|
static ssize_t st_lsm6dsr_sysfs_shub_scale_avail(struct device *dev,
|
|
struct device_attribute *attr,
|
|
char *buf)
|
|
{
|
|
struct st_lsm6dsr_sensor *sensor = iio_priv(dev_get_drvdata(dev));
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
int i, len = 0;
|
|
|
|
for (i = 0; i < ext_info->ext_dev_settings->fs_table.size; i++) {
|
|
u16 val = ext_info->ext_dev_settings->fs_table.fs_avl[i].gain;
|
|
|
|
if (val > 0)
|
|
len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
|
|
val);
|
|
}
|
|
buf[len - 1] = '\n';
|
|
|
|
return len;
|
|
}
|
|
|
|
static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsr_sysfs_shub_sampling_freq_avail);
|
|
static IIO_DEVICE_ATTR(in_ext_scale_available, 0444,
|
|
st_lsm6dsr_sysfs_shub_scale_avail, NULL, 0);
|
|
static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444,
|
|
st_lsm6dsr_get_max_watermark, NULL, 0);
|
|
static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lsm6dsr_flush_fifo, 0);
|
|
static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_lsm6dsr_get_watermark,
|
|
st_lsm6dsr_set_watermark, 0);
|
|
|
|
static struct attribute *st_lsm6dsr_ext_attributes[] = {
|
|
&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
|
|
&iio_dev_attr_in_ext_scale_available.dev_attr.attr,
|
|
&iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
|
|
&iio_dev_attr_hwfifo_watermark.dev_attr.attr,
|
|
&iio_dev_attr_hwfifo_flush.dev_attr.attr,
|
|
NULL,
|
|
};
|
|
|
|
static const struct attribute_group st_lsm6dsr_ext_attribute_group = {
|
|
.attrs = st_lsm6dsr_ext_attributes,
|
|
};
|
|
|
|
static const struct iio_info st_lsm6dsr_ext_info = {
|
|
.attrs = &st_lsm6dsr_ext_attribute_group,
|
|
.read_raw = st_lsm6dsr_shub_read_raw,
|
|
.write_raw = st_lsm6dsr_shub_write_raw,
|
|
};
|
|
|
|
/**
|
|
* Allocate IIO device [SHUB]
|
|
*
|
|
* @param hw: ST IMU MEMS hw instance.
|
|
* @param ext_settings: xternal sensor descritor entry.
|
|
* @param id: Sensor Identifier.
|
|
* @param i2c_addr: external I2C address on master bus.
|
|
* @return struct iio_dev *, NULL if ERROR
|
|
*/
|
|
static struct iio_dev *st_lsm6dsr_shub_alloc_iio_dev(struct st_lsm6dsr_hw *hw,
|
|
const struct st_lsm6dsr_ext_dev_settings *ext_settings,
|
|
enum st_lsm6dsr_sensor_id id, u8 i2c_addr)
|
|
{
|
|
struct st_lsm6dsr_sensor *sensor;
|
|
struct iio_dev *iio_dev;
|
|
|
|
iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
|
|
if (!iio_dev)
|
|
return NULL;
|
|
|
|
iio_dev->modes = INDIO_DIRECT_MODE;
|
|
iio_dev->dev.parent = hw->dev;
|
|
iio_dev->available_scan_masks = ext_settings->ext_available_scan_masks;
|
|
iio_dev->info = &st_lsm6dsr_ext_info;
|
|
iio_dev->channels = ext_settings->ext_channels;
|
|
iio_dev->num_channels = ext_settings->ext_chan_depth;
|
|
|
|
switch (iio_dev->channels[0].type) {
|
|
case IIO_MAGN:
|
|
iio_dev->name = "lsm6dsr_magn";
|
|
break;
|
|
case IIO_PRESSURE:
|
|
iio_dev->name = "lsm6dsr_press";
|
|
break;
|
|
default:
|
|
iio_dev->name = "lsm6dsr_ext";
|
|
break;
|
|
}
|
|
|
|
sensor = iio_priv(iio_dev);
|
|
sensor->id = id;
|
|
sensor->hw = hw;
|
|
sensor->odr = ext_settings->odr_table.odr_avl[0].hz;
|
|
sensor->gain = ext_settings->fs_table.fs_avl[0].gain;
|
|
sensor->max_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH;
|
|
sensor->watermark = 1;
|
|
sensor->ext_dev_info.ext_dev_i2c_addr = i2c_addr;
|
|
sensor->ext_dev_info.ext_dev_settings = ext_settings;
|
|
sensor->decimator = 0;
|
|
sensor->dec_counter = 0;
|
|
|
|
return iio_dev;
|
|
}
|
|
|
|
static int st_lsm6dsr_shub_init_remote_sensor(struct st_lsm6dsr_sensor *sensor)
|
|
{
|
|
struct st_lsm6dsr_ext_dev_info *ext_info = &sensor->ext_dev_info;
|
|
int err = 0;
|
|
|
|
if (ext_info->ext_dev_settings->bdu_reg.addr)
|
|
err = st_lsm6dsr_shub_write_with_mask(sensor,
|
|
ext_info->ext_dev_settings->bdu_reg.addr,
|
|
ext_info->ext_dev_settings->bdu_reg.mask, 1);
|
|
|
|
if (ext_info->ext_dev_settings->temp_comp_reg.addr)
|
|
err = st_lsm6dsr_shub_write_with_mask(sensor,
|
|
ext_info->ext_dev_settings->temp_comp_reg.addr,
|
|
ext_info->ext_dev_settings->temp_comp_reg.mask, 1);
|
|
|
|
if (ext_info->ext_dev_settings->off_canc_reg.addr)
|
|
err = st_lsm6dsr_shub_write_with_mask(sensor,
|
|
ext_info->ext_dev_settings->off_canc_reg.addr,
|
|
ext_info->ext_dev_settings->off_canc_reg.mask, 1);
|
|
|
|
return err;
|
|
}
|
|
|
|
/**
|
|
* Probe device function [SHUB]
|
|
*
|
|
* @param hw: ST IMU MEMS hw instance.
|
|
* @return 0 if OK, negative for ERROR
|
|
*/
|
|
int st_lsm6dsr_shub_probe(struct st_lsm6dsr_hw *hw)
|
|
{
|
|
const struct st_lsm6dsr_ext_dev_settings *settings;
|
|
struct st_lsm6dsr_sensor *acc_sensor, *sensor;
|
|
u8 config[3], data, num_ext_dev = 0;
|
|
enum st_lsm6dsr_sensor_id id;
|
|
int err, i = 0, j;
|
|
|
|
acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSR_ID_ACC]);
|
|
while (i < ARRAY_SIZE(st_lsm6dsr_ext_dev_table) &&
|
|
num_ext_dev < ST_LSM6DSR_MAX_SLV_NUM) {
|
|
settings = &st_lsm6dsr_ext_dev_table[i];
|
|
|
|
for (j = 0; j < ARRAY_SIZE(settings->i2c_addr); j++) {
|
|
if (!settings->i2c_addr[j])
|
|
continue;
|
|
|
|
/* read wai slave register */
|
|
config[0] = (settings->i2c_addr[j] << 1) | 1;
|
|
config[1] = settings->wai_addr;
|
|
config[2] = 1;
|
|
|
|
err = st_lsm6dsr_shub_write_reg(hw,
|
|
ST_LSM6DSR_REG_SLV0_ADDR,
|
|
config, sizeof(config));
|
|
if (err < 0)
|
|
return err;
|
|
|
|
err = st_lsm6dsr_shub_master_enable(acc_sensor, true);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
st_lsm6dsr_shub_wait_complete(hw);
|
|
|
|
err = st_lsm6dsr_shub_read_reg(hw,
|
|
ST_LSM6DSR_REG_SLV0_OUT_ADDR,
|
|
&data, sizeof(data));
|
|
|
|
st_lsm6dsr_shub_master_enable(acc_sensor, false);
|
|
|
|
if (err < 0)
|
|
return err;
|
|
|
|
if (data != settings->wai_val)
|
|
continue;
|
|
|
|
id = ST_LSM6DSR_ID_EXT0 + num_ext_dev;
|
|
hw->iio_devs[id] = st_lsm6dsr_shub_alloc_iio_dev(hw,
|
|
settings, id,
|
|
settings->i2c_addr[j]);
|
|
if (!hw->iio_devs[id])
|
|
return -ENOMEM;
|
|
|
|
sensor = iio_priv(hw->iio_devs[id]);
|
|
err = st_lsm6dsr_shub_init_remote_sensor(sensor);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
num_ext_dev++;
|
|
hw->ext_data_len += settings->data_len;
|
|
break;
|
|
}
|
|
|
|
i++;
|
|
}
|
|
|
|
if (!num_ext_dev)
|
|
return 0;
|
|
|
|
memset(config, 0, sizeof(config));
|
|
err = st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_SLV0_ADDR,
|
|
config, sizeof(config));
|
|
if (err < 0)
|
|
return err;
|
|
|
|
/* AuxSens = 3 + wr once */
|
|
data = ST_LSM6DSR_REG_WRITE_ONCE_MASK | 3;
|
|
return st_lsm6dsr_shub_write_reg(hw, ST_LSM6DSR_REG_MASTER_CONFIG_ADDR,
|
|
&data, sizeof(data));
|
|
}
|