[deliverable/linux.git] / drivers / iio / common / st_sensors / st_sensors_core.c

/*
 * STMicroelectronics sensors core library driver
 *
 * Copyright 2012-2013 STMicroelectronics Inc.
 *
 * Denis Ciocca <denis.ciocca@st.com>
 *
 * Licensed under the GPL-2.
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
#include <asm/unaligned.h>

#include <linux/iio/common/st_sensors.h>


#define ST_SENSORS_WAI_ADDRESS		0x0f

static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
{
	return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
}

static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
						u8 reg_addr, u8 mask, u8 data)
{
	int err;
	u8 new_data;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data);
	if (err < 0)
		goto st_sensors_write_data_with_mask_error;

	new_data = ((new_data & (~mask)) | ((data << __ffs(mask)) & mask));
	err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data);

st_sensors_write_data_with_mask_error:
	return err;
}

static int st_sensors_match_odr(struct st_sensors *sensor,
			unsigned int odr, struct st_sensor_odr_avl *odr_out)
{
	int i, ret = -EINVAL;

	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
		if (sensor->odr.odr_avl[i].hz == 0)
			goto st_sensors_match_odr_error;

		if (sensor->odr.odr_avl[i].hz == odr) {
			odr_out->hz = sensor->odr.odr_avl[i].hz;
			odr_out->value = sensor->odr.odr_avl[i].value;
			ret = 0;
			break;
		}
	}

st_sensors_match_odr_error:
	return ret;
}

int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
{
	int err;
	struct st_sensor_odr_avl odr_out = {0, 0};
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	err = st_sensors_match_odr(sdata->sensor, odr, &odr_out);
	if (err < 0)
		goto st_sensors_match_odr_error;

	if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
			(sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
		if (sdata->enabled == true) {
			err = st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor->odr.addr,
				sdata->sensor->odr.mask,
				odr_out.value);
		} else {
			err = 0;
		}
	} else {
		err = st_sensors_write_data_with_mask(indio_dev,
			sdata->sensor->odr.addr, sdata->sensor->odr.mask,
			odr_out.value);
	}
	if (err >= 0)
		sdata->odr = odr_out.hz;

st_sensors_match_odr_error:
	return err;
}
EXPORT_SYMBOL(st_sensors_set_odr);

static int st_sensors_match_fs(struct st_sensors *sensor,
					unsigned int fs, int *index_fs_avl)
{
	int i, ret = -EINVAL;

	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
		if (sensor->fs.fs_avl[i].num == 0)
			goto st_sensors_match_odr_error;

		if (sensor->fs.fs_avl[i].num == fs) {
			*index_fs_avl = i;
			ret = 0;
			break;
		}
	}

st_sensors_match_odr_error:
	return ret;
}

static int st_sensors_set_fullscale(struct iio_dev *indio_dev,
								unsigned int fs)
{
	int err, i = 0;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	err = st_sensors_match_fs(sdata->sensor, fs, &i);
	if (err < 0)
		goto st_accel_set_fullscale_error;

	err = st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor->fs.addr,
				sdata->sensor->fs.mask,
				sdata->sensor->fs.fs_avl[i].value);
	if (err < 0)
		goto st_accel_set_fullscale_error;

	sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
						&sdata->sensor->fs.fs_avl[i];
	return err;

st_accel_set_fullscale_error:
	dev_err(&indio_dev->dev, "failed to set new fullscale.\n");
	return err;
}

int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
{
	u8 tmp_value;
	int err = -EINVAL;
	bool found = false;
	struct st_sensor_odr_avl odr_out = {0, 0};
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	if (enable) {
		tmp_value = sdata->sensor->pw.value_on;
		if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
			(sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
			err = st_sensors_match_odr(sdata->sensor,
							sdata->odr, &odr_out);
			if (err < 0)
				goto set_enable_error;
			tmp_value = odr_out.value;
			found = true;
		}
		err = st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor->pw.addr,
				sdata->sensor->pw.mask, tmp_value);
		if (err < 0)
			goto set_enable_error;

		sdata->enabled = true;

		if (found)
			sdata->odr = odr_out.hz;
	} else {
		err = st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor->pw.addr,
				sdata->sensor->pw.mask,
				sdata->sensor->pw.value_off);
		if (err < 0)
			goto set_enable_error;

		sdata->enabled = false;
	}

set_enable_error:
	return err;
}
EXPORT_SYMBOL(st_sensors_set_enable);

int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
{
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	return st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor->enable_axis.addr,
				sdata->sensor->enable_axis.mask, axis_enable);
}
EXPORT_SYMBOL(st_sensors_set_axis_enable);

int st_sensors_init_sensor(struct iio_dev *indio_dev,
					struct st_sensors_platform_data *pdata)
{
	int err;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	mutex_init(&sdata->tb.buf_lock);

	switch (pdata->drdy_int_pin) {
	case 1:
		if (sdata->sensor->drdy_irq.mask_int1 == 0) {
			dev_err(&indio_dev->dev,
					"DRDY on INT1 not available.\n");
			err = -EINVAL;
			goto init_error;
		}
		sdata->drdy_int_pin = 1;
		break;
	case 2:
		if (sdata->sensor->drdy_irq.mask_int2 == 0) {
			dev_err(&indio_dev->dev,
					"DRDY on INT2 not available.\n");
			err = -EINVAL;
			goto init_error;
		}
		sdata->drdy_int_pin = 2;
		break;
	default:
		dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
		err = -EINVAL;
		goto init_error;
	}

	err = st_sensors_set_enable(indio_dev, false);
	if (err < 0)
		goto init_error;

	err = st_sensors_set_fullscale(indio_dev,
						sdata->current_fullscale->num);
	if (err < 0)
		goto init_error;

	err = st_sensors_set_odr(indio_dev, sdata->odr);
	if (err < 0)
		goto init_error;

	/* set BDU */
	err = st_sensors_write_data_with_mask(indio_dev,
			sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
	if (err < 0)
		goto init_error;

	err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);

