drivers/iio/common/st_sensors/st_sensors_buffer.c

   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
  25 int 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
  98 st_sensors_free_memory:
  99         kfree(addr);
 100 st_sensors_get_buffer_element_error:
 101         return len;
 102 }
 103 EXPORT_SYMBOL(st_sensors_get_buffer_element);
 104
 105 irqreturn_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
 122 st_sensors_get_buffer_element_error:
 123         iio_trigger_notify_done(indio_dev->trig);
 124
 125         return IRQ_HANDLED;
 126 }
 127 EXPORT_SYMBOL(st_sensors_trigger_handler);
 128
 129 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
 130 MODULE_DESCRIPTION("STMicroelectronics ST-sensors buffer");
 131 MODULE_LICENSE("GPL v2");