[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 <linux/regulator/consumer.h>
#include <linux/of.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_sensor_settings *sensor_settings,
			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_settings->odr.odr_avl[i].hz == 0)
			goto st_sensors_match_odr_error;

		if (sensor_settings->odr.odr_avl[i].hz == odr) {
			odr_out->hz = sensor_settings->odr.odr_avl[i].hz;
			odr_out->value = sensor_settings->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_settings, odr, &odr_out);
	if (err < 0)
		goto st_sensors_match_odr_error;

	if ((sdata->sensor_settings->odr.addr ==
					sdata->sensor_settings->pw.addr) &&
				(sdata->sensor_settings->odr.mask ==
					sdata->sensor_settings->pw.mask)) {
		if (sdata->enabled == true) {
			err = st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor_settings->odr.addr,
				sdata->sensor_settings->odr.mask,
				odr_out.value);
		} else {
			err = 0;
		}
	} else {
		err = st_sensors_write_data_with_mask(indio_dev,
			sdata->sensor_settings->odr.addr,
			sdata->sensor_settings->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_sensor_settings *sensor_settings,
					unsigned int fs, int *index_fs_avl)
{
	int i, ret = -EINVAL;

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

		if (sensor_settings->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_settings, fs, &i);
	if (err < 0)
		goto st_accel_set_fullscale_error;

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

	sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
					&sdata->sensor_settings->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_settings->pw.value_on;
		if ((sdata->sensor_settings->odr.addr ==
					sdata->sensor_settings->pw.addr) &&
				(sdata->sensor_settings->odr.mask ==
					sdata->sensor_settings->pw.mask)) {
			err = st_sensors_match_odr(sdata->sensor_settings,
							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_settings->pw.addr,
				sdata->sensor_settings->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_settings->pw.addr,
				sdata->sensor_settings->pw.mask,
				sdata->sensor_settings->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_settings->enable_axis.addr,
				sdata->sensor_settings->enable_axis.mask,
				axis_enable);
}
EXPORT_SYMBOL(st_sensors_set_axis_enable);

void st_sensors_power_enable(struct iio_dev *indio_dev)
{
	struct st_sensor_data *pdata = iio_priv(indio_dev);
	int err;

	/* Regulators not mandatory, but if requested we should enable them. */
	pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
	if (!IS_ERR(pdata->vdd)) {
		err = regulator_enable(pdata->vdd);
		if (err != 0)
			dev_warn(&indio_dev->dev,
				 "Failed to enable specified Vdd supply\n");
	}

	pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
	if (!IS_ERR(pdata->vdd_io)) {
		err = regulator_enable(pdata->vdd_io);
		if (err != 0)
			dev_warn(&indio_dev->dev,
				 "Failed to enable specified Vdd_IO supply\n");
	}
}
EXPORT_SYMBOL(st_sensors_power_enable);

void st_sensors_power_disable(struct iio_dev *indio_dev)
{
	struct st_sensor_data *pdata = iio_priv(indio_dev);

	if (!IS_ERR(pdata->vdd))
		regulator_disable(pdata->vdd);

	if (!IS_ERR(pdata->vdd_io))
		regulator_disable(pdata->vdd_io);
}
EXPORT_SYMBOL(st_sensors_power_disable);

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

	/* Sensor does not support interrupts */
	if (sdata->sensor_settings->drdy_irq.addr == 0) {
		if (pdata->drdy_int_pin)
			dev_info(&indio_dev->dev,
				 "DRDY on pin INT%d specified, but sensor "
				 "does not support interrupts\n",
				 pdata->drdy_int_pin);
		return 0;
	}

	switch (pdata->drdy_int_pin) {
	case 1:
		if (sdata->sensor_settings->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_settings->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;
}

#ifdef CONFIG_OF
static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
		struct st_sensors_platform_data *defdata)
{
	struct st_sensors_platform_data *pdata;
	struct device_node *np = dev->of_node;
	u32 val;

	if (!np)
		return NULL;

	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
	if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2))
		pdata->drdy_int_pin = (u8) val;
	else
		pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0;

	return pdata;
}
#else
static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
		struct st_sensors_platform_data *defdata)
{
	return NULL;
}
#endif

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);
	struct st_sensors_platform_data *of_pdata;
	int err = 0;

	mutex_init(&sdata->tb.buf_lock);

	/* If OF/DT pdata exists, it will take precedence of anything else */
	of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata);
	if (of_pdata)
		pdata = of_pdata;

	if (pdata) {
		err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
		if (err < 0)
			return err;
	}

	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_settings->bdu.addr,
					sdata->sensor_settings->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_settings->drdy_irq.addr)
		return 0;

	/* Enable/Disable the interrupt generator 1. */
	if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) {
		err = st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor_settings->drdy_irq.ig1.en_addr,
				sdata->sensor_settings->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_settings->drdy_irq.mask_int1;
	else
		drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2;

	/* Enable/Disable the interrupt generator for data ready. */
	err = st_sensors_write_data_with_mask(indio_dev,
					sdata->sensor_settings->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_settings->fs.fs_avl[i].gain == scale) &&
				(sdata->sensor_settings->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_settings->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_settings->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_sensor_settings *sensor_settings)
{
	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 (sensor_settings[i].wai == wai)
			break;
	}
	if (i == num_sensors_list)
		goto device_not_supported;

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

	sdata->sensor_settings =
			(struct st_sensor_settings *)&sensor_settings[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_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_settings->odr.odr_avl[i].hz == 0)
			break;

		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
				sdata->sensor_settings->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_settings->fs.fs_avl[i].num == 0)
			break;

		len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
				sdata->sensor_settings->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>
