[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 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)
{
	int err;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	mutex_init(&sdata->tb.buf_lock);

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

	/* Enable/Disable the interrupt generator for data ready. */
	err = st_sensors_write_data_with_mask(indio_dev,
			sdata->sensor->drdy_irq.addr,
			sdata->sensor->drdy_irq.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,
							u8 ch_addr, int *data)
{
	int err;
	u8 outdata[ST_SENSORS_BYTE_FOR_CHANNEL];
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
				ch_addr, ST_SENSORS_BYTE_FOR_CHANNEL,
				outdata, sdata->multiread_bit);
	if (err < 0)
		goto read_error;

	*data = (s16)get_unaligned_le16(outdata);

read_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->address, val);
		if (err < 0)
			goto read_error;

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