138 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*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 21static 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 34reset_fifo_fail:35 dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);36 return regmap_update_bits(st->map, st->reg->int_enable,37 INV_MPU6050_BIT_DATA_RDY_EN, INV_MPU6050_BIT_DATA_RDY_EN);38}39 40/*41 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.42 */43irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)44{45 struct iio_poll_func *pf = p;46 struct iio_dev *indio_dev = pf->indio_dev;47 struct inv_mpu6050_state *st = iio_priv(indio_dev);48 size_t bytes_per_datum;49 int result;50 u16 fifo_count;51 u32 fifo_period;52 s64 timestamp;53 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];54 size_t i, nb;55 56 mutex_lock(&st->lock);57 58 if (!(st->chip_config.accl_fifo_enable |59 st->chip_config.gyro_fifo_enable |60 st->chip_config.magn_fifo_enable))61 goto end_session;62 bytes_per_datum = 0;63 if (st->chip_config.accl_fifo_enable)64 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;65 66 if (st->chip_config.gyro_fifo_enable)67 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;68 69 if (st->chip_config.temp_fifo_enable)70 bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;71 72 if (st->chip_config.magn_fifo_enable)73 bytes_per_datum += INV_MPU9X50_BYTES_MAGN;74 75 /*76 * read fifo_count register to know how many bytes are inside the FIFO77 * right now78 */79 result = regmap_bulk_read(st->map, st->reg->fifo_count_h,80 st->data, INV_MPU6050_FIFO_COUNT_BYTE);81 if (result)82 goto end_session;83 fifo_count = be16_to_cpup((__be16 *)&st->data[0]);84 85 /*86 * Handle fifo overflow by resetting fifo.87 * Reset if there is only 3 data set free remaining to mitigate88 * possible delay between reading fifo count and fifo data.89 */90 nb = 3 * bytes_per_datum;91 if (fifo_count >= st->hw->fifo_size - nb) {92 dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");93 goto flush_fifo;94 }95 96 /* compute and process only all complete datum */97 nb = fifo_count / bytes_per_datum;98 fifo_count = nb * bytes_per_datum;99 if (nb == 0)100 goto end_session;101 /* Each FIFO data contains all sensors, so same number for FIFO and sensor data */102 fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);103 inv_sensors_timestamp_interrupt(&st->timestamp, 1, pf->timestamp);104 inv_sensors_timestamp_apply_odr(&st->timestamp, fifo_period, 1, 0);105 106 /* clear internal data buffer for avoiding kernel data leak */107 memset(data, 0, sizeof(data));108 109 /* read all data once and process every samples */110 result = regmap_noinc_read(st->map, st->reg->fifo_r_w, st->data, fifo_count);111 if (result)112 goto flush_fifo;113 for (i = 0; i < nb; ++i) {114 /* skip first samples if needed */115 if (st->skip_samples) {116 st->skip_samples--;117 continue;118 }119 memcpy(data, &st->data[i * bytes_per_datum], bytes_per_datum);120 timestamp = inv_sensors_timestamp_pop(&st->timestamp);121 iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);122 }123 124end_session:125 mutex_unlock(&st->lock);126 iio_trigger_notify_done(indio_dev->trig);127 128 return IRQ_HANDLED;129 130flush_fifo:131 /* Flush HW and SW FIFOs. */132 inv_reset_fifo(indio_dev);133 mutex_unlock(&st->lock);134 iio_trigger_notify_done(indio_dev->trig);135 136 return IRQ_HANDLED;137}138