blob: b485540da89e3c9b8fb8541d6469851e277e68e3 [file] [log] [blame]
/*
* STMicroelectronics st_lsm6dsx sensor driver
*
* The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
* and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
* interface standard output.
* LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
* acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
* +-125/+-245/+-500/+-1000/+-2000 dps
* LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
* allowing dynamic batching of sensor data.
*
* Supported sensors:
* - LSM6DS3:
* - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
* - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
* - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
* - FIFO size: 8KB
*
* - LSM6DS3H/LSM6DSL/LSM6DSM:
* - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
* - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
* - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
* - FIFO size: 4KB
*
* Copyright 2016 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
* Denis Ciocca <denis.ciocca@st.com>
*
* Licensed under the GPL-2.
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
#include <linux/iio/sysfs.h>
#include <linux/pm.h>
#include <linux/platform_data/st_sensors_pdata.h>
#include "st_lsm6dsx.h"
#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
#define ST_LSM6DSX_REG_INT2_ADDR 0x0e
#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
#define ST_LSM6DSX_REG_RESET_ADDR 0x12
#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
#define ST_LSM6DSX_REG_BDU_ADDR 0x12
#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16
#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
#define ST_LSM6DSX_REG_LIR_ADDR 0x58
#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10
#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4)
#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10
#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2)
#define ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR 0x28
#define ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR 0x2a
#define ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR 0x2c
#define ST_LSM6DSX_REG_GYRO_ODR_ADDR 0x11
#define ST_LSM6DSX_REG_GYRO_ODR_MASK GENMASK(7, 4)
#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11
#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2)
#define ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR 0x22
#define ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR 0x24
#define ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR 0x26
#define ST_LSM6DSX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61)
#define ST_LSM6DSX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122)
#define ST_LSM6DSX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244)
#define ST_LSM6DSX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488)
#define ST_LSM6DSX_GYRO_FS_245_GAIN IIO_DEGREE_TO_RAD(8750)
#define ST_LSM6DSX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500)
#define ST_LSM6DSX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000)
#define ST_LSM6DSX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000)
struct st_lsm6dsx_odr {
u16 hz;
u8 val;
};
#define ST_LSM6DSX_ODR_LIST_SIZE 6
struct st_lsm6dsx_odr_table_entry {
struct st_lsm6dsx_reg reg;
struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
};
static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
[ST_LSM6DSX_ID_ACC] = {
.reg = {
.addr = ST_LSM6DSX_REG_ACC_ODR_ADDR,
.mask = ST_LSM6DSX_REG_ACC_ODR_MASK,
},
.odr_avl[0] = { 13, 0x01 },
.odr_avl[1] = { 26, 0x02 },
.odr_avl[2] = { 52, 0x03 },
.odr_avl[3] = { 104, 0x04 },
.odr_avl[4] = { 208, 0x05 },
.odr_avl[5] = { 416, 0x06 },
},
[ST_LSM6DSX_ID_GYRO] = {
.reg = {
.addr = ST_LSM6DSX_REG_GYRO_ODR_ADDR,
.mask = ST_LSM6DSX_REG_GYRO_ODR_MASK,
},
.odr_avl[0] = { 13, 0x01 },
.odr_avl[1] = { 26, 0x02 },
.odr_avl[2] = { 52, 0x03 },
.odr_avl[3] = { 104, 0x04 },
.odr_avl[4] = { 208, 0x05 },
.odr_avl[5] = { 416, 0x06 },
}
};
struct st_lsm6dsx_fs {
u32 gain;
u8 val;
};
#define ST_LSM6DSX_FS_LIST_SIZE 4
struct st_lsm6dsx_fs_table_entry {
struct st_lsm6dsx_reg reg;
struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE];
};
static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = {
[ST_LSM6DSX_ID_ACC] = {
.reg = {
.addr = ST_LSM6DSX_REG_ACC_FS_ADDR,
.mask = ST_LSM6DSX_REG_ACC_FS_MASK,
},
.fs_avl[0] = { ST_LSM6DSX_ACC_FS_2G_GAIN, 0x0 },
.fs_avl[1] = { ST_LSM6DSX_ACC_FS_4G_GAIN, 0x2 },
.fs_avl[2] = { ST_LSM6DSX_ACC_FS_8G_GAIN, 0x3 },
.fs_avl[3] = { ST_LSM6DSX_ACC_FS_16G_GAIN, 0x1 },
},
[ST_LSM6DSX_ID_GYRO] = {
.reg = {
.addr = ST_LSM6DSX_REG_GYRO_FS_ADDR,
.mask = ST_LSM6DSX_REG_GYRO_FS_MASK,
},
.fs_avl[0] = { ST_LSM6DSX_GYRO_FS_245_GAIN, 0x0 },
.fs_avl[1] = { ST_LSM6DSX_GYRO_FS_500_GAIN, 0x1 },
.fs_avl[2] = { ST_LSM6DSX_GYRO_FS_1000_GAIN, 0x2 },
.fs_avl[3] = { ST_LSM6DSX_GYRO_FS_2000_GAIN, 0x3 },
}
};
static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
{
.wai = 0x69,
.max_fifo_size = 8192,
.id = {
[0] = ST_LSM6DS3_ID,
},
},
{
.wai = 0x69,
.max_fifo_size = 4096,
.id = {
[0] = ST_LSM6DS3H_ID,
},
},
{
.wai = 0x6a,
.max_fifo_size = 4096,
.id = {
[0] = ST_LSM6DSL_ID,
[1] = ST_LSM6DSM_ID,
},
},
};
#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
{ \
.type = chan_type, \
.address = addr, \
.modified = 1, \
.channel2 = mod, \
.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
BIT(IIO_CHAN_INFO_SCALE), \
.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
.scan_index = scan_idx, \
.scan_type = { \
.sign = 's', \
.realbits = 16, \
.storagebits = 16, \
.endianness = IIO_LE, \
}, \
}
static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
IIO_MOD_X, 0),
ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
IIO_MOD_Y, 1),
ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
IIO_MOD_Z, 2),
IIO_CHAN_SOFT_TIMESTAMP(3),
};
static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
IIO_MOD_X, 0),
ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
IIO_MOD_Y, 1),
ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
IIO_MOD_Z, 2),
IIO_CHAN_SOFT_TIMESTAMP(3),
};
int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask,
u8 val)
{
u8 data;
int err;
mutex_lock(&hw->lock);
err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
if (err < 0) {
dev_err(hw->dev, "failed to read %02x register\n", addr);
goto out;
}
data = (data & ~mask) | ((val << __ffs(mask)) & mask);
err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
if (err < 0)
dev_err(hw->dev, "failed to write %02x register\n", addr);
out:
mutex_unlock(&hw->lock);
return err;
}
static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
{
int err, i, j;
u8 data;
for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) {
if (id == st_lsm6dsx_sensor_settings[i].id[j])
break;
}
if (j < ST_LSM6DSX_MAX_ID)
break;
}
if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
return -ENODEV;
}
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_WHOAMI_ADDR, sizeof(data),
&data);
if (err < 0) {
dev_err(hw->dev, "failed to read whoami register\n");
return err;
}
if (data != st_lsm6dsx_sensor_settings[i].wai) {
dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
return -ENODEV;
}
hw->settings = &st_lsm6dsx_sensor_settings[i];
return 0;
}
static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
u32 gain)
{
enum st_lsm6dsx_sensor_id id = sensor->id;
int i, err;
u8 val;
for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
if (st_lsm6dsx_fs_table[id].fs_avl[i].gain == gain)
break;
if (i == ST_LSM6DSX_FS_LIST_SIZE)
return -EINVAL;
val = st_lsm6dsx_fs_table[id].fs_avl[i].val;
err = st_lsm6dsx_write_with_mask(sensor->hw,
st_lsm6dsx_fs_table[id].reg.addr,
st_lsm6dsx_fs_table[id].reg.mask,
val);
if (err < 0)
return err;
sensor->gain = gain;
return 0;
}
static int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr,
u8 *val)
{
int i;
for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz == odr)
break;
if (i == ST_LSM6DSX_ODR_LIST_SIZE)
return -EINVAL;
*val = st_lsm6dsx_odr_table[sensor->id].odr_avl[i].val;
sensor->odr = odr;
return 0;
}
static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
{
enum st_lsm6dsx_sensor_id id = sensor->id;
int err;
u8 val;
err = st_lsm6dsx_check_odr(sensor, odr, &val);
if (err < 0)
return err;
return st_lsm6dsx_write_with_mask(sensor->hw,
st_lsm6dsx_odr_table[id].reg.addr,
st_lsm6dsx_odr_table[id].reg.mask,
val);
}
int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
{
int err;
err = st_lsm6dsx_set_odr(sensor, sensor->odr);
if (err < 0)
return err;
sensor->hw->enable_mask |= BIT(sensor->id);
return 0;
}
int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor)
{
enum st_lsm6dsx_sensor_id id = sensor->id;
int err;
err = st_lsm6dsx_write_with_mask(sensor->hw,
st_lsm6dsx_odr_table[id].reg.addr,
st_lsm6dsx_odr_table[id].reg.mask, 0);
if (err < 0)
return err;
sensor->hw->enable_mask &= ~BIT(id);
return 0;
}
static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
u8 addr, int *val)
{
int err, delay;
__le16 data;
err = st_lsm6dsx_sensor_enable(sensor);
if (err < 0)
return err;
delay = 1000000 / sensor->odr;
usleep_range(delay, 2 * delay);
err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data),
(u8 *)&data);
if (err < 0)
return err;
st_lsm6dsx_sensor_disable(sensor);
*val = (s16)data;
return IIO_VAL_INT;
}
static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev,
struct iio_chan_spec const *ch,
int *val, int *val2, long mask)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
int ret;
switch (mask) {
case IIO_CHAN_INFO_RAW:
ret = iio_device_claim_direct_mode(iio_dev);
if (ret)
break;
ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
iio_device_release_direct_mode(iio_dev);
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;
}
static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
struct iio_chan_spec const *chan,
int val, int val2, long mask)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
int err;
err = iio_device_claim_direct_mode(iio_dev);
if (err)
return err;
switch (mask) {
case IIO_CHAN_INFO_SCALE:
err = st_lsm6dsx_set_full_scale(sensor, val2);
break;
case IIO_CHAN_INFO_SAMP_FREQ: {
u8 data;
err = st_lsm6dsx_check_odr(sensor, val, &data);
break;
}
default:
err = -EINVAL;
break;
}
iio_device_release_direct_mode(iio_dev);
return err;
}
static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;
int err, max_fifo_len;
max_fifo_len = hw->settings->max_fifo_size / ST_LSM6DSX_SAMPLE_SIZE;
if (val < 1 || val > max_fifo_len)
return -EINVAL;
err = st_lsm6dsx_update_watermark(sensor, val);
if (err < 0)
return err;
sensor->watermark = val;
return 0;
}
static ssize_t
st_lsm6dsx_sysfs_sampling_frequency_avail(struct device *dev,
struct device_attribute *attr,
char *buf)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
enum st_lsm6dsx_sensor_id id = sensor->id;
int i, len = 0;
for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
st_lsm6dsx_odr_table[id].odr_avl[i].hz);
buf[len - 1] = '\n';
return len;
}
static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *dev,
struct device_attribute *attr,
char *buf)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
enum st_lsm6dsx_sensor_id id = sensor->id;
int i, len = 0;
for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
st_lsm6dsx_fs_table[id].fs_avl[i].gain);
buf[len - 1] = '\n';
return len;
}
static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avail);
static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
st_lsm6dsx_sysfs_scale_avail, NULL, 0);
static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
st_lsm6dsx_sysfs_scale_avail, NULL, 0);
static struct attribute *st_lsm6dsx_acc_attributes[] = {
&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
NULL,
};
static const struct attribute_group st_lsm6dsx_acc_attribute_group = {
.attrs = st_lsm6dsx_acc_attributes,
};
static const struct iio_info st_lsm6dsx_acc_info = {
.driver_module = THIS_MODULE,
.attrs = &st_lsm6dsx_acc_attribute_group,
.read_raw = st_lsm6dsx_read_raw,
.write_raw = st_lsm6dsx_write_raw,
.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
};
static struct attribute *st_lsm6dsx_gyro_attributes[] = {
&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
&iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
NULL,
};
static const struct attribute_group st_lsm6dsx_gyro_attribute_group = {
.attrs = st_lsm6dsx_gyro_attributes,
};
static const struct iio_info st_lsm6dsx_gyro_info = {
.driver_module = THIS_MODULE,
.attrs = &st_lsm6dsx_gyro_attribute_group,
.read_raw = st_lsm6dsx_read_raw,
.write_raw = st_lsm6dsx_write_raw,
.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
};
static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
static int st_lsm6dsx_of_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin)
{
struct device_node *np = hw->dev->of_node;
if (!np)
return -EINVAL;
return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
}
static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
{
int err = 0, drdy_pin;
if (st_lsm6dsx_of_get_drdy_pin(hw, &drdy_pin) < 0) {
struct st_sensors_platform_data *pdata;
struct device *dev = hw->dev;
pdata = (struct st_sensors_platform_data *)dev->platform_data;
drdy_pin = pdata ? pdata->drdy_int_pin : 1;
}
switch (drdy_pin) {
case 1:
*drdy_reg = ST_LSM6DSX_REG_INT1_ADDR;
break;
case 2:
*drdy_reg = ST_LSM6DSX_REG_INT2_ADDR;
break;
default:
dev_err(hw->dev, "unsupported data ready pin\n");
err = -EINVAL;
break;
}
return err;
}
static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
{
u8 data, drdy_int_reg;
int err;
data = ST_LSM6DSX_REG_RESET_MASK;
err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_RESET_ADDR, sizeof(data),
&data);
if (err < 0)
return err;
msleep(200);
/* latch interrupts */
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_LIR_ADDR,
ST_LSM6DSX_REG_LIR_MASK, 1);
if (err < 0)
return err;
/* enable Block Data Update */
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_BDU_ADDR,
ST_LSM6DSX_REG_BDU_MASK, 1);
if (err < 0)
return err;
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_ROUNDING_ADDR,
ST_LSM6DSX_REG_ROUNDING_MASK, 1);
if (err < 0)
return err;
/* enable FIFO watermak interrupt */
err = st_lsm6dsx_get_drdy_reg(hw, &drdy_int_reg);
if (err < 0)
return err;
return st_lsm6dsx_write_with_mask(hw, drdy_int_reg,
ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, 1);
}
static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
enum st_lsm6dsx_sensor_id id,
const char *name)
{
struct st_lsm6dsx_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 = st_lsm6dsx_available_scan_masks;
sensor = iio_priv(iio_dev);
sensor->id = id;
sensor->hw = hw;
sensor->odr = st_lsm6dsx_odr_table[id].odr_avl[0].hz;
sensor->gain = st_lsm6dsx_fs_table[id].fs_avl[0].gain;
sensor->watermark = 1;
switch (id) {
case ST_LSM6DSX_ID_ACC:
iio_dev->channels = st_lsm6dsx_acc_channels;
iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels);
iio_dev->info = &st_lsm6dsx_acc_info;
sensor->decimator_mask = ST_LSM6DSX_REG_ACC_DEC_MASK;
scnprintf(sensor->name, sizeof(sensor->name), "%s_accel",
name);
break;
case ST_LSM6DSX_ID_GYRO:
iio_dev->channels = st_lsm6dsx_gyro_channels;
iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels);
iio_dev->info = &st_lsm6dsx_gyro_info;
sensor->decimator_mask = ST_LSM6DSX_REG_GYRO_DEC_MASK;
scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro",
name);
break;
default:
return NULL;
}
iio_dev->name = sensor->name;
return iio_dev;
}
int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
const struct st_lsm6dsx_transfer_function *tf_ops)
{
struct st_lsm6dsx_hw *hw;
int i, err;
hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
if (!hw)
return -ENOMEM;
dev_set_drvdata(dev, (void *)hw);
mutex_init(&hw->lock);
mutex_init(&hw->fifo_lock);
hw->dev = dev;
hw->irq = irq;
hw->tf = tf_ops;
err = st_lsm6dsx_check_whoami(hw, hw_id);
if (err < 0)
return err;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name);
if (!hw->iio_devs[i])
return -ENOMEM;
}
err = st_lsm6dsx_init_device(hw);
if (err < 0)
return err;
if (hw->irq > 0) {
err = st_lsm6dsx_fifo_setup(hw);
if (err < 0)
return err;
}
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
if (err)
return err;
}
return 0;
}
EXPORT_SYMBOL(st_lsm6dsx_probe);
static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
{
struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
struct st_lsm6dsx_sensor *sensor;
int i, err = 0;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
sensor = iio_priv(hw->iio_devs[i]);
if (!(hw->enable_mask & BIT(sensor->id)))
continue;
err = st_lsm6dsx_write_with_mask(hw,
st_lsm6dsx_odr_table[sensor->id].reg.addr,
st_lsm6dsx_odr_table[sensor->id].reg.mask, 0);
if (err < 0)
return err;
}
if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
err = st_lsm6dsx_flush_fifo(hw);
return err;
}
static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
{
struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
struct st_lsm6dsx_sensor *sensor;
int i, err = 0;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
sensor = iio_priv(hw->iio_devs[i]);
if (!(hw->enable_mask & BIT(sensor->id)))
continue;
err = st_lsm6dsx_set_odr(sensor, sensor->odr);
if (err < 0)
return err;
}
if (hw->enable_mask)
err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
return err;
}
const struct dev_pm_ops st_lsm6dsx_pm_ops = {
SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsx_suspend, st_lsm6dsx_resume)
};
EXPORT_SYMBOL(st_lsm6dsx_pm_ops);
MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver");
MODULE_LICENSE("GPL v2");