ea7e586b	16	#include <linux/regulator/consumer.h>
3ce85cc4	17	#include <linux/of.h>
23491b51	18	#include <asm/unaligned.h>
23491b51 DC	19	#include <linux/iio/common/st_sensors.h>
	20
	21
	22	#define ST_SENSORS_WAI_ADDRESS 0x0f
	23
607a568a DC	24	static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
607a568a DC	25	{
23cde4d6	26	return (s32)((p[0] \| p[1] << 8 \| p[2] << 16) << 8) >> 8;
607a568a DC	27	}
607a568a DC	28
23491b51 DC	29	static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
	30	u8 reg_addr, u8 mask, u8 data)
	31	{
	32	int err;
	33	u8 new_data;
	34	struct st_sensor_data *sdata = iio_priv(indio_dev);
	35
	36	err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data);
	37	if (err < 0)
	38	goto st_sensors_write_data_with_mask_error;
	39
	40	new_data = ((new_data & (~mask)) \| ((data << __ffs(mask)) & mask));
	41	err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data);
	42
	43	st_sensors_write_data_with_mask_error:
	44	return err;
	45	}
	46
a7ee8839	47	static int st_sensors_match_odr(struct st_sensor_settings *sensor_settings,
23491b51 DC	48	unsigned int odr, struct st_sensor_odr_avl *odr_out)
	49	{
	50	int i, ret = -EINVAL;
	51
	52	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
a7ee8839	53	if (sensor_settings->odr.odr_avl[i].hz == 0)
23491b51 DC	54	goto st_sensors_match_odr_error;
23491b51 DC	55
a7ee8839 DC	56	if (sensor_settings->odr.odr_avl[i].hz == odr) {
	57	odr_out->hz = sensor_settings->odr.odr_avl[i].hz;
	58	odr_out->value = sensor_settings->odr.odr_avl[i].value;
23491b51 DC	59	ret = 0;
	60	break;
	61	}
	62	}
	63
	64	st_sensors_match_odr_error:
	65	return ret;
	66	}
	67
	68	int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
	69	{
	70	int err;
852afe99	71	struct st_sensor_odr_avl odr_out = {0, 0};
23491b51 DC	72	struct st_sensor_data *sdata = iio_priv(indio_dev);
23491b51 DC	73
a7ee8839	74	err = st_sensors_match_odr(sdata->sensor_settings, odr, &odr_out);
23491b51 DC	75	if (err < 0)
	76	goto st_sensors_match_odr_error;
	77
a7ee8839 DC	78	if ((sdata->sensor_settings->odr.addr ==
	79	sdata->sensor_settings->pw.addr) &&
	80	(sdata->sensor_settings->odr.mask ==
	81	sdata->sensor_settings->pw.mask)) {
23491b51 DC	82	if (sdata->enabled == true) {
23491b51 DC	83	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	84	sdata->sensor_settings->odr.addr,
a7ee8839 DC	85	sdata->sensor_settings->odr.mask,
23491b51 DC	86	odr_out.value);
	87	} else {
	88	err = 0;
	89	}
	90	} else {
	91	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	92	sdata->sensor_settings->odr.addr,
a7ee8839 DC	93	sdata->sensor_settings->odr.mask,
23491b51 DC	94	odr_out.value);
	95	}
	96	if (err >= 0)
	97	sdata->odr = odr_out.hz;
	98
	99	st_sensors_match_odr_error:
	100	return err;
	101	}
	102	EXPORT_SYMBOL(st_sensors_set_odr);
	103
