diff options
Diffstat (limited to 'drivers/iio/common')
-rw-r--r-- | drivers/iio/common/st_sensors/st_sensors_buffer.c | 7 | ||||
-rw-r--r-- | drivers/iio/common/st_sensors/st_sensors_core.c | 70 |
2 files changed, 41 insertions, 36 deletions
diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c index 71a2c5f..1665c8e 100644 --- a/drivers/iio/common/st_sensors/st_sensors_buffer.c +++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c @@ -113,11 +113,8 @@ irqreturn_t st_sensors_trigger_handler(int irq, void *p) if (len < 0) goto st_sensors_get_buffer_element_error; - if (indio_dev->scan_timestamp) - *(s64 *)((u8 *)sdata->buffer_data + - ALIGN(len, sizeof(s64))) = pf->timestamp; - - iio_push_to_buffers(indio_dev, sdata->buffer_data); + iio_push_to_buffers_with_timestamp(indio_dev, sdata->buffer_data, + pf->timestamp); st_sensors_get_buffer_element_error: iio_trigger_notify_done(indio_dev->trig); diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c index 965ee22..7ba1ef2 100644 --- a/drivers/iio/common/st_sensors/st_sensors_core.c +++ b/drivers/iio/common/st_sensors/st_sensors_core.c @@ -198,21 +198,17 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable) } EXPORT_SYMBOL(st_sensors_set_axis_enable); -int st_sensors_init_sensor(struct iio_dev *indio_dev, - struct st_sensors_platform_data *pdata) +static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev, + struct st_sensors_platform_data *pdata) { - int err; struct st_sensor_data *sdata = iio_priv(indio_dev); - mutex_init(&sdata->tb.buf_lock); - 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"); - err = -EINVAL; - goto init_error; + return -EINVAL; } sdata->drdy_int_pin = 1; break; @@ -220,39 +216,53 @@ int st_sensors_init_sensor(struct iio_dev *indio_dev, if (sdata->sensor->drdy_irq.mask_int2 == 0) { dev_err(&indio_dev->dev, "DRDY on INT2 not available.\n"); - err = -EINVAL; - goto init_error; + return -EINVAL; } sdata->drdy_int_pin = 2; break; default: dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n"); - err = -EINVAL; - goto init_error; + 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) - goto init_error; + return err; - err = st_sensors_set_fullscale(indio_dev, - sdata->current_fullscale->num); - if (err < 0) - goto init_error; + 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) - goto init_error; + 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) - goto init_error; + return err; err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); -init_error: return err; } EXPORT_SYMBOL(st_sensors_init_sensor); @@ -263,6 +273,9 @@ int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) 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, @@ -318,10 +331,8 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev, unsigned int byte_for_channel = ch->scan_type.storagebits >> 3; outdata = kmalloc(byte_for_channel, GFP_KERNEL); - if (!outdata) { - err = -EINVAL; - goto st_sensors_read_axis_data_error; - } + if (!outdata) + return -ENOMEM; err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, ch->address, byte_for_channel, @@ -336,7 +347,7 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev, st_sensors_free_memory: kfree(outdata); -st_sensors_read_axis_data_error: + return err; } @@ -349,28 +360,25 @@ int st_sensors_read_info_raw(struct iio_dev *indio_dev, mutex_lock(&indio_dev->mlock); if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { err = -EBUSY; - goto read_error; + goto out; } else { err = st_sensors_set_enable(indio_dev, true); if (err < 0) - goto read_error; + goto out; msleep((sdata->sensor->bootime * 1000) / sdata->odr); err = st_sensors_read_axis_data(indio_dev, ch, val); if (err < 0) - goto read_error; + goto out; *val = *val >> ch->scan_type.shift; err = st_sensors_set_enable(indio_dev, false); } +out: mutex_unlock(&indio_dev->mlock); return err; - -read_error: - mutex_unlock(&indio_dev->mlock); - return err; } EXPORT_SYMBOL(st_sensors_read_info_raw); |