diff options
Diffstat (limited to 'drivers/iio/common/st_sensors/st_sensors_buffer.c')
-rw-r--r-- | drivers/iio/common/st_sensors/st_sensors_buffer.c | 97 |
1 files changed, 32 insertions, 65 deletions
diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c index e18bc6782256..c55898543a47 100644 --- a/drivers/iio/common/st_sensors/st_sensors_buffer.c +++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c @@ -24,81 +24,30 @@ int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf) { - u8 *addr; - int i, n = 0, len; + int i, len; + int total = 0; struct st_sensor_data *sdata = iio_priv(indio_dev); unsigned int num_data_channels = sdata->num_data_channels; - unsigned int byte_for_channel = - indio_dev->channels[0].scan_type.storagebits >> 3; - - addr = kmalloc(num_data_channels, GFP_KERNEL); - if (!addr) { - len = -ENOMEM; - goto st_sensors_get_buffer_element_error; - } for (i = 0; i < num_data_channels; i++) { + unsigned int bytes_to_read; + if (test_bit(i, indio_dev->active_scan_mask)) { - addr[n] = indio_dev->channels[i].address; - n++; - } - } - switch (n) { - case 1: - len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, - addr[0], byte_for_channel, buf, sdata->multiread_bit); - break; - case 2: - if ((addr[1] - addr[0]) == byte_for_channel) { + bytes_to_read = indio_dev->channels[i].scan_type.storagebits >> 3; len = sdata->tf->read_multiple_byte(&sdata->tb, - sdata->dev, addr[0], byte_for_channel * n, - buf, sdata->multiread_bit); - } else { - u8 *rx_array; - rx_array = kmalloc(byte_for_channel * num_data_channels, - GFP_KERNEL); - if (!rx_array) { - len = -ENOMEM; - goto st_sensors_free_memory; - } + sdata->dev, indio_dev->channels[i].address, + bytes_to_read, + buf + total, sdata->multiread_bit); - len = sdata->tf->read_multiple_byte(&sdata->tb, - sdata->dev, addr[0], - byte_for_channel * num_data_channels, - rx_array, sdata->multiread_bit); - if (len < 0) { - kfree(rx_array); - goto st_sensors_free_memory; - } - - for (i = 0; i < n * byte_for_channel; i++) { - if (i < n) - buf[i] = rx_array[i]; - else - buf[i] = rx_array[n + i]; - } - kfree(rx_array); - len = byte_for_channel * n; + if (len < bytes_to_read) + return -EIO; + + /* Advance the buffer pointer */ + total += len; } - break; - case 3: - len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, - addr[0], byte_for_channel * num_data_channels, - buf, sdata->multiread_bit); - break; - default: - len = -EINVAL; - goto st_sensors_free_memory; - } - if (len != byte_for_channel * n) { - len = -EIO; - goto st_sensors_free_memory; } -st_sensors_free_memory: - kfree(addr); -st_sensors_get_buffer_element_error: - return len; + return total; } EXPORT_SYMBOL(st_sensors_get_buffer_element); @@ -109,6 +58,24 @@ irqreturn_t st_sensors_trigger_handler(int irq, void *p) struct iio_dev *indio_dev = pf->indio_dev; struct st_sensor_data *sdata = iio_priv(indio_dev); + /* If we have a status register, check if this IRQ came from us */ + if (sdata->sensor_settings->drdy_irq.addr_stat_drdy) { + u8 status; + + len = sdata->tf->read_byte(&sdata->tb, sdata->dev, + sdata->sensor_settings->drdy_irq.addr_stat_drdy, + &status); + if (len < 0) + dev_err(sdata->dev, "could not read channel status\n"); + + /* + * If this was not caused by any channels on this sensor, + * return IRQ_NONE + */ + if (!(status & (u8)indio_dev->active_scan_mask[0])) + return IRQ_NONE; + } + len = st_sensors_get_buffer_element(indio_dev, sdata->buffer_data); if (len < 0) goto st_sensors_get_buffer_element_error; |