a7ee8839	104	static int st_sensors_match_fs(struct st_sensor_settings *sensor_settings,
23491b51 DC	105	unsigned int fs, int *index_fs_avl)
	106	{
	107	int i, ret = -EINVAL;
	108
	109	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
a7ee8839	110	if (sensor_settings->fs.fs_avl[i].num == 0)
23491b51 DC	111	goto st_sensors_match_odr_error;
23491b51 DC	112
a7ee8839	113	if (sensor_settings->fs.fs_avl[i].num == fs) {
23491b51 DC	114	*index_fs_avl = i;
	115	ret = 0;
	116	break;
	117	}
	118	}
	119
	120	st_sensors_match_odr_error:
	121	return ret;
	122	}
	123
a7ee8839	124	static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
23491b51	125	{
852afe99	126	int err, i = 0;
23491b51 DC	127	struct st_sensor_data *sdata = iio_priv(indio_dev);
23491b51 DC	128
a7ee8839	129	err = st_sensors_match_fs(sdata->sensor_settings, fs, &i);
23491b51 DC	130	if (err < 0)
	131	goto st_accel_set_fullscale_error;
	132
	133	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	134	sdata->sensor_settings->fs.addr,
	135	sdata->sensor_settings->fs.mask,
	136	sdata->sensor_settings->fs.fs_avl[i].value);
23491b51 DC	137	if (err < 0)
	138	goto st_accel_set_fullscale_error;
	139
	140	sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
a7ee8839	141	&sdata->sensor_settings->fs.fs_avl[i];
23491b51 DC	142	return err;
	143
	144	st_accel_set_fullscale_error:
	145	dev_err(&indio_dev->dev, "failed to set new fullscale.\n");
	146	return err;
	147	}
	148
	149	int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
	150	{
23491b51 DC	151	u8 tmp_value;
23491b51 DC	152	int err = -EINVAL;
852afe99 DC	153	bool found = false;
852afe99 DC	154	struct st_sensor_odr_avl odr_out = {0, 0};
23491b51 DC	155	struct st_sensor_data *sdata = iio_priv(indio_dev);
	156
	157	if (enable) {
a7ee8839 DC	158	tmp_value = sdata->sensor_settings->pw.value_on;
	159	if ((sdata->sensor_settings->odr.addr ==
	160	sdata->sensor_settings->pw.addr) &&
	161	(sdata->sensor_settings->odr.mask ==
	162	sdata->sensor_settings->pw.mask)) {
	163	err = st_sensors_match_odr(sdata->sensor_settings,
23491b51 DC	164	sdata->odr, &odr_out);
	165	if (err < 0)
	166	goto set_enable_error;
	167	tmp_value = odr_out.value;
	168	found = true;
	169	}
	170	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	171	sdata->sensor_settings->pw.addr,
a7ee8839 DC	172	sdata->sensor_settings->pw.mask, tmp_value);
23491b51 DC	173	if (err < 0)
	174	goto set_enable_error;
	175
	176	sdata->enabled = true;
	177
	178	if (found)
	179	sdata->odr = odr_out.hz;
	180	} else {
	181	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	182	sdata->sensor_settings->pw.addr,
	183	sdata->sensor_settings->pw.mask,
	184	sdata->sensor_settings->pw.value_off);
