Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 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 | { |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 27 | u8 *addr; |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 28 | int i, n = 0, len; |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 29 | struct st_sensor_data *sdata = iio_priv(indio_dev); |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 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; |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 33 | |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 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++) { |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 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, |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 49 | addr[0], byte_for_channel, buf, sdata->multiread_bit); |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 50 | break; |
| 51 | case 2: |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 52 | if ((addr[1] - addr[0]) == byte_for_channel) { |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 53 | len = sdata->tf->read_multiple_byte(&sdata->tb, |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 54 | sdata->dev, addr[0], byte_for_channel * n, |
| 55 | buf, sdata->multiread_bit); |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 56 | } else { |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 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 | |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 65 | len = sdata->tf->read_multiple_byte(&sdata->tb, |
| 66 | sdata->dev, addr[0], |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 67 | byte_for_channel * num_data_channels, |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 68 | rx_array, sdata->multiread_bit); |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 69 | if (len < 0) { |
| 70 | kfree(rx_array); |
| 71 | goto st_sensors_free_memory; |
| 72 | } |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 73 | |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 74 | for (i = 0; i < n * num_data_channels; i++) { |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 75 | if (i < n) |
| 76 | buf[i] = rx_array[i]; |
| 77 | else |
| 78 | buf[i] = rx_array[n + i]; |
| 79 | } |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 80 | kfree(rx_array); |
| 81 | len = byte_for_channel * n; |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 82 | } |
| 83 | break; |
| 84 | case 3: |
| 85 | len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 86 | addr[0], byte_for_channel * num_data_channels, |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 87 | buf, sdata->multiread_bit); |
| 88 | break; |
| 89 | default: |
| 90 | len = -EINVAL; |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 91 | goto st_sensors_free_memory; |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 92 | } |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 93 | if (len != byte_for_channel * n) { |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 94 | len = -EIO; |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 95 | goto st_sensors_free_memory; |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 96 | } |
| 97 | |
Denis CIOCCA | 607a568 | 2013-06-03 15:58:00 +0100 | [diff] [blame] | 98 | st_sensors_free_memory: |
| 99 | kfree(addr); |
| 100 | st_sensors_get_buffer_element_error: |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 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 | |
Lars-Peter Clausen | aa4e242 | 2013-09-19 13:59:00 +0100 | [diff] [blame] | 116 | iio_push_to_buffers_with_timestamp(indio_dev, sdata->buffer_data, |
| 117 | pf->timestamp); |
Denis Ciocca | 23491b5 | 2013-01-25 23:44:00 +0000 | [diff] [blame] | 118 | |
| 119 | st_sensors_get_buffer_element_error: |
| 120 | iio_trigger_notify_done(indio_dev->trig); |
| 121 | |
| 122 | return IRQ_HANDLED; |
| 123 | } |
| 124 | EXPORT_SYMBOL(st_sensors_trigger_handler); |
| 125 | |
| 126 | MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); |
| 127 | MODULE_DESCRIPTION("STMicroelectronics ST-sensors buffer"); |
| 128 | MODULE_LICENSE("GPL v2"); |