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

#include "st_sensors_core.h"

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

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

int st_sensors_debugfs_reg_access(struct iio_dev *indio_dev,
				  unsigned reg, unsigned writeval,
				  unsigned *readval)
{
	struct st_sensor_data *sdata = iio_priv(indio_dev);
	u8 readdata;
	int err;

	if (!readval)
		return sdata->tf->write_byte(&sdata->tb, sdata->dev,
					     (u8)reg, (u8)writeval);

	err = sdata->tf->read_byte(&sdata->tb, sdata->dev, (u8)reg, &readdata);
	if (err < 0)
		return err;

	*readval = (unsigned)readdata;

	return 0;
}
EXPORT_SYMBOL(st_sensors_debugfs_reg_access);

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

	if (sdata->sensor_settings->fs.addr == 0)
		return 0;

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

	if (pdata->open_drain) {
		if (!sdata->sensor_settings->drdy_irq.addr_od)
			dev_err(&indio_dev->dev,
				"open drain requested but unsupported.\n");
		else
			sdata->int_pin_open_drain = true;
	}

	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;

	pdata->open_drain = of_property_read_bool(np, "drive-open-drain");

	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;

	/* 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;

	/* Disable DRDY, this might be still be enabled after reboot. */
	err = st_sensors_set_dataready_irq(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 */
	if (sdata->sensor_settings->bdu.addr) {
		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;
	}

	if (sdata->int_pin_open_drain) {
		dev_info(&indio_dev->dev,
			 "set interrupt line to open drain mode\n");
		err = st_sensors_write_data_with_mask(indio_dev,
				sdata->sensor_settings->drdy_irq.addr_od,
				sdata->sensor_settings->drdy_irq.mask_od, 1);
		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;

	/* Flag to the poll function that the hardware trigger is in use */
	sdata->hw_irq_trigger = enable;

	/* 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 == 1)
		*data = (s8)*outdata;
	else 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)
{
	int i, n, err;
	u8 wai;
	struct st_sensor_data *sdata = iio_priv(indio_dev);

	for (i = 0; i < num_sensors_list; i++) {
		for (n = 0; n < ST_SENSORS_MAX_4WAI; n++) {
			if (strcmp(indio_dev->name,
				sensor_settings[i].sensors_supported[n]) == 0) {
				break;
			}
		}
		if (n < ST_SENSORS_MAX_4WAI)
			break;
	}
	if (i == num_sensors_list) {
		dev_err(&indio_dev->dev, "device name %s not recognized.\n",
							indio_dev->name);
		return -ENODEV;
	}

	err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
					sensor_settings[i].wai_addr, &wai);
	if (err < 0) {
		dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
		return err;
	}

	if (sensor_settings[i].wai != wai) {
		dev_err(&indio_dev->dev, "%s: WhoAmI mismatch (0x%x).\n",
						indio_dev->name, wai);
		return -EINVAL;
	}

	sdata->sensor_settings =
			(struct st_sensor_settings *)&sensor_settings[i];

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