23491b51 DC	185	if (err < 0)
	186	goto set_enable_error;
	187
	188	sdata->enabled = false;
	189	}
	190
	191	set_enable_error:
	192	return err;
	193	}
	194	EXPORT_SYMBOL(st_sensors_set_enable);
	195
	196	int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
	197	{
	198	struct st_sensor_data *sdata = iio_priv(indio_dev);
	199
	200	return st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	201	sdata->sensor_settings->enable_axis.addr,
	202	sdata->sensor_settings->enable_axis.mask,
	203	axis_enable);
23491b51 DC	204	}
	205	EXPORT_SYMBOL(st_sensors_set_axis_enable);
	206
ea7e586b LW	207	void st_sensors_power_enable(struct iio_dev *indio_dev)
	208	{
	209	struct st_sensor_data *pdata = iio_priv(indio_dev);
	210	int err;
	211
	212	/* Regulators not mandatory, but if requested we should enable them. */
	213	pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
	214	if (!IS_ERR(pdata->vdd)) {
	215	err = regulator_enable(pdata->vdd);
	216	if (err != 0)
	217	dev_warn(&indio_dev->dev,
	218	"Failed to enable specified Vdd supply\n");
	219	}
	220
	221	pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
	222	if (!IS_ERR(pdata->vdd_io)) {
	223	err = regulator_enable(pdata->vdd_io);
	224	if (err != 0)
	225	dev_warn(&indio_dev->dev,
	226	"Failed to enable specified Vdd_IO supply\n");
	227	}
	228	}
	229	EXPORT_SYMBOL(st_sensors_power_enable);
	230
	231	void st_sensors_power_disable(struct iio_dev *indio_dev)
	232	{
	233	struct st_sensor_data *pdata = iio_priv(indio_dev);
	234
	235	if (!IS_ERR(pdata->vdd))
	236	regulator_disable(pdata->vdd);
	237
	238	if (!IS_ERR(pdata->vdd_io))
	239	regulator_disable(pdata->vdd_io);
	240	}
	241	EXPORT_SYMBOL(st_sensors_power_disable);
	242
38d1c6a9	243	static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
a7ee8839	244	struct st_sensors_platform_data *pdata)
23491b51	245	{
23491b51 DC	246	struct st_sensor_data *sdata = iio_priv(indio_dev);
23491b51 DC	247
d2bc4318 LW	248	/* Sensor does not support interrupts */
	249	if (sdata->sensor_settings->drdy_irq.addr == 0) {
	250	if (pdata->drdy_int_pin)
	251	dev_info(&indio_dev->dev,
	252	"DRDY on pin INT%d specified, but sensor "
	253	"does not support interrupts\n",
	254	pdata->drdy_int_pin);
	255	return 0;
	256	}
	257
23cde4d6 DC	258	switch (pdata->drdy_int_pin) {
23cde4d6 DC	259	case 1:
a7ee8839	260	if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) {
23cde4d6 DC	261	dev_err(&indio_dev->dev,
23cde4d6 DC	262	"DRDY on INT1 not available.\n");
38d1c6a9	263	return -EINVAL;
23cde4d6 DC	264	}
	265	sdata->drdy_int_pin = 1;
	266	break;
	267	case 2:
a7ee8839	268	if (sdata->sensor_settings->drdy_irq.mask_int2 == 0) {
23cde4d6 DC	269	dev_err(&indio_dev->dev,
23cde4d6 DC	270	"DRDY on INT2 not available.\n");
38d1c6a9	271	return -EINVAL;
23cde4d6 DC	272	}
	273	sdata->drdy_int_pin = 2;
	274	break;
	275	default:
	276	dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
38d1c6a9	277	return -EINVAL;
23cde4d6 DC	278	}
23cde4d6 DC	279
38d1c6a9 LJ	280	return 0;
	281	}
	282
