diff options
Diffstat (limited to 'drivers/iio/imu-aosp/inv_mpu6050/inv_mpu_ring.c')
| -rw-r--r-- | drivers/iio/imu-aosp/inv_mpu6050/inv_mpu_ring.c | 195 |
1 files changed, 0 insertions, 195 deletions
diff --git a/drivers/iio/imu-aosp/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu-aosp/inv_mpu6050/inv_mpu_ring.c deleted file mode 100644 index 7da0832f187..00000000000 --- a/drivers/iio/imu-aosp/inv_mpu6050/inv_mpu_ring.c +++ /dev/null @@ -1,195 +0,0 @@ -/* -* Copyright (C) 2012 Invensense, Inc. -* -* This software is licensed under the terms of the GNU General Public -* License version 2, as published by the Free Software Foundation, and -* may be copied, distributed, and modified under those terms. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -*/ - -#include <linux/module.h> -#include <linux/init.h> -#include <linux/slab.h> -#include <linux/i2c.h> -#include <linux/err.h> -#include <linux/delay.h> -#include <linux/sysfs.h> -#include <linux/jiffies.h> -#include <linux/irq.h> -#include <linux/interrupt.h> -#include <linux/kfifo.h> -#include <linux/poll.h> -#include "inv_mpu_iio.h" - -int inv_reset_fifo(struct iio_dev *indio_dev) -{ - int result; - u8 d; - struct inv_mpu6050_state *st = iio_priv(indio_dev); - - /* disable interrupt */ - result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0); - if (result) { - dev_err(&st->client->dev, "int_enable failed %d\n", result); - return result; - } - /* disable the sensor output to FIFO */ - result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0); - if (result) - goto reset_fifo_fail; - /* disable fifo reading */ - result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0); - if (result) - goto reset_fifo_fail; - - /* reset FIFO*/ - result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, - INV_MPU6050_BIT_FIFO_RST); - if (result) - goto reset_fifo_fail; - /* enable interrupt */ - if (st->chip_config.accl_fifo_enable || - st->chip_config.gyro_fifo_enable) { - result = inv_mpu6050_write_reg(st, st->reg->int_enable, - INV_MPU6050_BIT_DATA_RDY_EN); - if (result) - return result; - } - /* enable FIFO reading and I2C master interface*/ - result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, - INV_MPU6050_BIT_FIFO_EN); - if (result) - goto reset_fifo_fail; - /* enable sensor output to FIFO */ - d = 0; - if (st->chip_config.gyro_fifo_enable) - d |= INV_MPU6050_BITS_GYRO_OUT; - if (st->chip_config.accl_fifo_enable) - d |= INV_MPU6050_BIT_ACCEL_OUT; - result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d); - if (result) - goto reset_fifo_fail; - - return 0; - -reset_fifo_fail: - dev_err(&st->client->dev, "reset fifo failed %d\n", result); - result = inv_mpu6050_write_reg(st, st->reg->int_enable, - INV_MPU6050_BIT_DATA_RDY_EN); - - return result; -} - -static void inv_clear_kfifo(struct inv_mpu6050_state *st) -{ - unsigned long flags; - - /* take the spin lock sem to avoid interrupt kick in */ - spin_lock_irqsave(&st->time_stamp_lock, flags); - kfifo_reset(&st->timestamps); - spin_unlock_irqrestore(&st->time_stamp_lock, flags); -} - -/** - * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt. - */ -irqreturn_t inv_mpu6050_irq_handler(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); - s64 timestamp; - - timestamp = iio_get_time_ns(); - kfifo_in_spinlocked(&st->timestamps, ×tamp, 1, - &st->time_stamp_lock); - - return IRQ_WAKE_THREAD; -} - -/** - * 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; - u64 *tmp; - - 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; - - tmp = (u64 *)data; - tmp[DIV_ROUND_UP(bytes_per_datum, 8)] = timestamp; - result = iio_push_to_buffers(indio_dev, data); - 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; -} |