init_error:
	return err;
}
EXPORT_SYMBOL(st_sensors_init_sensor);

int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
{
	int err;
	u8 drdy_mask;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	/* Enable/Disable the interrupt generator 1. */
	if (sdata->sensor->drdy_irq.ig1.en_addr > 0) {
		err = st_sensors_write_data_with_mask(indio_dev,
			sdata->sensor->drdy_irq.ig1.en_addr,
			sdata->sensor->drdy_irq.ig1.en_mask, (int)enable);
		if (err < 0)
			goto st_accel_set_dataready_irq_error;
	}

	if (sdata->drdy_int_pin == 1)
		drdy_mask = sdata->sensor->drdy_irq.mask_int1;
	else
		drdy_mask = sdata->sensor->drdy_irq.mask_int2;

	/* Enable/Disable the interrupt generator for data ready. */
	err = st_sensors_write_data_with_mask(indio_dev,
			sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable);

st_accel_set_dataready_irq_error:
	return err;
}
EXPORT_SYMBOL(st_sensors_set_dataready_irq);

int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
{
	int err = -EINVAL, i;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
		if ((sdata->sensor->fs.fs_avl[i].gain == scale) &&
				(sdata->sensor->fs.fs_avl[i].gain != 0)) {
			err = 0;
			break;
		}
	}
	if (err < 0)
		goto st_sensors_match_scale_error;

	err = st_sensors_set_fullscale(indio_dev,
					sdata->sensor->fs.fs_avl[i].num);

st_sensors_match_scale_error:
	return err;
}
EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);

static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
				struct iio_chan_spec const *ch, int *data)
{
	int err;
	u8 *outdata;
	struct st_sensor_data *sdata = iio_priv(indio_dev);
	unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;

	outdata = kmalloc(byte_for_channel, GFP_KERNEL);
	if (!outdata) {
		err = -EINVAL;
		goto st_sensors_read_axis_data_error;
	}

	err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
				ch->address, byte_for_channel,
				outdata, sdata->multiread_bit);
	if (err < 0)
		goto st_sensors_free_memory;

	if (byte_for_channel == 2)
		*data = (s16)get_unaligned_le16(outdata);
	else if (byte_for_channel == 3)
		*data = (s32)st_sensors_get_unaligned_le24(outdata);

st_sensors_free_memory:
	kfree(outdata);
st_sensors_read_axis_data_error:
	return err;
}