3ce85cc4 LW	283	#ifdef CONFIG_OF
	284	static struct st_sensors_platform_data st_sensors_of_probe(struct device dev,
	285	struct st_sensors_platform_data *defdata)
	286	{
	287	struct st_sensors_platform_data *pdata;
	288	struct device_node *np = dev->of_node;
	289	u32 val;
	290
	291	if (!np)
	292	return NULL;
	293
	294	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
	295	if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2))
	296	pdata->drdy_int_pin = (u8) val;
	297	else
d2bc4318	298	pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0;
3ce85cc4 LW	299
	300	return pdata;
	301	}
	302	#else
	303	static struct st_sensors_platform_data st_sensors_of_probe(struct device dev,
	304	struct st_sensors_platform_data *defdata)
	305	{
	306	return NULL;
	307	}
	308	#endif
	309
38d1c6a9 LJ	310	int st_sensors_init_sensor(struct iio_dev *indio_dev,
	311	struct st_sensors_platform_data *pdata)
	312	{
	313	struct st_sensor_data *sdata = iio_priv(indio_dev);
3ce85cc4	314	struct st_sensors_platform_data *of_pdata;
38d1c6a9 LJ	315	int err = 0;
	316
	317	mutex_init(&sdata->tb.buf_lock);
	318
3ce85cc4 LW	319	/* If OF/DT pdata exists, it will take precedence of anything else */
	320	of_pdata = st_sensors_of_probe(indio_dev->dev.parent, pdata);
	321	if (of_pdata)
	322	pdata = of_pdata;
	323
3c8bf223	324	if (pdata) {
38d1c6a9	325	err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
3c8bf223 LJ	326	if (err < 0)
	327	return err;
	328	}
38d1c6a9	329
23491b51 DC	330	err = st_sensors_set_enable(indio_dev, false);
23491b51 DC	331	if (err < 0)
efd9566f	332	return err;
23491b51	333
362f2f86 LJ	334	if (sdata->current_fullscale) {
362f2f86 LJ	335	err = st_sensors_set_fullscale(indio_dev,
a7ee8839	336	sdata->current_fullscale->num);
362f2f86	337	if (err < 0)
efd9566f	338	return err;
362f2f86 LJ	339	} else
362f2f86 LJ	340	dev_info(&indio_dev->dev, "Full-scale not possible\n");
23491b51 DC	341
	342	err = st_sensors_set_odr(indio_dev, sdata->odr);
	343	if (err < 0)
efd9566f	344	return err;
23491b51 DC	345
	346	/* set BDU */
	347	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	348	sdata->sensor_settings->bdu.addr,
a7ee8839 DC	349	sdata->sensor_settings->bdu.mask, true);
23491b51	350	if (err < 0)
efd9566f	351	return err;
23491b51 DC	352
	353	err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
	354
23491b51 DC	355	return err;
	356	}
	357	EXPORT_SYMBOL(st_sensors_init_sensor);
	358
	359	int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
	360	{
	361	int err;
23cde4d6	362	u8 drdy_mask;
23491b51 DC	363	struct st_sensor_data *sdata = iio_priv(indio_dev);
23491b51 DC	364
a7ee8839	365	if (!sdata->sensor_settings->drdy_irq.addr)
38d1c6a9 LJ	366	return 0;
38d1c6a9 LJ	367
23491b51	368	/* Enable/Disable the interrupt generator 1. */
a7ee8839	369	if (sdata->sensor_settings->drdy_irq.ig1.en_addr > 0) {
23491b51	370	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	371	sdata->sensor_settings->drdy_irq.ig1.en_addr,
	372	sdata->sensor_settings->drdy_irq.ig1.en_mask,
	373	(int)enable);
23491b51 DC	374	if (err < 0)
	375	goto st_accel_set_dataready_irq_error;
	376	}
	377
