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 * byte_for_channel; 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	iio_push_to_buffers_with_timestamp(indio_dev, sdata->buffer_data,
117		pf->timestamp);
118
119st_sensors_get_buffer_element_error:
120	iio_trigger_notify_done(indio_dev->trig);
121
122	return IRQ_HANDLED;
123}
124EXPORT_SYMBOL(st_sensors_trigger_handler);
125
126MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
127MODULE_DESCRIPTION("STMicroelectronics ST-sensors buffer");
128MODULE_LICENSE("GPL v2");
129