int st_sensors_read_info_raw(struct iio_dev *indio_dev,
				struct iio_chan_spec const *ch, int *val)
{
	int err;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	mutex_lock(&indio_dev->mlock);
	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
		err = -EBUSY;
		goto read_error;
	} else {
		err = st_sensors_set_enable(indio_dev, true);
		if (err < 0)
			goto read_error;

		msleep((sdata->sensor->bootime * 1000) / sdata->odr);
		err = st_sensors_read_axis_data(indio_dev, ch, val);
		if (err < 0)
			goto read_error;

		*val = *val >> ch->scan_type.shift;

		err = st_sensors_set_enable(indio_dev, false);
	}
	mutex_unlock(&indio_dev->mlock);

	return err;

read_error:
	mutex_unlock(&indio_dev->mlock);
	return err;
}
EXPORT_SYMBOL(st_sensors_read_info_raw);

int st_sensors_check_device_support(struct iio_dev *indio_dev,
			int num_sensors_list, const struct st_sensors *sensors)
{
	u8 wai;
	int i, n, err;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
					ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
	if (err < 0) {
		dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
		goto read_wai_error;
	}

	for (i = 0; i < num_sensors_list; i++) {
		if (sensors[i].wai == wai)
			break;
	}
	if (i == num_sensors_list)
		goto device_not_supported;

	for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) {
		if (strcmp(indio_dev->name,
				&sensors[i].sensors_supported[n][0]) == 0)
			break;
	}
	if (n == ARRAY_SIZE(sensors[i].sensors_supported)) {
		dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
		goto sensor_name_mismatch;
	}

	sdata->sensor = (struct st_sensors *)&sensors[i];

	return i;

device_not_supported:
	dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
sensor_name_mismatch:
	err = -ENODEV;
read_wai_error:
	return err;
}
EXPORT_SYMBOL(st_sensors_check_device_support);

ssize_t st_sensors_sysfs_get_sampling_frequency(struct device *dev,
				struct device_attribute *attr, char *buf)
{
	struct st_sensor_data *adata = iio_priv(dev_get_drvdata(dev));

	return sprintf(buf, "%d\n", adata->odr);
}
EXPORT_SYMBOL(st_sensors_sysfs_get_sampling_frequency);

ssize_t st_sensors_sysfs_set_sampling_frequency(struct device *dev,
		struct device_attribute *attr, const char *buf, size_t size)
{
	int err;
	unsigned int odr;
	struct iio_dev *indio_dev = dev_get_drvdata(dev);

	err = kstrtoint(buf, 10, &odr);
	if (err < 0)
		goto conversion_error;

	mutex_lock(&indio_dev->mlock);
	err = st_sensors_set_odr(indio_dev, odr);
	mutex_unlock(&indio_dev->mlock);

conversion_error:
	return err < 0 ? err : size;
}
EXPORT_SYMBOL(st_sensors_sysfs_set_sampling_frequency);

ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
				struct device_attribute *attr, char *buf)
{
	int i, len = 0;
	struct iio_dev *indio_dev = dev_get_drvdata(dev);
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	mutex_lock(&indio_dev->mlock);
	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
		if (sdata->sensor->odr.odr_avl[i].hz == 0)
			break;

		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
					sdata->sensor->odr.odr_avl[i].hz);
	}
	mutex_unlock(&indio_dev->mlock);
	buf[len - 1] = '\n';

	return len;
}
EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);

ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
				struct device_attribute *attr, char *buf)
{
	int i, len = 0;
	struct iio_dev *indio_dev = dev_get_drvdata(dev);
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	mutex_lock(&indio_dev->mlock);
	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
		if (sdata->sensor->fs.fs_avl[i].num == 0)
			break;

		len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
					sdata->sensor->fs.fs_avl[i].gain);
	}
	mutex_unlock(&indio_dev->mlock);
	buf[len - 1] = '\n';

	return len;
}
EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);

MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
MODULE_LICENSE("GPL v2");
Commit	Line	Data
23491b51 DC	1	/*
	2	* STMicroelectronics sensors core library driver
	3	*
	4	* Copyright 2012-2013 STMicroelectronics Inc.
	5	*
	6	* Denis Ciocca <denis.ciocca@st.com>
	7	*
	8	* Licensed under the GPL-2.
	9	*/
	10
	11	#include <linux/kernel.h>
	12	#include <linux/module.h>
	13	#include <linux/slab.h>
	14	#include <linux/delay.h>
	15	#include <linux/iio/iio.h>
	16	#include <asm/unaligned.h>
	17
	18	#include <linux/iio/common/st_sensors.h>
	19
	20
	21	#define ST_SENSORS_WAI_ADDRESS 0x0f
	22
