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

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

	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");
			return -EINVAL;
		}
		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");
			return -EINVAL;
		}
		sdata->drdy_int_pin = 2;
		break;
	default:
		dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
		return -EINVAL;
	}

	return 0;
}

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

	mutex_init(&sdata->tb.buf_lock);

	if (pdata)
		err = st_sensors_set_drdy_int_pin(indio_dev, pdata);

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

	if (sdata->current_fullscale) {
		err = st_sensors_set_fullscale(indio_dev,
					       sdata->current_fullscale->num);
		if (err < 0)
			return err;
	} else
		dev_info(&indio_dev->dev, "Full-scale not possible\n");

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

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

	err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);

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

	if (!sdata->sensor->drdy_irq.addr)
		return 0;

	/* 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)
		return -ENOMEM;

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

	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 out;
	} else {
		err = st_sensors_set_enable(indio_dev, true);
		if (err < 0)
			goto out;

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

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

		err = st_sensors_set_enable(indio_dev, false);
	}
out:
	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
38d1c6a9 LJ	201	static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
38d1c6a9 LJ	202	struct st_sensors_platform_data *pdata)
23491b51	203	{
23491b51 DC	204	struct st_sensor_data *sdata = iio_priv(indio_dev);
23491b51 DC	205
23cde4d6 DC	206	switch (pdata->drdy_int_pin) {
	207	case 1:
	208	if (sdata->sensor->drdy_irq.mask_int1 == 0) {
	209	dev_err(&indio_dev->dev,
	210	"DRDY on INT1 not available.\n");
38d1c6a9	211	return -EINVAL;
23cde4d6 DC	212	}
	213	sdata->drdy_int_pin = 1;
	214	break;
	215	case 2:
	216	if (sdata->sensor->drdy_irq.mask_int2 == 0) {
	217	dev_err(&indio_dev->dev,
	218	"DRDY on INT2 not available.\n");
38d1c6a9	219	return -EINVAL;
23cde4d6 DC	220	}
	221	sdata->drdy_int_pin = 2;
	222	break;
	223	default:
	224	dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
38d1c6a9	225	return -EINVAL;
23cde4d6 DC	226	}
23cde4d6 DC	227
38d1c6a9 LJ	228	return 0;
	229	}
	230
	231	int st_sensors_init_sensor(struct iio_dev *indio_dev,
	232	struct st_sensors_platform_data *pdata)
	233	{
	234	struct st_sensor_data *sdata = iio_priv(indio_dev);
	235	int err = 0;
	236
	237	mutex_init(&sdata->tb.buf_lock);
	238
	239	if (pdata)
	240	err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
	241
23491b51 DC	242	err = st_sensors_set_enable(indio_dev, false);
23491b51 DC	243	if (err < 0)
efd9566f	244	return err;
23491b51	245
362f2f86 LJ	246	if (sdata->current_fullscale) {
	247	err = st_sensors_set_fullscale(indio_dev,
	248	sdata->current_fullscale->num);
	249	if (err < 0)
efd9566f	250	return err;
362f2f86 LJ	251	} else
362f2f86 LJ	252	dev_info(&indio_dev->dev, "Full-scale not possible\n");
23491b51 DC	253
	254	err = st_sensors_set_odr(indio_dev, sdata->odr);
	255	if (err < 0)
efd9566f	256	return err;
23491b51 DC	257
	258	/* set BDU */
	259	err = st_sensors_write_data_with_mask(indio_dev,
	260	sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
	261	if (err < 0)
efd9566f	262	return err;
23491b51 DC	263
	264	err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
	265
23491b51 DC	266	return err;
	267	}
	268	EXPORT_SYMBOL(st_sensors_init_sensor);
	269
	270	int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
	271	{
	272	int err;
23cde4d6	273	u8 drdy_mask;
23491b51 DC	274	struct st_sensor_data *sdata = iio_priv(indio_dev);
23491b51 DC	275
38d1c6a9 LJ	276	if (!sdata->sensor->drdy_irq.addr)
	277	return 0;
	278
23491b51 DC	279	/* Enable/Disable the interrupt generator 1. */
	280	if (sdata->sensor->drdy_irq.ig1.en_addr > 0) {
	281	err = st_sensors_write_data_with_mask(indio_dev,
	282	sdata->sensor->drdy_irq.ig1.en_addr,
	283	sdata->sensor->drdy_irq.ig1.en_mask, (int)enable);
	284	if (err < 0)
	285	goto st_accel_set_dataready_irq_error;
	286	}
	287
23cde4d6 DC	288	if (sdata->drdy_int_pin == 1)
	289	drdy_mask = sdata->sensor->drdy_irq.mask_int1;
	290	else
	291	drdy_mask = sdata->sensor->drdy_irq.mask_int2;
	292
23491b51 DC	293	/* Enable/Disable the interrupt generator for data ready. */
23491b51 DC	294	err = st_sensors_write_data_with_mask(indio_dev,
23cde4d6	295	sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable);
23491b51 DC	296
	297	st_accel_set_dataready_irq_error:
	298	return err;
	299	}
	300	EXPORT_SYMBOL(st_sensors_set_dataready_irq);
	301
	302	int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
	303	{
	304	int err = -EINVAL, i;
	305	struct st_sensor_data *sdata = iio_priv(indio_dev);
	306
	307	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
	308	if ((sdata->sensor->fs.fs_avl[i].gain == scale) &&
	309	(sdata->sensor->fs.fs_avl[i].gain != 0)) {
	310	err = 0;
	311	break;
	312	}
	313	}
	314	if (err < 0)
	315	goto st_sensors_match_scale_error;
	316
	317	err = st_sensors_set_fullscale(indio_dev,
	318	sdata->sensor->fs.fs_avl[i].num);
	319
	320	st_sensors_match_scale_error:
	321	return err;
	322	}
	323	EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
	324
	325	static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