23cde4d6	378	if (sdata->drdy_int_pin == 1)
a7ee8839	379	drdy_mask = sdata->sensor_settings->drdy_irq.mask_int1;
23cde4d6	380	else
a7ee8839	381	drdy_mask = sdata->sensor_settings->drdy_irq.mask_int2;
23cde4d6	382
23491b51 DC	383	/* Enable/Disable the interrupt generator for data ready. */
23491b51 DC	384	err = st_sensors_write_data_with_mask(indio_dev,
a7ee8839 DC	385	sdata->sensor_settings->drdy_irq.addr,
a7ee8839 DC	386	drdy_mask, (int)enable);
23491b51 DC	387
	388	st_accel_set_dataready_irq_error:
	389	return err;
	390	}
	391	EXPORT_SYMBOL(st_sensors_set_dataready_irq);
	392
	393	int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
	394	{
	395	int err = -EINVAL, i;
	396	struct st_sensor_data *sdata = iio_priv(indio_dev);
	397
	398	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
a7ee8839 DC	399	if ((sdata->sensor_settings->fs.fs_avl[i].gain == scale) &&
a7ee8839 DC	400	(sdata->sensor_settings->fs.fs_avl[i].gain != 0)) {
23491b51 DC	401	err = 0;
	402	break;
	403	}
	404	}
	405	if (err < 0)
	406	goto st_sensors_match_scale_error;
	407
	408	err = st_sensors_set_fullscale(indio_dev,
a7ee8839	409	sdata->sensor_settings->fs.fs_avl[i].num);
23491b51 DC	410
	411	st_sensors_match_scale_error:
	412	return err;
	413	}
	414	EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
	415
	416	static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
607a568a	417	struct iio_chan_spec const ch, int data)
23491b51 DC	418	{
23491b51 DC	419	int err;
607a568a	420	u8 *outdata;
23491b51	421	struct st_sensor_data *sdata = iio_priv(indio_dev);
607a568a DC	422	unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
	423
	424	outdata = kmalloc(byte_for_channel, GFP_KERNEL);
caf5ca12 LJ	425	if (!outdata)
caf5ca12 LJ	426	return -ENOMEM;
23491b51 DC	427
23491b51 DC	428	err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
607a568a	429	ch->address, byte_for_channel,
23491b51 DC	430	outdata, sdata->multiread_bit);
23491b51 DC	431	if (err < 0)
607a568a	432	goto st_sensors_free_memory;
23491b51	433
607a568a DC	434	if (byte_for_channel == 2)
	435	*data = (s16)get_unaligned_le16(outdata);
	436	else if (byte_for_channel == 3)
	437	*data = (s32)st_sensors_get_unaligned_le24(outdata);
23491b51	438
607a568a DC	439	st_sensors_free_memory:
607a568a DC	440	kfree(outdata);
caf5ca12	441
23491b51 DC	442	return err;
	443	}
	444
	445	int st_sensors_read_info_raw(struct iio_dev *indio_dev,
	446	struct iio_chan_spec const ch, int val)
	447	{
	448	int err;
	449	struct st_sensor_data *sdata = iio_priv(indio_dev);
	450
	451	mutex_lock(&indio_dev->mlock);
	452	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
	453	err = -EBUSY;
5bb8e72d	454	goto out;
23491b51 DC	455	} else {
	456	err = st_sensors_set_enable(indio_dev, true);
	457	if (err < 0)
5bb8e72d	458	goto out;
23491b51	459
a7ee8839	460	msleep((sdata->sensor_settings->bootime * 1000) / sdata->odr);
607a568a	461	err = st_sensors_read_axis_data(indio_dev, ch, val);
23491b51	462	if (err < 0)
5bb8e72d	463	goto out;
23491b51 DC	464
23491b51 DC	465	val = val >> ch->scan_type.shift;
d61a04dc DC	466
d61a04dc DC	467	err = st_sensors_set_enable(indio_dev, false);
23491b51	468	}
5bb8e72d	469	out:
23491b51 DC	470	mutex_unlock(&indio_dev->mlock);
	471
	472	return err;
