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        u8 *addr;
  28        int i, n = 0, len;
  29        struct st_sensor_data *sdata = iio_priv(indio_dev);
  30        unsigned int num_data_channels = sdata->num_data_channels;
  31        unsigned int byte_for_channel =
  32                        indio_dev->channels[0].scan_type.storagebits >> 3;
  33
  34        addr = kmalloc(num_data_channels, GFP_KERNEL);
  35        if (!addr) {
  36                len = -ENOMEM;
  37                goto st_sensors_get_buffer_element_error;
  38        }
  39
  40        for (i = 0; i < num_data_channels; i++) {
  41                if (test_bit(i, indio_dev->active_scan_mask)) {
  42                        addr[n] = indio_dev->channels[i].address;
  43                        n++;
  44                }
  45        }
  46        switch (n) {
  47        case 1:
  48                len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
  49                        addr[0], byte_for_channel, buf, sdata->multiread_bit);
  50                break;
  51        case 2:
  52                if ((addr[1] - addr[0]) == byte_for_channel) {
  53                        len = sdata->tf->read_multiple_byte(&sdata->tb,
  54                                sdata->dev, addr[0], byte_for_channel * n,
  55                                buf, sdata->multiread_bit);
  56                } else {
  57                        u8 *rx_array;
  58                        rx_array = kmalloc(byte_for_channel * num_data_channels,
  59                                           GFP_KERNEL);
  60                        if (!rx_array) {
  61                                len = -ENOMEM;
  62                                goto st_sensors_free_memory;
  63                        }
  64
  65                        len = sdata->tf->read_multiple_byte(&sdata->tb,
  66                                sdata->dev, addr[0],
  67                                byte_for_channel * num_data_channels,
  68                                rx_array, sdata->multiread_bit);
  69                        if (len < 0) {
  70                                kfree(rx_array);
  71                                goto st_sensors_free_memory;
  72                        }
  73
  74                        for (i = 0; i < n * num_data_channels; i++) {
  75                                if (i < n)
  76                                        buf[i] = rx_array[i];
  77                                else
  78                                        buf[i] = rx_array[n + i];
  79                        }
  80                        kfree(rx_array);
  81                        len = byte_for_channel * n;
  82                }
  83                break;
  84        case 3:
  85                len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
  86                        addr[0], byte_for_channel * num_data_channels,
  87                        buf, sdata->multiread_bit);
  88                break;
  89        default:
  90                len = -EINVAL;
  91                goto st_sensors_free_memory;
  92        }
  93        if (len != byte_for_channel * n) {
  94                len = -EIO;
  95                goto st_sensors_free_memory;
  96        }
  97
  98st_sensors_free_memory:
  99        kfree(addr);
 100st_sensors_get_buffer_element_error:
 101        return len;
 102}
 103EXPORT_SYMBOL(st_sensors_get_buffer_element);
 104
 105irqreturn_t st_sensors_trigger_handler(int irq, void *p)
 106{
 107        int len;
 108        struct iio_poll_func *pf = p;
 109        struct iio_dev *indio_dev = pf->indio_dev;
 110        struct st_sensor_data *sdata = iio_priv(indio_dev);
 111
 112        len = st_sensors_get_buffer_element(indio_dev, sdata->buffer_data);
 113        if (len < 0)
 114                goto st_sensors_get_buffer_element_error;
 115
 116        if (indio_dev->scan_timestamp)
 117                *(s64 *)((u8 *)sdata->buffer_data +
 118                                ALIGN(len, sizeof(s64))) = pf->timestamp;
 119
 120        iio_push_to_buffers(indio_dev, sdata->buffer_data);
 121
 122st_sensors_get_buffer_element_error:
 123        iio_trigger_notify_done(indio_dev->trig);
 124
 125        return IRQ_HANDLED;
 126}
 127EXPORT_SYMBOL(st_sensors_trigger_handler);
 128
 129MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
 130MODULE_DESCRIPTION("STMicroelectronics ST-sensors buffer");
 131MODULE_LICENSE("GPL v2");
 132