diff options
Diffstat (limited to 'drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c')
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 196 |
1 files changed, 196 insertions, 0 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c new file mode 100644 index 000000000000..331781ffbb15 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c @@ -0,0 +1,196 @@ +/* +* 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(); + spin_lock(&st->time_stamp_lock); + kfifo_in(&st->timestamps, ×tamp, 1); + spin_unlock(&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; +} |