607a568a DC	23	static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
607a568a DC	24	{
23cde4d6	25	return (s32)((p[0] \| p[1] << 8 \| p[2] << 16) << 8) >> 8;
607a568a DC	26	}
607a568a DC	27
23491b51 DC	28	static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
	29	u8 reg_addr, u8 mask, u8 data)
	30	{
	31	int err;
	32	u8 new_data;
	33	struct st_sensor_data *sdata = iio_priv(indio_dev);
	34
	35	err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data);
	36	if (err < 0)
	37	goto st_sensors_write_data_with_mask_error;
	38
	39	new_data = ((new_data & (~mask)) \| ((data << __ffs(mask)) & mask));
	40	err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data);
	41
	42	st_sensors_write_data_with_mask_error:
	43	return err;
	44	}
	45
23491b51 DC	46	static int st_sensors_match_odr(struct st_sensors *sensor,
	47	unsigned int odr, struct st_sensor_odr_avl *odr_out)
	48	{
	49	int i, ret = -EINVAL;
	50
	51	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
	52	if (sensor->odr.odr_avl[i].hz == 0)
	53	goto st_sensors_match_odr_error;
	54
	55	if (sensor->odr.odr_avl[i].hz == odr) {
	56	odr_out->hz = sensor->odr.odr_avl[i].hz;
	57	odr_out->value = sensor->odr.odr_avl[i].value;
	58	ret = 0;
	59	break;
	60	}
	61	}
	62
	63	st_sensors_match_odr_error:
	64	return ret;
	65	}
	66
	67	int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
	68	{
	69	int err;
852afe99	70	struct st_sensor_odr_avl odr_out = {0, 0};
23491b51 DC	71	struct st_sensor_data *sdata = iio_priv(indio_dev);
	72
	73	err = st_sensors_match_odr(sdata->sensor, odr, &odr_out);
	74	if (err < 0)
	75	goto st_sensors_match_odr_error;
	76
	77	if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
	78	(sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
	79	if (sdata->enabled == true) {
	80	err = st_sensors_write_data_with_mask(indio_dev,
	81	sdata->sensor->odr.addr,
	82	sdata->sensor->odr.mask,
	83	odr_out.value);
	84	} else {
	85	err = 0;
	86	}
	87	} else {
	88	err = st_sensors_write_data_with_mask(indio_dev,
	89	sdata->sensor->odr.addr, sdata->sensor->odr.mask,
	90	odr_out.value);
	91	}
	92	if (err >= 0)
	93	sdata->odr = odr_out.hz;
	94
	95	st_sensors_match_odr_error:
	96	return err;
	97	}
	98	EXPORT_SYMBOL(st_sensors_set_odr);
	99
	100	static int st_sensors_match_fs(struct st_sensors *sensor,
	101	unsigned int fs, int *index_fs_avl)
	102	{
	103	int i, ret = -EINVAL;
	104
	105	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
	106	if (sensor->fs.fs_avl[i].num == 0)
	107	goto st_sensors_match_odr_error;
	108
	109	if (sensor->fs.fs_avl[i].num == fs) {
	110	*index_fs_avl = i;
	111	ret = 0;
	112	break;
	113	}
	114	}
	115
	116	st_sensors_match_odr_error:
	117	return ret;
	118	}
	119