607a568a	326	struct iio_chan_spec const ch, int data)
23491b51 DC	327	{
23491b51 DC	328	int err;
607a568a	329	u8 *outdata;
23491b51	330	struct st_sensor_data *sdata = iio_priv(indio_dev);
607a568a DC	331	unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
	332
	333	outdata = kmalloc(byte_for_channel, GFP_KERNEL);
caf5ca12 LJ	334	if (!outdata)
caf5ca12 LJ	335	return -ENOMEM;
23491b51 DC	336
23491b51 DC	337	err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
607a568a	338	ch->address, byte_for_channel,
23491b51 DC	339	outdata, sdata->multiread_bit);
23491b51 DC	340	if (err < 0)
607a568a	341	goto st_sensors_free_memory;
23491b51	342
607a568a DC	343	if (byte_for_channel == 2)
	344	*data = (s16)get_unaligned_le16(outdata);
	345	else if (byte_for_channel == 3)
	346	*data = (s32)st_sensors_get_unaligned_le24(outdata);
23491b51	347
607a568a DC	348	st_sensors_free_memory:
607a568a DC	349	kfree(outdata);
caf5ca12	350
23491b51 DC	351	return err;
	352	}
	353
	354	int st_sensors_read_info_raw(struct iio_dev *indio_dev,
	355	struct iio_chan_spec const ch, int val)
	356	{
	357	int err;
	358	struct st_sensor_data *sdata = iio_priv(indio_dev);
	359
	360	mutex_lock(&indio_dev->mlock);
	361	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
	362	err = -EBUSY;
5bb8e72d	363	goto out;
23491b51 DC	364	} else {
	365	err = st_sensors_set_enable(indio_dev, true);
	366	if (err < 0)
5bb8e72d	367	goto out;
23491b51 DC	368
23491b51 DC	369	msleep((sdata->sensor->bootime * 1000) / sdata->odr);
607a568a	370	err = st_sensors_read_axis_data(indio_dev, ch, val);
23491b51	371	if (err < 0)
5bb8e72d	372	goto out;
23491b51 DC	373
23491b51 DC	374	val = val >> ch->scan_type.shift;
d61a04dc DC	375
d61a04dc DC	376	err = st_sensors_set_enable(indio_dev, false);
23491b51	377	}
5bb8e72d	378	out:
23491b51 DC	379	mutex_unlock(&indio_dev->mlock);
	380
	381	return err;
