123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca/* 223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca * STMicroelectronics sensors buffer library driver 323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca * 423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca * Copyright 2012-2013 STMicroelectronics Inc. 523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca * 623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca * Denis Ciocca <denis.ciocca@st.com> 723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca * 823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca * Licensed under the GPL-2. 923491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca */ 1023491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 1123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/kernel.h> 1223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/module.h> 1323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/slab.h> 1423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/iio/iio.h> 1523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/iio/trigger.h> 1623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/interrupt.h> 1723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/iio/buffer.h> 1823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/iio/trigger_consumer.h> 1923491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/iio/triggered_buffer.h> 2023491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/irqreturn.h> 2123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 2223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca#include <linux/iio/common/st_sensors.h> 2323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 2423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 2523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Cioccaint st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf) 2623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca{ 27607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA u8 *addr; 2823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca int i, n = 0, len; 2923491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca struct st_sensor_data *sdata = iio_priv(indio_dev); 30607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA unsigned int num_data_channels = sdata->num_data_channels; 31607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA unsigned int byte_for_channel = 32607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA indio_dev->channels[0].scan_type.storagebits >> 3; 3323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 34607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA addr = kmalloc(num_data_channels, GFP_KERNEL); 35607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA if (!addr) { 36607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA len = -ENOMEM; 37607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA goto st_sensors_get_buffer_element_error; 38607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA } 39607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA 40607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA for (i = 0; i < num_data_channels; i++) { 4123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca if (test_bit(i, indio_dev->active_scan_mask)) { 4223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca addr[n] = indio_dev->channels[i].address; 4323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca n++; 4423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca } 4523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca } 4623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca switch (n) { 4723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca case 1: 4823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, 49607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA addr[0], byte_for_channel, buf, sdata->multiread_bit); 5023491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca break; 5123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca case 2: 52607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA if ((addr[1] - addr[0]) == byte_for_channel) { 5323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca len = sdata->tf->read_multiple_byte(&sdata->tb, 54607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA sdata->dev, addr[0], byte_for_channel * n, 55607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA buf, sdata->multiread_bit); 5623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca } else { 57607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA u8 *rx_array; 58607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA rx_array = kmalloc(byte_for_channel * num_data_channels, 59607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA GFP_KERNEL); 60607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA if (!rx_array) { 61607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA len = -ENOMEM; 62607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA goto st_sensors_free_memory; 63607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA } 64607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA 6523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca len = sdata->tf->read_multiple_byte(&sdata->tb, 6623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca sdata->dev, addr[0], 67607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA byte_for_channel * num_data_channels, 6823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca rx_array, sdata->multiread_bit); 69607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA if (len < 0) { 70607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA kfree(rx_array); 71607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA goto st_sensors_free_memory; 72607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA } 7323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 744250c90b30b8bf2a1a21122ba0484f8f351f152dRobin van der Gracht for (i = 0; i < n * byte_for_channel; i++) { 7523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca if (i < n) 7623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca buf[i] = rx_array[i]; 7723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca else 7823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca buf[i] = rx_array[n + i]; 7923491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca } 80607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA kfree(rx_array); 81607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA len = byte_for_channel * n; 8223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca } 8323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca break; 8423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca case 3: 8523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, 86607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA addr[0], byte_for_channel * num_data_channels, 8723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca buf, sdata->multiread_bit); 8823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca break; 8923491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca default: 9023491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca len = -EINVAL; 91607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA goto st_sensors_free_memory; 9223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca } 93607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA if (len != byte_for_channel * n) { 9423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca len = -EIO; 95607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA goto st_sensors_free_memory; 9623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca } 9723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 98607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCAst_sensors_free_memory: 99607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCA kfree(addr); 100607a568ab69c5ac345a286267a27294888f8bb5fDenis CIOCCAst_sensors_get_buffer_element_error: 10123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca return len; 10223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca} 10323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis CioccaEXPORT_SYMBOL(st_sensors_get_buffer_element); 10423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 10523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Cioccairqreturn_t st_sensors_trigger_handler(int irq, void *p) 10623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca{ 10723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca int len; 10823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca struct iio_poll_func *pf = p; 10923491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca struct iio_dev *indio_dev = pf->indio_dev; 11023491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca struct st_sensor_data *sdata = iio_priv(indio_dev); 11123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 11223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca len = st_sensors_get_buffer_element(indio_dev, sdata->buffer_data); 11323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca if (len < 0) 11423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca goto st_sensors_get_buffer_element_error; 11523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 116aa4e24279a6b3caa40def6fe81a01772f8cade7eLars-Peter Clausen iio_push_to_buffers_with_timestamp(indio_dev, sdata->buffer_data, 117aa4e24279a6b3caa40def6fe81a01772f8cade7eLars-Peter Clausen pf->timestamp); 11823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 11923491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Cioccast_sensors_get_buffer_element_error: 12023491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca iio_trigger_notify_done(indio_dev->trig); 12123491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 12223491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca return IRQ_HANDLED; 12323491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca} 12423491b513bcd3dfe4ddb94547d73d9deb94eda44Denis CioccaEXPORT_SYMBOL(st_sensors_trigger_handler); 12523491b513bcd3dfe4ddb94547d73d9deb94eda44Denis Ciocca 12623491b513bcd3dfe4ddb94547d73d9deb94eda44Denis CioccaMODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); 12723491b513bcd3dfe4ddb94547d73d9deb94eda44Denis CioccaMODULE_DESCRIPTION("STMicroelectronics ST-sensors buffer"); 12823491b513bcd3dfe4ddb94547d73d9deb94eda44Denis CioccaMODULE_LICENSE("GPL v2"); 129