607a568a	120	static int st_sensors_set_fullscale(struct iio_dev *indio_dev,
23cde4d6	121	unsigned int fs)
23491b51	122	{
852afe99	123	int err, i = 0;
23491b51 DC	124	struct st_sensor_data *sdata = iio_priv(indio_dev);
	125
	126	err = st_sensors_match_fs(sdata->sensor, fs, &i);
	127	if (err < 0)
	128	goto st_accel_set_fullscale_error;
	129
	130	err = st_sensors_write_data_with_mask(indio_dev,
	131	sdata->sensor->fs.addr,
	132	sdata->sensor->fs.mask,
	133	sdata->sensor->fs.fs_avl[i].value);
	134	if (err < 0)
	135	goto st_accel_set_fullscale_error;
	136
	137	sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
	138	&sdata->sensor->fs.fs_avl[i];
	139	return err;
	140
	141	st_accel_set_fullscale_error:
	142	dev_err(&indio_dev->dev, "failed to set new fullscale.\n");
	143	return err;
	144	}
	145
	146	int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
	147	{
23491b51 DC	148	u8 tmp_value;
23491b51 DC	149	int err = -EINVAL;
852afe99 DC	150	bool found = false;
852afe99 DC	151	struct st_sensor_odr_avl odr_out = {0, 0};
23491b51 DC	152	struct st_sensor_data *sdata = iio_priv(indio_dev);
	153
	154	if (enable) {
23491b51 DC	155	tmp_value = sdata->sensor->pw.value_on;
	156	if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
	157	(sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
	158	err = st_sensors_match_odr(sdata->sensor,
	159	sdata->odr, &odr_out);
	160	if (err < 0)
	161	goto set_enable_error;
	162	tmp_value = odr_out.value;
	163	found = true;
	164	}
	165	err = st_sensors_write_data_with_mask(indio_dev,
	166	sdata->sensor->pw.addr,
	167	sdata->sensor->pw.mask, tmp_value);
	168	if (err < 0)
	169	goto set_enable_error;
	170
	171	sdata->enabled = true;
	172
	173	if (found)
	174	sdata->odr = odr_out.hz;
	175	} else {
	176	err = st_sensors_write_data_with_mask(indio_dev,
	177	sdata->sensor->pw.addr,
	178	sdata->sensor->pw.mask,
	179	sdata->sensor->pw.value_off);
	180	if (err < 0)
	181	goto set_enable_error;
	182
	183	sdata->enabled = false;
	184	}
	185
	186	set_enable_error:
	187	return err;
	188	}
	189	EXPORT_SYMBOL(st_sensors_set_enable);
	190
	191	int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
	192	{
	193	struct st_sensor_data *sdata = iio_priv(indio_dev);
	194
	195	return st_sensors_write_data_with_mask(indio_dev,
	196	sdata->sensor->enable_axis.addr,
	197	sdata->sensor->enable_axis.mask, axis_enable);
	198	}
	199	EXPORT_SYMBOL(st_sensors_set_axis_enable);
	200
23cde4d6 DC	201	int st_sensors_init_sensor(struct iio_dev *indio_dev,
23cde4d6 DC	202	struct st_sensors_platform_data *pdata)
23491b51 DC	203	{
	204	int err;
	205	struct st_sensor_data *sdata = iio_priv(indio_dev);
	206
	207	mutex_init(&sdata->tb.buf_lock);
	208
23cde4d6 DC	209	switch (pdata->drdy_int_pin) {
	210	case 1:
	211	if (sdata->sensor->drdy_irq.mask_int1 == 0) {
	212	dev_err(&indio_dev->dev,
	213	"DRDY on INT1 not available.\n");
	214	err = -EINVAL;
	215	goto init_error;
	216	}
	217	sdata->drdy_int_pin = 1;
	218	break;
	219	case 2:
	220	if (sdata->sensor->drdy_irq.mask_int2 == 0) {
	221	dev_err(&indio_dev->dev,
	222	"DRDY on INT2 not available.\n");
	223	err = -EINVAL;
	224	goto init_error;
	225	}
	226	sdata->drdy_int_pin = 2;
	227	break;
	228	default:
	229	dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
	230	err = -EINVAL;
	231	goto init_error;
	232	}
	233
23491b51 DC	234	err = st_sensors_set_enable(indio_dev, false);
	235	if (err < 0)
	236	goto init_error;
	237
	238	err = st_sensors_set_fullscale(indio_dev,
	239	sdata->current_fullscale->num);
	240	if (err < 0)
	241	goto init_error;
	242
	243	err = st_sensors_set_odr(indio_dev, sdata->odr);
	244	if (err < 0)
	245	goto init_error;
	246
	247	/* set BDU */
	248	err = st_sensors_write_data_with_mask(indio_dev,
	249	sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
	250	if (err < 0)
	251	goto init_error;
	252
	253	err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
	254
	255	init_error:
	256	return err;
	257	}
	258	EXPORT_SYMBOL(st_sensors_init_sensor);
	259
	260	int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
	261	{
	262	int err;
23cde4d6	263	u8 drdy_mask;
23491b51 DC	264	struct st_sensor_data *sdata = iio_priv(indio_dev);
	265
	266	/* Enable/Disable the interrupt generator 1. */
	267	if (sdata->sensor->drdy_irq.ig1.en_addr > 0) {
	268	err = st_sensors_write_data_with_mask(indio_dev,
	269	sdata->sensor->drdy_irq.ig1.en_addr,
	270	sdata->sensor->drdy_irq.ig1.en_mask, (int)enable);
	271	if (err < 0)
	272	goto st_accel_set_dataready_irq_error;
	273	}
	274
23cde4d6 DC	275	if (sdata->drdy_int_pin == 1)
	276	drdy_mask = sdata->sensor->drdy_irq.mask_int1;
	277	else
	278	drdy_mask = sdata->sensor->drdy_irq.mask_int2;
	279
23491b51 DC	280	/* Enable/Disable the interrupt generator for data ready. */
23491b51 DC	281	err = st_sensors_write_data_with_mask(indio_dev,
23cde4d6	282	sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable);
23491b51 DC	283
	284	st_accel_set_dataready_irq_error:
	285	return err;
	286	}
	287	EXPORT_SYMBOL(st_sensors_set_dataready_irq);
	288
	289	int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
	290	{
	291	int err = -EINVAL, i;
	292	struct st_sensor_data *sdata = iio_priv(indio_dev);
	293
	294	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
	295	if ((sdata->sensor->fs.fs_avl[i].gain == scale) &&
	296	(sdata->sensor->fs.fs_avl[i].gain != 0)) {
	297	err = 0;
	298	break;
	299	}
	300	}
	301	if (err < 0)
	302	goto st_sensors_match_scale_error;
	303
	304	err = st_sensors_set_fullscale(indio_dev,
	305	sdata->sensor->fs.fs_avl[i].num);
	306
	307	st_sensors_match_scale_error:
	308	return err;
	309	}
	310	EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
	311
	312	static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