23491b51 DC	382	}
	383	EXPORT_SYMBOL(st_sensors_read_info_raw);
	384
	385	int st_sensors_check_device_support(struct iio_dev *indio_dev,
	386	int num_sensors_list, const struct st_sensors *sensors)
	387	{
	388	u8 wai;
	389	int i, n, err;
	390	struct st_sensor_data *sdata = iio_priv(indio_dev);
	391
	392	err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
	393	ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
	394	if (err < 0) {
	395	dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
	396	goto read_wai_error;
	397	}
	398
	399	for (i = 0; i < num_sensors_list; i++) {
	400	if (sensors[i].wai == wai)
	401	break;
	402	}
	403	if (i == num_sensors_list)
	404	goto device_not_supported;
	405
	406	for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) {
	407	if (strcmp(indio_dev->name,
	408	&sensors[i].sensors_supported[n][0]) == 0)
	409	break;
	410	}
	411	if (n == ARRAY_SIZE(sensors[i].sensors_supported)) {
	412	dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
	413	goto sensor_name_mismatch;
	414	}
	415
	416	sdata->sensor = (struct st_sensors *)&sensors[i];
	417
	418	return i;
	419
	420	device_not_supported:
	421	dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
	422	sensor_name_mismatch:
	423	err = -ENODEV;
	424	read_wai_error:
	425	return err;
	426	}
	427	EXPORT_SYMBOL(st_sensors_check_device_support);
	428
	429	ssize_t st_sensors_sysfs_get_sampling_frequency(struct device *dev,
	430	struct device_attribute attr, char buf)
	431	{
	432	struct st_sensor_data *adata = iio_priv(dev_get_drvdata(dev));
	433
	434	return sprintf(buf, "%d\n", adata->odr);
	435	}
	436	EXPORT_SYMBOL(st_sensors_sysfs_get_sampling_frequency);
	437
	438	ssize_t st_sensors_sysfs_set_sampling_frequency(struct device *dev,
	439	struct device_attribute attr, const char buf, size_t size)
	440	{
	441	int err;
	442	unsigned int odr;
	443	struct iio_dev *indio_dev = dev_get_drvdata(dev);
	444
	445	err = kstrtoint(buf, 10, &odr);
446	if (err < 0)
447	goto conversion_error;
448
449	mutex_lock(&indio_dev->mlock);
450	err = st_sensors_set_odr(indio_dev, odr);
451	mutex_unlock(&indio_dev->mlock);
452
453	conversion_error:
454	return err < 0 ? err : size;
455	}
456	EXPORT_SYMBOL(st_sensors_sysfs_set_sampling_frequency);
457
458	ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
459	struct device_attribute attr, char buf)
460	{
4d2e4fc2	461	int i, len = 0;
23491b51	462	struct iio_dev *indio_dev = dev_get_drvdata(dev);
4d2e4fc2 DC	463	struct st_sensor_data *sdata = iio_priv(indio_dev);
	464
	465	mutex_lock(&indio_dev->mlock);
	466	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
	467	if (sdata->sensor->odr.odr_avl[i].hz == 0)
	468	break;
	469
	470	len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
	471	sdata->sensor->odr.odr_avl[i].hz);
	472	}
	473	mutex_unlock(&indio_dev->mlock);
	474	buf[len - 1] = '\n';
23491b51	475
4d2e4fc2	476	return len;
23491b51 DC	477	}
	478	EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
	479
	480	ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
	481	struct device_attribute attr, char buf)
	482	{
4d2e4fc2	483	int i, len = 0;
23491b51	484	struct iio_dev *indio_dev = dev_get_drvdata(dev);
4d2e4fc2 DC	485	struct st_sensor_data *sdata = iio_priv(indio_dev);
	486
	487	mutex_lock(&indio_dev->mlock);
	488	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
	489	if (sdata->sensor->fs.fs_avl[i].num == 0)
	490	break;
	491
	492	len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
	493	sdata->sensor->fs.fs_avl[i].gain);
	494	}
	495	mutex_unlock(&indio_dev->mlock);
	496	buf[len - 1] = '\n';
23491b51	497
4d2e4fc2	498	return len;
23491b51 DC	499	}
	500	EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
	501
	502	MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
	503	MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
	504	MODULE_LICENSE("GPL v2");