static irqreturn_t adis_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct adis *adis = iio_device_get_drvdata(indio_dev);
int ret;
if (!adis->buffer)
return -ENOMEM;
if (adis->data->has_paging) {
mutex_lock(&adis->txrx_lock);
if (adis->current_page != 0) {
adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID);
adis->tx[1] = 0;
spi_write(adis->spi, adis->tx, 2);
}
}
ret = spi_sync(adis->spi, &adis->msg);
if (ret)
dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
if (adis->data->has_paging) {
adis->current_page = 0;
mutex_unlock(&adis->txrx_lock);
}
/* Guaranteed to be aligned with 8 byte boundary */
if (indio_dev->scan_timestamp) {
void *b = adis->buffer + indio_dev->scan_bytes - sizeof(s64);
*(s64 *)b = pf->timestamp;
}
iio_push_to_buffers(indio_dev, adis->buffer);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t yas_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct iio_buffer *buffer = indio_dev->buffer;
struct yas_state *st = iio_priv(indio_dev);
int len = 0, i, j;
size_t datasize = buffer->access->get_bytes_per_datum(buffer);
int32_t *mag;
struct timespec ts;
s64 timestamp;
mag = (int32_t *) kmalloc(datasize, GFP_KERNEL);
if (mag == NULL)
goto done;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
j = 0;
for (i = 0; i < 3; i++) {
if (test_bit(i, indio_dev->active_scan_mask)) {
mag[j] = st->compass_data[i];
j++;
}
}
len = j * 4;
}
/* Guaranteed to be aligned with 8 byte boundary */
//if (indio_dev->scan_timestamp)
// *(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = pf->timestamp;
ts = ktime_to_timespec(ktime_get_boottime());
timestamp = ts.tv_sec * 1000000000ULL + ts.tv_nsec;
*(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = timestamp;
if (timestamp <= 0)
pr_err("[%s] invalid time = %lld\n", __func__, timestamp);
iio_push_to_buffer(indio_dev->buffer, (u8 *)mag, 0);
kfree(mag);
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t adc108s102_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct adc108s102_state *st = iio_priv(indio_dev);
int ret;
ret = spi_sync(st->spi, &st->ring_msg);
if (ret < 0)
goto out_notify;
/* Skip the dummy response in the first slot */
iio_push_to_buffers_with_timestamp(indio_dev,
(u8 *)&st->rx_buf[1],
iio_get_time_ns(indio_dev));
out_notify:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device
* specific to be rolled into the core.
*/
static irqreturn_t ade7758_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct ade7758_state *st = iio_priv(indio_dev);
s64 dat64[2];
u32 *dat32 = (u32 *)dat64;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
if (ade7758_spi_read_burst(indio_dev) >= 0)
*dat32 = get_unaligned_be32(&st->rx_buf[5]) & 0xFFFFFF;
/* Guaranteed to be aligned with 8 byte boundary */
if (indio_dev->scan_timestamp)
dat64[1] = pf->timestamp;
iio_push_to_buffers(indio_dev, (u8 *)dat64);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t yas_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct yas_state *st = iio_priv(indio_dev);
int len = 0, i, j;
int32_t *mag;
mag = (int32_t *) kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
if (mag == NULL)
goto done;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
j = 0;
for (i = 0; i < 3; i++) {
if (test_bit(i, indio_dev->active_scan_mask)) {
mag[j] = st->compass_data[i];
j++;
}
}
len = j * 4;
}
if ((pf->timestamp - st->old_timestamp) > ((1800 /st->sampling_frequency) * 1000000)
&& (st->old_timestamp != 0)) {
if (indio_dev->scan_timestamp)
*(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = (pf->timestamp + st->old_timestamp) >> 1;
iio_push_to_buffers(indio_dev, (u8 *)mag);
}
/* Guaranteed to be aligned with 8 byte boundary */
if (indio_dev->scan_timestamp)
*(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = pf->timestamp;
iio_push_to_buffers(indio_dev, (u8 *)mag);
st->old_timestamp = pf->timestamp;
kfree(mag);
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t yas_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct iio_buffer *buffer = indio_dev->buffer;
struct yas_state *st = iio_priv(indio_dev);
int len = 0, i, j;
size_t datasize = buffer->access->get_bytes_per_datum(buffer);
int32_t *acc;
acc = (int32_t *) kmalloc(datasize, GFP_KERNEL);
if (acc == NULL) {
dev_err(indio_dev->dev.parent,
"memory alloc failed in buffer bh");
return -ENOMEM;
}
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
j = 0;
for (i = 0; i < 3; i++) {
if (test_bit(i, indio_dev->active_scan_mask)) {
acc[j] = st->compass_data[i];
j++;
}
}
len = j * 4;
}
/* Guaranteed to be aligned with 8 byte boundary */
if (buffer->scan_timestamp)
*(s64 *)(((phys_addr_t)acc + len
+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
= pf->timestamp;
buffer->access->store_to(buffer, (u8 *)acc, pf->timestamp);
iio_trigger_notify_done(indio_dev->trig);
kfree(acc);
return IRQ_HANDLED;
}
static irqreturn_t bma220_trigger_handler(int irq, void *p)
{
int ret;
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct bma220_data *data = iio_priv(indio_dev);
struct spi_device *spi = data->spi_device;
mutex_lock(&data->lock);
data->tx_buf[0] = BMA220_REG_ACCEL_X | BMA220_READ_MASK;
ret = spi_write_then_read(spi, data->tx_buf, 1, data->buffer,
ARRAY_SIZE(bma220_channels) - 1);
if (ret < 0)
goto err;
iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
pf->timestamp);
err:
mutex_unlock(&data->lock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t ade7758_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct iio_buffer *ring = indio_dev->buffer;
struct ade7758_state *st = iio_priv(indio_dev);
s64 dat64[2];
u32 *dat32 = (u32 *)dat64;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
if (ade7758_spi_read_burst(&indio_dev->dev) >= 0)
*dat32 = get_unaligned_be32(&st->rx_buf[5]) & 0xFFFFFF;
if (ring->scan_timestamp)
dat64[1] = pf->timestamp;
ring->access->store_to(ring, (u8 *)dat64, pf->timestamp);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t kxcjk1013_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct kxcjk1013_data *data = iio_priv(indio_dev);
int ret;
mutex_lock(&data->mutex);
ret = i2c_smbus_read_i2c_block_data_or_emulated(data->client,
KXCJK1013_REG_XOUT_L,
AXIS_MAX * 2,
(u8 *)data->buffer);
mutex_unlock(&data->mutex);
if (ret < 0)
goto err;
iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
data->timestamp);
err:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct iio_buffer *buffer = indio_dev->buffer;
int len = 0;
size_t datasize = buffer->access->get_bytes_per_datum(buffer);
u16 *data = kmalloc(datasize, GFP_KERNEL);
if (data == NULL)
return -ENOMEM;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
int i, j;
for (i = 0, j = 0;
i < bitmap_weight(indio_dev->active_scan_mask,
indio_dev->masklength);
i++) {
j = find_next_bit(buffer->scan_mask,
indio_dev->masklength, j + 1);
data[i] = fakedata[j];
len += 2;
}
}
if (buffer->scan_timestamp)
*(s64 *)(((phys_addr_t)data + len
+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
= iio_get_time_ns();
buffer->access->store_to(buffer, (u8 *)data, pf->timestamp);
kfree(data);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device
* specific to be rolled into the core.
*/
static void adis16209_trigger_bh_to_ring(struct work_struct *work_s)
{
struct adis16209_state *st
= container_of(work_s, struct adis16209_state,
work_trigger_to_ring);
int i = 0;
s16 *data;
size_t datasize = st->indio_dev
->ring->access.get_bpd(st->indio_dev->ring);
data = kmalloc(datasize , GFP_KERNEL);
if (data == NULL) {
dev_err(&st->us->dev, "memory alloc failed in ring bh");
return;
}
if (st->indio_dev->scan_count)
if (adis16209_read_ring_data(&st->indio_dev->dev, st->rx) >= 0)
for (; i < st->indio_dev->scan_count; i++) {
data[i] = combine_8_to_16(st->rx[i*2+1],
st->rx[i*2]);
}
/* Guaranteed to be aligned with 8 byte boundary */
if (st->indio_dev->scan_timestamp)
*((s64 *)(data + ((i + 3)/4)*4)) = st->last_timestamp;
st->indio_dev->ring->access.store_to(st->indio_dev->ring,
(u8 *)data,
st->last_timestamp);
iio_trigger_notify_done(st->indio_dev->trig);
kfree(data);
return;
}
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device
* specific to be rolled into the core.
*/
static irqreturn_t adis16203_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct adis16203_state *st = iio_priv(indio_dev);
struct iio_buffer *ring = indio_dev->buffer;
int i = 0;
s16 *data;
data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
if (data == NULL) {
dev_err(&st->us->dev, "memory alloc failed in ring bh");
goto done;
}
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
adis16203_read_ring_data(indio_dev, st->rx) >= 0)
for (; i < bitmap_weight(indio_dev->active_scan_mask,
indio_dev->masklength); i++)
data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
/* Guaranteed to be aligned with 8 byte boundary */
if (indio_dev->scan_timestamp)
*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
ring->access->store_to(ring,
(u8 *)data,
pf->timestamp);
kfree(data);
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static void adis16240_trigger_bh_to_ring(struct work_struct *work_s)
{
struct adis16240_state *st
= container_of(work_s, struct adis16240_state,
work_trigger_to_ring);
struct iio_ring_buffer *ring = st->indio_dev->ring;
int i = 0;
s16 *data;
size_t datasize = ring->access.get_bytes_per_datum(ring);
data = kmalloc(datasize , GFP_KERNEL);
if (data == NULL) {
dev_err(&st->us->dev, "memory alloc failed in ring bh");
return;
}
if (ring->scan_count)
if (adis16240_read_ring_data(&st->indio_dev->dev, st->rx) >= 0)
for (; i < ring->scan_count; i++)
data[i] = be16_to_cpup(
(__be16 *)&(st->rx[i*2]));
/* Guaranteed to be aligned with 8 byte boundary */
if (ring->scan_timestamp)
*((s64 *)(data + ((i + 3)/4)*4)) = st->last_timestamp;
ring->access.store_to(ring,
(u8 *)data,
st->last_timestamp);
iio_trigger_notify_done(st->indio_dev->trig);
kfree(data);
return;
}
/**
* inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
*/
irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
size_t bytes_per_datum;
int result;
u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
u16 fifo_count;
s64 timestamp;
mutex_lock(&indio_dev->mlock);
if (!(st->chip_config.accl_fifo_enable |
st->chip_config.gyro_fifo_enable))
goto end_session;
bytes_per_datum = 0;
if (st->chip_config.accl_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
if (st->chip_config.gyro_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
/*
* read fifo_count register to know how many bytes inside FIFO
* right now
*/
result = i2c_smbus_read_i2c_block_data(st->client,
st->reg->fifo_count_h,
INV_MPU6050_FIFO_COUNT_BYTE, data);
if (result != INV_MPU6050_FIFO_COUNT_BYTE)
goto end_session;
fifo_count = be16_to_cpup((__be16 *)(&data[0]));
if (fifo_count < bytes_per_datum)
goto end_session;
/* fifo count can't be odd number, if it is odd, reset fifo*/
if (fifo_count & 1)
goto flush_fifo;
if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
goto flush_fifo;
/* Timestamp mismatch. */
if (kfifo_len(&st->timestamps) >
fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
goto flush_fifo;
while (fifo_count >= bytes_per_datum) {
result = i2c_smbus_read_i2c_block_data(st->client,
st->reg->fifo_r_w,
bytes_per_datum, data);
if (result != bytes_per_datum)
goto flush_fifo;
result = kfifo_out(&st->timestamps, ×tamp, 1);
/* when there is no timestamp, put timestamp as 0 */
if (0 == result)
timestamp = 0;
result = iio_push_to_buffers_with_timestamp(indio_dev, data,
timestamp);
if (result)
goto flush_fifo;
fifo_count -= bytes_per_datum;
}
end_session:
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
flush_fifo:
/* Flush HW and SW FIFOs. */
inv_reset_fifo(indio_dev);
inv_clear_kfifo(st);
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t mpu3050_trigger_handler(int irq, void *p)
{
const struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct mpu3050 *mpu3050 = iio_priv(indio_dev);
int ret;
/*
* Temperature 1*16 bits
* Three axes 3*16 bits
* Timestamp 64 bits (4*16 bits)
* Sum total 8*16 bits
*/
__be16 hw_values[8];
s64 timestamp;
unsigned int datums_from_fifo = 0;
/*
* If we're using the hardware trigger, get the precise timestamp from
* the top half of the threaded IRQ handler. Otherwise get the
* timestamp here so it will be close in time to the actual values
* read from the registers.
*/
if (iio_trigger_using_own(indio_dev))
timestamp = mpu3050->hw_timestamp;
else
timestamp = iio_get_time_ns(indio_dev);
mutex_lock(&mpu3050->lock);
/* Using the hardware IRQ trigger? Check the buffer then. */
if (mpu3050->hw_irq_trigger) {
__be16 raw_fifocnt;
u16 fifocnt;
/* X, Y, Z + temperature */
unsigned int bytes_per_datum = 8;
bool fifo_overflow = false;
ret = regmap_bulk_read(mpu3050->map,
MPU3050_FIFO_COUNT_H,
&raw_fifocnt,
sizeof(raw_fifocnt));
if (ret)
goto out_trigger_unlock;
fifocnt = be16_to_cpu(raw_fifocnt);
if (fifocnt == 512) {
dev_info(mpu3050->dev,
"FIFO overflow! Emptying and resetting FIFO\n");
fifo_overflow = true;
/* Reset and enable the FIFO */
ret = regmap_update_bits(mpu3050->map,
MPU3050_USR_CTRL,
MPU3050_USR_CTRL_FIFO_EN |
MPU3050_USR_CTRL_FIFO_RST,
MPU3050_USR_CTRL_FIFO_EN |
MPU3050_USR_CTRL_FIFO_RST);
if (ret) {
dev_info(mpu3050->dev, "error resetting FIFO\n");
goto out_trigger_unlock;
}
mpu3050->pending_fifo_footer = false;
}
if (fifocnt)
dev_dbg(mpu3050->dev,
"%d bytes in the FIFO\n",
fifocnt);
while (!fifo_overflow && fifocnt > bytes_per_datum) {
unsigned int toread;
unsigned int offset;
__be16 fifo_values[5];
/*
* If there is a FIFO footer in the pipe, first clear
* that out. This follows the complex algorithm in the
* datasheet that states that you may never leave the
* FIFO empty after the first reading: you have to
* always leave two footer bytes in it. The footer is
* in practice just two zero bytes.
*/
if (mpu3050->pending_fifo_footer) {
toread = bytes_per_datum + 2;
offset = 0;
} else {
toread = bytes_per_datum;
offset = 1;
/* Put in some dummy value */
fifo_values[0] = 0xAAAA;
}
ret = regmap_bulk_read(mpu3050->map,
MPU3050_FIFO_R,
&fifo_values[offset],
toread);
dev_dbg(mpu3050->dev,
"%04x %04x %04x %04x %04x\n",
fifo_values[0],
fifo_values[1],
fifo_values[2],
fifo_values[3],
fifo_values[4]);
/* Index past the footer (fifo_values[0]) and push */
iio_push_to_buffers_with_timestamp(indio_dev,
&fifo_values[1],
timestamp);
fifocnt -= toread;
datums_from_fifo++;
mpu3050->pending_fifo_footer = true;
/*
* If we're emptying the FIFO, just make sure to
* check if something new appeared.
*/
if (fifocnt < bytes_per_datum) {
ret = regmap_bulk_read(mpu3050->map,
MPU3050_FIFO_COUNT_H,
&raw_fifocnt,
sizeof(raw_fifocnt));
if (ret)
goto out_trigger_unlock;
fifocnt = be16_to_cpu(raw_fifocnt);
}
if (fifocnt < bytes_per_datum)
dev_dbg(mpu3050->dev,
"%d bytes left in the FIFO\n",
fifocnt);
/*
* At this point, the timestamp that triggered the
* hardware interrupt is no longer valid for what
* we are reading (the interrupt likely fired for
* the value on the top of the FIFO), so set the
* timestamp to zero and let userspace deal with it.
*/
timestamp = 0;
}
}
/*
* If we picked some datums from the FIFO that's enough, else
* fall through and just read from the current value registers.
* This happens in two cases:
*
* - We are using some other trigger (external, like an HRTimer)
* than the sensor's own sample generator. In this case the
* sensor is just set to the max sampling frequency and we give
* the trigger a copy of the latest value every time we get here.
*
* - The hardware trigger is active but unused and we actually use
* another trigger which calls here with a frequency higher
* than what the device provides data. We will then just read
* duplicate values directly from the hardware registers.
*/
if (datums_from_fifo) {
dev_dbg(mpu3050->dev,
"read %d datums from the FIFO\n",
datums_from_fifo);
goto out_trigger_unlock;
}
ret = regmap_bulk_read(mpu3050->map, MPU3050_TEMP_H, &hw_values,
sizeof(hw_values));
if (ret) {
dev_err(mpu3050->dev,
"error reading axis data\n");
goto out_trigger_unlock;
}
iio_push_to_buffers_with_timestamp(indio_dev, hw_values, timestamp);
out_trigger_unlock:
mutex_unlock(&mpu3050->lock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