607a568a	313	struct iio_chan_spec const ch, int data)
23491b51 DC	314	{
23491b51 DC	315	int err;
607a568a	316	u8 *outdata;
23491b51	317	struct st_sensor_data *sdata = iio_priv(indio_dev);
607a568a DC	318	unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
	319
	320	outdata = kmalloc(byte_for_channel, GFP_KERNEL);
	321	if (!outdata) {
	322	err = -EINVAL;
	323	goto st_sensors_read_axis_data_error;
	324	}
23491b51 DC	325
23491b51 DC	326	err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
607a568a	327	ch->address, byte_for_channel,
23491b51 DC	328	outdata, sdata->multiread_bit);
23491b51 DC	329	if (err < 0)
607a568a	330	goto st_sensors_free_memory;
23491b51	331
607a568a DC	332	if (byte_for_channel == 2)
	333	*data = (s16)get_unaligned_le16(outdata);
	334	else if (byte_for_channel == 3)
	335	*data = (s32)st_sensors_get_unaligned_le24(outdata);
23491b51	336
607a568a DC	337	st_sensors_free_memory:
	338	kfree(outdata);
	339	st_sensors_read_axis_data_error:
23491b51 DC	340	return err;
	341	}
	342
	343	int st_sensors_read_info_raw(struct iio_dev *indio_dev,
	344	struct iio_chan_spec const ch, int val)
	345	{
	346	int err;
	347	struct st_sensor_data *sdata = iio_priv(indio_dev);
	348
	349	mutex_lock(&indio_dev->mlock);
	350	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
	351	err = -EBUSY;
	352	goto read_error;
	353	} else {
	354	err = st_sensors_set_enable(indio_dev, true);
	355	if (err < 0)
	356	goto read_error;
	357
	358	msleep((sdata->sensor->bootime * 1000) / sdata->odr);
607a568a	359	err = st_sensors_read_axis_data(indio_dev, ch, val);
23491b51 DC	360	if (err < 0)
	361	goto read_error;
	362
	363	val = val >> ch->scan_type.shift;
d61a04dc DC	364
d61a04dc DC	365	err = st_sensors_set_enable(indio_dev, false);
23491b51 DC	366	}
	367	mutex_unlock(&indio_dev->mlock);
	368
	369	return err;
	370
	371	read_error:
	372	mutex_unlock(&indio_dev->mlock);
	373	return err;
	374	}
	375	EXPORT_SYMBOL(st_sensors_read_info_raw);
	376
	377	int st_sensors_check_device_support(struct iio_dev *indio_dev,
	378	int num_sensors_list, const struct st_sensors *sensors)
	379	{
	380	u8 wai;
	381	int i, n, err;
	382	struct st_sensor_data *sdata = iio_priv(indio_dev);
	383
	384	err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
	385	ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
	386	if (err < 0) {
	387	dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
	388	goto read_wai_error;
	389	}
	390
	391	for (i = 0; i < num_sensors_list; i++) {
	392	if (sensors[i].wai == wai)
	393	break;
	394	}
	395	if (i == num_sensors_list)
	396	goto device_not_supported;
	397
	398	for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) {
	399	if (strcmp(indio_dev->name,
	400	&sensors[i].sensors_supported[n][0]) == 0)
	401	break;
	402	}
	403	if (n == ARRAY_SIZE(sensors[i].sensors_supported)) {
	404	dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
	405	goto sensor_name_mismatch;
	406	}
	407
	408	sdata->sensor = (struct st_sensors *)&sensors[i];
	409
	410	return i;
	411
	412	device_not_supported:
	413	dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
	414	sensor_name_mismatch:
	415	err = -ENODEV;
	416	read_wai_error:
	417	return err;
	418	}
	419	EXPORT_SYMBOL(st_sensors_check_device_support);
	420
	421	ssize_t st_sensors_sysfs_get_sampling_frequency(struct device *dev,
	422	struct device_attribute attr, char buf)
	423	{
	424	struct st_sensor_data *adata = iio_priv(dev_get_drvdata(dev));
	425
	426	return sprintf(buf, "%d\n", adata->odr);
	427	}
	428	EXPORT_SYMBOL(st_sensors_sysfs_get_sampling_frequency);
	429
