drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3 * Copyright (C) 2012 Invensense, Inc.
   4 */
   5
   6 #include <linux/module.h>
   7 #include <linux/slab.h>
   8 #include <linux/err.h>
   9 #include <linux/delay.h>
  10 #include <linux/sysfs.h>
  11 #include <linux/jiffies.h>
  12 #include <linux/irq.h>
  13 #include <linux/interrupt.h>
  14 #include <linux/poll.h>
  15 #include <linux/math64.h>
  16
  17 #include <linux/iio/common/inv_sensors_timestamp.h>
  18
  19 #include "inv_mpu_iio.h"
  20
  21 static int inv_reset_fifo(struct iio_dev *indio_dev)
  22 {
  23         int result;
  24         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
  25
  26         /* disable fifo and reenable it */
  27         inv_mpu6050_prepare_fifo(st, false);
  28         result = inv_mpu6050_prepare_fifo(st, true);
  29         if (result)
  30                 goto reset_fifo_fail;
  31
  32         return 0;
  33
  34 reset_fifo_fail:
  35         dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
  36         result = regmap_write(st->map, st->reg->int_enable,
  37                               INV_MPU6050_BIT_DATA_RDY_EN);
  38
  39         return result;
  40 }
  41
  42 /*
  43  * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
  44  */
  45 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
  46 {
  47         struct iio_poll_func *pf = p;
  48         struct iio_dev *indio_dev = pf->indio_dev;
  49         struct inv_mpu6050_state *st = iio_priv(indio_dev);
  50         size_t bytes_per_datum;
  51         int result;
  52         u16 fifo_count;
  53         u32 fifo_period;
  54         s64 timestamp;
  55         u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
  56         int int_status;
  57         size_t i, nb;
  58
  59         mutex_lock(&st->lock);
  60
  61         /* ack interrupt and check status */
  62         result = regmap_read(st->map, st->reg->int_status, &int_status);
  63         if (result) {
  64                 dev_err(regmap_get_device(st->map),
  65                         "failed to ack interrupt\n");
  66                 goto flush_fifo;
  67         }
  68         if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
  69                 goto end_session;
  70
  71         if (!(st->chip_config.accl_fifo_enable |
  72                 st->chip_config.gyro_fifo_enable |
  73                 st->chip_config.magn_fifo_enable))
  74                 goto end_session;
  75         bytes_per_datum = 0;
  76         if (st->chip_config.accl_fifo_enable)
  77                 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
  78
  79         if (st->chip_config.gyro_fifo_enable)
  80                 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
  81
  82         if (st->chip_config.temp_fifo_enable)
  83                 bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
  84
  85         if (st->chip_config.magn_fifo_enable)
  86                 bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
  87
  88         /*
  89          * read fifo_count register to know how many bytes are inside the FIFO
  90          * right now
  91          */
  92         result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
  93                                   st->data, INV_MPU6050_FIFO_COUNT_BYTE);
  94         if (result)
  95                 goto end_session;
  96         fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
  97
  98         /*
  99          * Handle fifo overflow by resetting fifo.
 100          * Reset if there is only 3 data set free remaining to mitigate
 101          * possible delay between reading fifo count and fifo data.
 102          */
 103         nb = 3 * bytes_per_datum;
 104         if (fifo_count >= st->hw->fifo_size - nb) {
 105                 dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
 106                 goto flush_fifo;
 107         }
 108
 109         /* compute and process only all complete datum */
 110         nb = fifo_count / bytes_per_datum;
 111         fifo_count = nb * bytes_per_datum;
 112         if (nb == 0)
 113                 goto end_session;
 114         /* Each FIFO data contains all sensors, so same number for FIFO and sensor data */
 115         fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
 116         inv_sensors_timestamp_interrupt(&st->timestamp, fifo_period, nb, nb, pf->timestamp);
 117         inv_sensors_timestamp_apply_odr(&st->timestamp, fifo_period, nb, 0);
 118
 119         /* clear internal data buffer for avoiding kernel data leak */
 120         memset(data, 0, sizeof(data));
 121
 122         /* read all data once and process every samples */
 123         result = regmap_noinc_read(st->map, st->reg->fifo_r_w, st->data, fifo_count);
 124         if (result)
 125                 goto flush_fifo;
 126         for (i = 0; i < nb; ++i) {
 127                 /* skip first samples if needed */
 128                 if (st->skip_samples) {
 129                         st->skip_samples--;
 130                         continue;
 131                 }
 132                 memcpy(data, &st->data[i * bytes_per_datum], bytes_per_datum);
 133                 timestamp = inv_sensors_timestamp_pop(&st->timestamp);
 134                 iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
 135         }
 136
 137 end_session:
 138         mutex_unlock(&st->lock);
 139         iio_trigger_notify_done(indio_dev->trig);
 140
 141         return IRQ_HANDLED;
 142
 143 flush_fifo:
 144         /* Flush HW and SW FIFOs. */
 145         inv_reset_fifo(indio_dev);
 146         mutex_unlock(&st->lock);
 147         iio_trigger_notify_done(indio_dev->trig);
 148
 149         return IRQ_HANDLED;
 150 }