diff options
Diffstat (limited to 'drivers/iio/common')
-rw-r--r-- | drivers/iio/common/st_sensors/st_sensors_buffer.c | 63 |
1 files changed, 13 insertions, 50 deletions
diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c index 73764961feac..2ce0d2a3f855 100644 --- a/drivers/iio/common/st_sensors/st_sensors_buffer.c +++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c @@ -24,67 +24,30 @@ int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf) { - u8 addr[3]; /* no ST sensor has more than 3 channels */ - 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; 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) - return -ENOMEM; + 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); - return len; - } + if (len < bytes_to_read) + return -EIO; - 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; + /* 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: - return -EINVAL; } - if (len != byte_for_channel * n) - return -EIO; - return len; + return total; } EXPORT_SYMBOL(st_sensors_get_buffer_element); |