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;
}
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;
}
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;
}
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_init_sensor(struct iio_dev *indio_dev)
{
int err;
struct st_sensor_data *sdata = iio_priv(indio_dev);
mutex_init(&sdata->tb.buf_lock);
err = st_sensors_set_enable(indio_dev, false);
if (err < 0)
goto init_error;
err = st_sensors_set_fullscale(indio_dev,
sdata->current_fullscale->num);
if (err < 0)
goto init_error;
err = st_sensors_set_odr(indio_dev, sdata->odr);
if (err < 0)
goto init_error;
/* set BDU */
err = st_sensors_write_data_with_mask(indio_dev,
sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
if (err < 0)
goto init_error;
err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
init_error:
return err;
}
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);
}
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;
}
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;
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;
}
int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
const struct iio_trigger_ops *trigger_ops)
{
int err, irq;
struct st_sensor_data *sdata = iio_priv(indio_dev);
unsigned long irq_trig;
sdata->trig = iio_trigger_alloc("%s-trigger", indio_dev->name);
if (sdata->trig == NULL) {
dev_err(&indio_dev->dev, "failed to allocate iio trigger.\n");
return -ENOMEM;
}
irq = sdata->get_irq_data_ready(indio_dev);
irq_trig = irqd_get_trigger_type(irq_get_irq_data(irq));
/*
* If the IRQ is triggered on falling edge, we need to mark the
* interrupt as active low, if the hardware supports this.
*/
if (irq_trig == IRQF_TRIGGER_FALLING) {
if (!sdata->sensor_settings->drdy_irq.addr_ihl) {
dev_err(&indio_dev->dev,
"falling edge specified for IRQ but hardware "
"only support rising edge, will request "
"rising edge\n");
irq_trig = IRQF_TRIGGER_RISING;
} else {
/* Set up INT active low i.e. falling edge */
err = st_sensors_write_data_with_mask(indio_dev,
sdata->sensor_settings->drdy_irq.addr_ihl,
sdata->sensor_settings->drdy_irq.mask_ihl, 1);
if (err < 0)
goto iio_trigger_free;
dev_info(&indio_dev->dev,
"interrupts on the falling edge\n");
}
} else if (irq_trig == IRQF_TRIGGER_RISING) {
dev_info(&indio_dev->dev,
"interrupts on the rising edge\n");
} else {
dev_err(&indio_dev->dev,
"unsupported IRQ trigger specified (%lx), only "
"rising and falling edges supported, enforce "
"rising edge\n", irq_trig);
irq_trig = IRQF_TRIGGER_RISING;
}
/*
* If the interrupt pin is Open Drain, by definition this
* means that the interrupt line may be shared with other
* peripherals. But to do this we also need to have a status
* register and mask to figure out if this sensor was firing
* the IRQ or not, so we can tell the interrupt handle that
* it was "our" interrupt.
*/
if (sdata->int_pin_open_drain &&
sdata->sensor_settings->drdy_irq.addr_stat_drdy)
irq_trig |= IRQF_SHARED;
err = request_threaded_irq(irq,
iio_trigger_generic_data_rdy_poll,
NULL,
irq_trig,
sdata->trig->name,
sdata->trig);
if (err) {
dev_err(&indio_dev->dev, "failed to request trigger IRQ.\n");
goto iio_trigger_free;
}
iio_trigger_set_drvdata(sdata->trig, indio_dev);
sdata->trig->ops = trigger_ops;
sdata->trig->dev.parent = sdata->dev;
err = iio_trigger_register(sdata->trig);
if (err < 0) {
dev_err(&indio_dev->dev, "failed to register iio trigger.\n");
goto iio_trigger_register_error;
}
indio_dev->trig = iio_trigger_get(sdata->trig);
return 0;
iio_trigger_register_error:
free_irq(sdata->get_irq_data_ready(indio_dev), sdata->trig);
iio_trigger_free:
iio_trigger_free(sdata->trig);
return err;
}
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;
}
int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
const struct iio_trigger_ops *trigger_ops)
{
int err, irq;
struct st_sensor_data *sdata = iio_priv(indio_dev);
unsigned long irq_trig;
sdata->trig = iio_trigger_alloc("%s-trigger", indio_dev->name);
if (sdata->trig == NULL) {
dev_err(&indio_dev->dev, "failed to allocate iio trigger.\n");
return -ENOMEM;
}
irq = sdata->get_irq_data_ready(indio_dev);
irq_trig = irqd_get_trigger_type(irq_get_irq_data(irq));
/*
* If the IRQ is triggered on falling edge, we need to mark the
* interrupt as active low, if the hardware supports this.
*/
if (irq_trig == IRQF_TRIGGER_FALLING) {
if (!sdata->sensor_settings->drdy_irq.addr_ihl) {
dev_err(&indio_dev->dev,
"falling edge specified for IRQ but hardware "
"only support rising edge, will request "
"rising edge\n");
irq_trig = IRQF_TRIGGER_RISING;
} else {
/* Set up INT active low i.e. falling edge */
err = st_sensors_write_data_with_mask(indio_dev,
sdata->sensor_settings->drdy_irq.addr_ihl,
sdata->sensor_settings->drdy_irq.mask_ihl, 1);
if (err < 0)
goto iio_trigger_free;
dev_info(&indio_dev->dev,
"interrupts on the falling edge\n");
}
} else if (irq_trig == IRQF_TRIGGER_RISING) {
dev_info(&indio_dev->dev,
"interrupts on the rising edge\n");
} else {
dev_err(&indio_dev->dev,
"unsupported IRQ trigger specified (%lx), only "
"rising and falling edges supported, enforce "
"rising edge\n", irq_trig);
irq_trig = IRQF_TRIGGER_RISING;
}
err = request_threaded_irq(irq,
iio_trigger_generic_data_rdy_poll,
NULL,
irq_trig,
sdata->trig->name,
sdata->trig);
if (err) {
dev_err(&indio_dev->dev, "failed to request trigger IRQ.\n");
goto iio_trigger_free;
}
iio_trigger_set_drvdata(sdata->trig, indio_dev);
sdata->trig->ops = trigger_ops;
sdata->trig->dev.parent = sdata->dev;
err = iio_trigger_register(sdata->trig);
if (err < 0) {
dev_err(&indio_dev->dev, "failed to register iio trigger.\n");
goto iio_trigger_register_error;
}
indio_dev->trig = iio_trigger_get(sdata->trig);
return 0;
iio_trigger_register_error:
free_irq(sdata->get_irq_data_ready(indio_dev), sdata->trig);
iio_trigger_free:
iio_trigger_free(sdata->trig);
return err;
}
