in gyro/mpu3050-core.c [468:648]
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;
struct {
__be16 chans[4];
s64 timestamp __aligned(8);
} scan;
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] = cpu_to_be16(0xAAAA);
}
ret = regmap_bulk_read(mpu3050->map,
MPU3050_FIFO_R,
&fifo_values[offset],
toread);
if (ret)
goto out_trigger_unlock;
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_ts_unaligned(indio_dev,
&fifo_values[1],
sizeof(__be16) * 4,
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, scan.chans,
sizeof(scan.chans));
if (ret) {
dev_err(mpu3050->dev,
"error reading axis data\n");
goto out_trigger_unlock;
}
iio_push_to_buffers_with_timestamp(indio_dev, &scan, timestamp);
out_trigger_unlock:
mutex_unlock(&mpu3050->lock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}