23491b51 DC	473	}
	474	EXPORT_SYMBOL(st_sensors_read_info_raw);
	475
	476	int st_sensors_check_device_support(struct iio_dev *indio_dev,
a7ee8839 DC	477	int num_sensors_list,
a7ee8839 DC	478	const struct st_sensor_settings *sensor_settings)
23491b51 DC	479	{
	480	u8 wai;
	481	int i, n, err;
	482	struct st_sensor_data *sdata = iio_priv(indio_dev);
	483
	484	err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
	485	ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
	486	if (err < 0) {
	487	dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
	488	goto read_wai_error;
	489	}
	490
	491	for (i = 0; i < num_sensors_list; i++) {
a7ee8839	492	if (sensor_settings[i].wai == wai)
23491b51 DC	493	break;
	494	}
	495	if (i == num_sensors_list)
	496	goto device_not_supported;
	497
a7ee8839	498	for (n = 0; n < ARRAY_SIZE(sensor_settings[i].sensors_supported); n++) {
23491b51	499	if (strcmp(indio_dev->name,
a7ee8839	500	&sensor_settings[i].sensors_supported[n][0]) == 0)
23491b51 DC	501	break;
23491b51 DC	502	}
a7ee8839	503	if (n == ARRAY_SIZE(sensor_settings[i].sensors_supported)) {
23491b51 DC	504	dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
	505	goto sensor_name_mismatch;
	506	}
	507
a7ee8839 DC	508	sdata->sensor_settings =
a7ee8839 DC	509	(struct st_sensor_settings *)&sensor_settings[i];
23491b51 DC	510
	511	return i;
	512
	513	device_not_supported:
	514	dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
	515	sensor_name_mismatch:
	516	err = -ENODEV;
	517	read_wai_error:
	518	return err;
	519	}
	520	EXPORT_SYMBOL(st_sensors_check_device_support);
	521
23491b51 DC	522	ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
	523	struct device_attribute attr, char buf)
	524	{
4d2e4fc2	525	int i, len = 0;
23491b51	526	struct iio_dev *indio_dev = dev_get_drvdata(dev);
4d2e4fc2 DC	527	struct st_sensor_data *sdata = iio_priv(indio_dev);
	528
	529	mutex_lock(&indio_dev->mlock);
	530	for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
a7ee8839	531	if (sdata->sensor_settings->odr.odr_avl[i].hz == 0)
4d2e4fc2 DC	532	break;
	533
	534	len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
a7ee8839	535	sdata->sensor_settings->odr.odr_avl[i].hz);
4d2e4fc2 DC	536	}
	537	mutex_unlock(&indio_dev->mlock);
	538	buf[len - 1] = '\n';
23491b51	539
4d2e4fc2	540	return len;
23491b51 DC	541	}
	542	EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
	543
	544	ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
	545	struct device_attribute attr, char buf)
	546	{
4d2e4fc2	547	int i, len = 0;
23491b51	548	struct iio_dev *indio_dev = dev_get_drvdata(dev);
4d2e4fc2 DC	549	struct st_sensor_data *sdata = iio_priv(indio_dev);
	550
	551	mutex_lock(&indio_dev->mlock);
	552	for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
a7ee8839	553	if (sdata->sensor_settings->fs.fs_avl[i].num == 0)
4d2e4fc2 DC	554	break;
	555
	556	len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
a7ee8839	557	sdata->sensor_settings->fs.fs_avl[i].gain);
4d2e4fc2 DC	558	}
	559	mutex_unlock(&indio_dev->mlock);
	560	buf[len - 1] = '\n';
23491b51	561
4d2e4fc2	562	return len;
23491b51 DC	563	}
	564	EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
	565
	566	MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
	567	MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
	568	MODULE_LICENSE("GPL v2");