430	ssize_t st_sensors_sysfs_set_sampling_frequency(struct device *dev,
431	struct device_attribute attr, const char buf, size_t size)
432	{
433	int err;
434	unsigned int odr;
435	struct iio_dev *indio_dev = dev_get_drvdata(dev);
436
437	err = kstrtoint(buf, 10, &odr);
438	if (err < 0)
439	goto conversion_error;
440
441	mutex_lock(&indio_dev->mlock);
442	err = st_sensors_set_odr(indio_dev, odr);
443	mutex_unlock(&indio_dev->mlock);
444
445	conversion_error:
446	return err < 0 ? err : size;
447	}
448	EXPORT_SYMBOL(st_sensors_sysfs_set_sampling_frequency);
449
450	ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
451	struct device_attribute attr, char buf)
452	{
4d2e4fc2	453	int i, len = 0;
23491b51	454	struct iio_dev *indio_dev = dev_get_drvdata(dev);
4d2e4fc2 DC	455	struct st_sensor_data *sdata = iio_priv(indio_dev);
	456
	457	mutex_lock(&indio_dev->mlock);
	458	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
	459	if (sdata->sensor->odr.odr_avl[i].hz == 0)
	460	break;
	461
	462	len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
	463	sdata->sensor->odr.odr_avl[i].hz);
	464	}
	465	mutex_unlock(&indio_dev->mlock);
	466	buf[len - 1] = '\n';
23491b51	467
4d2e4fc2	468	return len;
23491b51 DC	469	}
	470	EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
	471
	472	ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
	473	struct device_attribute attr, char buf)
	474	{
4d2e4fc2	475	int i, len = 0;
23491b51	476	struct iio_dev *indio_dev = dev_get_drvdata(dev);
4d2e4fc2 DC	477	struct st_sensor_data *sdata = iio_priv(indio_dev);
	478
	479	mutex_lock(&indio_dev->mlock);
	480	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
	481	if (sdata->sensor->fs.fs_avl[i].num == 0)
	482	break;
	483
	484	len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
	485	sdata->sensor->fs.fs_avl[i].gain);
	486	}
	487	mutex_unlock(&indio_dev->mlock);
	488	buf[len - 1] = '\n';
23491b51	489
4d2e4fc2	490	return len;
23491b51 DC	491	}
	492	EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
	493
	494	MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
	495	MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
	496	MODULE_LICENSE("GPL v2");