LXR linux/drivers/iio/common/st_sensors/st_sensors

   1/*
   2 * STMicroelectronics sensors buffer 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/iio/iio.h>
  15#include <linux/iio/trigger.h>
  16#include <linux/interrupt.h>
  17#include <linux/iio/buffer.h>
  18#include <linux/iio/trigger_consumer.h>
  19#include <linux/iio/triggered_buffer.h>
  20#include <linux/irqreturn.h>
  21
  22#include <linux/iio/common/st_sensors.h>
  23
  24
  25int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf)
  26{
  27        int i, n = 0, len;
  28        u8 addr[ST_SENSORS_NUMBER_DATA_CHANNELS];
  29        struct st_sensor_data *sdata = iio_priv(indio_dev);
  30
  31        for (i = 0; i < ST_SENSORS_NUMBER_DATA_CHANNELS; i++) {
  32                if (test_bit(i, indio_dev->active_scan_mask)) {
  33                        addr[n] = indio_dev->channels[i].address;
  34                        n++;
  35                }
  36        }
  37        switch (n) {
  38        case 1:
  39                len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
  40                        addr[0], ST_SENSORS_BYTE_FOR_CHANNEL, buf,
  41                        sdata->multiread_bit);
  42                break;
  43        case 2:
  44                if ((addr[1] - addr[0]) == ST_SENSORS_BYTE_FOR_CHANNEL) {
  45                        len = sdata->tf->read_multiple_byte(&sdata->tb,
  46                                        sdata->dev, addr[0],
  47                                        ST_SENSORS_BYTE_FOR_CHANNEL*n,
  48                                        buf, sdata->multiread_bit);
  49                } else {
  50                        u8 rx_array[ST_SENSORS_BYTE_FOR_CHANNEL*
  51                                    ST_SENSORS_NUMBER_DATA_CHANNELS];
  52                        len = sdata->tf->read_multiple_byte(&sdata->tb,
  53                                sdata->dev, addr[0],
  54                                ST_SENSORS_BYTE_FOR_CHANNEL*
  55                                ST_SENSORS_NUMBER_DATA_CHANNELS,
  56                                rx_array, sdata->multiread_bit);
  57                        if (len < 0)
  58                                goto read_data_channels_error;
  59
  60                        for (i = 0; i < n * ST_SENSORS_NUMBER_DATA_CHANNELS;
  61                                                                        i++) {
  62                                if (i < n)
  63                                        buf[i] = rx_array[i];
  64                                else
  65                                        buf[i] = rx_array[n + i];
  66                        }
  67                        len = ST_SENSORS_BYTE_FOR_CHANNEL*n;
  68                }
  69                break;
  70        case 3:
  71                len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
  72                        addr[0], ST_SENSORS_BYTE_FOR_CHANNEL*
  73                        ST_SENSORS_NUMBER_DATA_CHANNELS,
  74                        buf, sdata->multiread_bit);
  75                break;
  76        default:
  77                len = -EINVAL;
  78                goto read_data_channels_error;
  79        }
  80        if (len != ST_SENSORS_BYTE_FOR_CHANNEL*n) {
  81                len = -EIO;
  82                goto read_data_channels_error;
  83        }
  84
  85read_data_channels_error:
  86        return len;
  87}
  88EXPORT_SYMBOL(st_sensors_get_buffer_element);
  89
  90irqreturn_t st_sensors_trigger_handler(int irq, void *p)
  91{
  92        int len;
  93        struct iio_poll_func *pf = p;
  94        struct iio_dev *indio_dev = pf->indio_dev;
  95        struct st_sensor_data *sdata = iio_priv(indio_dev);
  96
  97        len = st_sensors_get_buffer_element(indio_dev, sdata->buffer_data);
  98        if (len < 0)
  99                goto st_sensors_get_buffer_element_error;
 100
 101        if (indio_dev->scan_timestamp)
 102                *(s64 *)((u8 *)sdata->buffer_data +
 103                                ALIGN(len, sizeof(s64))) = pf->timestamp;
 104
 105        iio_push_to_buffers(indio_dev, sdata->buffer_data);
 106
 107st_sensors_get_buffer_element_error:
 108        iio_trigger_notify_done(indio_dev->trig);
 109
 110        return IRQ_HANDLED;
 111}
 112EXPORT_SYMBOL(st_sensors_trigger_handler);
 113
 114MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
 115MODULE_DESCRIPTION("STMicroelectronics ST-sensors buffer");
 116MODULE_LICENSE("GPL v2");
 117