Release 4.11 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c

/*
 * 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
 *
 * - 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/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_LSM6DS3_WHOAMI			0x69

#define ST_LSM6DSM_WHOAMI			0x6a


#define ST_LSM6DS3_MAX_FIFO_SIZE		8192

#define ST_LSM6DSM_MAX_FIFO_SIZE		4096


#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 = ST_LSM6DS3_WHOAMI,
		.max_fifo_size = ST_LSM6DS3_MAX_FIFO_SIZE,
		.id = ST_LSM6DS3_ID,
        },
	{
		.wai = ST_LSM6DSM_WHOAMI,
		.max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
		.id = 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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
153
100.00%
1
100.00%
Total
153
100.00%
1
100.00%



static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
{
	int err, i;
	u8 data;

	for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
		if (id == st_lsm6dsx_sensor_settings[i].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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
162
100.00%
1
100.00%
Total
162
100.00%
1
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
131
100.00%
1
100.00%
Total
131
100.00%
1
100.00%



static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
{
	enum st_lsm6dsx_sensor_id id = sensor->id;
	int i, err;
	u8 val;

	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
		if (st_lsm6dsx_odr_table[id].odr_avl[i].hz == odr)
			break;

	if (i == ST_LSM6DSX_ODR_LIST_SIZE)
		return -EINVAL;

	val = st_lsm6dsx_odr_table[id].odr_avl[i].val;
	err = st_lsm6dsx_write_with_mask(sensor->hw,
					 st_lsm6dsx_odr_table[id].reg.addr,
					 st_lsm6dsx_odr_table[id].reg.mask,
					 val);
	if (err < 0)
		return err;

	sensor->odr = odr;

	return 0;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
131
100.00%
1
100.00%
Total
131
100.00%
1
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
49
100.00%
1
100.00%
Total
49
100.00%
1
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
74
100.00%
1
100.00%
Total
74
100.00%
1
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
115
100.00%
1
100.00%
Total
115
100.00%
1
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
126
100.00%
1
100.00%
Total
126
100.00%
1
100.00%



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:
		err = st_lsm6dsx_set_odr(sensor, val);
		break;
	default:
		err = -EINVAL;
		break;
	}

	iio_device_release_direct_mode(iio_dev);

	return err;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
100
100.00%
1
100.00%
Total
100
100.00%
1
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
90
100.00%
1
100.00%
Total
90
100.00%
1
100.00%


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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
100
100.00%
1
100.00%
Total
100
100.00%
1
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
100
100.00%
1
100.00%
Total
100
100.00%
1
100.00%

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;
	int err;

	if (!np)
		return -EINVAL;

	err = of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
	if (err == -ENODATA) {
		/* if the property has not been specified use default value */
		*drdy_pin = 1;
		err = 0;
	}

	return err;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
71
100.00%
2
100.00%
Total
71
100.00%
2
100.00%



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;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
114
100.00%
2
100.00%
Total
114
100.00%
2
100.00%



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);
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
160
100.00%
2
100.00%
Total
160
100.00%
2
100.00%



static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
					       enum st_lsm6dsx_sensor_id id)
{
	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->name = "lsm6dsx_accel";
		iio_dev->info = &st_lsm6dsx_acc_info;

		sensor->decimator_mask = ST_LSM6DSX_REG_ACC_DEC_MASK;
		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->name = "lsm6dsx_gyro";
		iio_dev->info = &st_lsm6dsx_gyro_info;

		sensor->decimator_mask = ST_LSM6DSX_REG_GYRO_DEC_MASK;
		break;
	default:
		return NULL;
	}

	return iio_dev;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
218
100.00%
1
100.00%
Total
218
100.00%
1
100.00%



int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
		     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);
		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;
}
Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
245
100.00%
1
100.00%
Total
245
100.00%
1
100.00%


EXPORT_SYMBOL(st_lsm6dsx_probe);

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");
Overall Contributors
Person
Tokens
Prop
Commits
CommitProp
Lorenzo Bianconi
3100
100.00%
3
100.00%
Total
3100
100.00%
3
100.00%
Directory: drivers/iio/imu/st_lsm6dsx

Information contained on this website is for historical information purposes only and does not indicate or represent copyright ownership.