1 /* 2 * Copyright (C) 2012 Invensense, Inc. 3 * 4 * This software is licensed under the terms of the GNU General Public 5 * License version 2, as published by the Free Software Foundation, and 6 * may be copied, distributed, and modified under those terms. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 */ 13 14 #include <linux/module.h> 15 #include <linux/slab.h> 16 #include <linux/i2c.h> 17 #include <linux/err.h> 18 #include <linux/delay.h> 19 #include <linux/sysfs.h> 20 #include <linux/jiffies.h> 21 #include <linux/irq.h> 22 #include <linux/interrupt.h> 23 #include <linux/kfifo.h> 24 #include <linux/poll.h> 25 #include "inv_mpu_iio.h" 26 27 int inv_reset_fifo(struct iio_dev *indio_dev) 28 { 29 int result; 30 u8 d; 31 struct inv_mpu6050_state *st = iio_priv(indio_dev); 32 33 /* disable interrupt */ 34 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0); 35 if (result) { 36 dev_err(&st->client->dev, "int_enable failed %d\n", result); 37 return result; 38 } 39 /* disable the sensor output to FIFO */ 40 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0); 41 if (result) 42 goto reset_fifo_fail; 43 /* disable fifo reading */ 44 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0); 45 if (result) 46 goto reset_fifo_fail; 47 48 /* reset FIFO*/ 49 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 50 INV_MPU6050_BIT_FIFO_RST); 51 if (result) 52 goto reset_fifo_fail; 53 /* enable interrupt */ 54 if (st->chip_config.accl_fifo_enable || 55 st->chip_config.gyro_fifo_enable) { 56 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 57 INV_MPU6050_BIT_DATA_RDY_EN); 58 if (result) 59 return result; 60 } 61 /* enable FIFO reading and I2C master interface*/ 62 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 63 INV_MPU6050_BIT_FIFO_EN); 64 if (result) 65 goto reset_fifo_fail; 66 /* enable sensor output to FIFO */ 67 d = 0; 68 if (st->chip_config.gyro_fifo_enable) 69 d |= INV_MPU6050_BITS_GYRO_OUT; 70 if (st->chip_config.accl_fifo_enable) 71 d |= INV_MPU6050_BIT_ACCEL_OUT; 72 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d); 73 if (result) 74 goto reset_fifo_fail; 75 76 return 0; 77 78 reset_fifo_fail: 79 dev_err(&st->client->dev, "reset fifo failed %d\n", result); 80 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 81 INV_MPU6050_BIT_DATA_RDY_EN); 82 83 return result; 84 } 85 86 static void inv_clear_kfifo(struct inv_mpu6050_state *st) 87 { 88 unsigned long flags; 89 90 /* take the spin lock sem to avoid interrupt kick in */ 91 spin_lock_irqsave(&st->time_stamp_lock, flags); 92 kfifo_reset(&st->timestamps); 93 spin_unlock_irqrestore(&st->time_stamp_lock, flags); 94 } 95 96 /** 97 * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt. 98 */ 99 irqreturn_t inv_mpu6050_irq_handler(int irq, void *p) 100 { 101 struct iio_poll_func *pf = p; 102 struct iio_dev *indio_dev = pf->indio_dev; 103 struct inv_mpu6050_state *st = iio_priv(indio_dev); 104 s64 timestamp; 105 106 timestamp = iio_get_time_ns(); 107 kfifo_in_spinlocked(&st->timestamps, ×tamp, 1, 108 &st->time_stamp_lock); 109 110 return IRQ_WAKE_THREAD; 111 } 112 113 /** 114 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO. 115 */ 116 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) 117 { 118 struct iio_poll_func *pf = p; 119 struct iio_dev *indio_dev = pf->indio_dev; 120 struct inv_mpu6050_state *st = iio_priv(indio_dev); 121 size_t bytes_per_datum; 122 int result; 123 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE]; 124 u16 fifo_count; 125 s64 timestamp; 126 127 mutex_lock(&indio_dev->mlock); 128 if (!(st->chip_config.accl_fifo_enable | 129 st->chip_config.gyro_fifo_enable)) 130 goto end_session; 131 bytes_per_datum = 0; 132 if (st->chip_config.accl_fifo_enable) 133 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 134 135 if (st->chip_config.gyro_fifo_enable) 136 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 137 138 /* 139 * read fifo_count register to know how many bytes inside FIFO 140 * right now 141 */ 142 result = i2c_smbus_read_i2c_block_data(st->client, 143 st->reg->fifo_count_h, 144 INV_MPU6050_FIFO_COUNT_BYTE, data); 145 if (result != INV_MPU6050_FIFO_COUNT_BYTE) 146 goto end_session; 147 fifo_count = be16_to_cpup((__be16 *)(&data[0])); 148 if (fifo_count < bytes_per_datum) 149 goto end_session; 150 /* fifo count can't be odd number, if it is odd, reset fifo*/ 151 if (fifo_count & 1) 152 goto flush_fifo; 153 if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) 154 goto flush_fifo; 155 /* Timestamp mismatch. */ 156 if (kfifo_len(&st->timestamps) > 157 fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR) 158 goto flush_fifo; 159 while (fifo_count >= bytes_per_datum) { 160 result = i2c_smbus_read_i2c_block_data(st->client, 161 st->reg->fifo_r_w, 162 bytes_per_datum, data); 163 if (result != bytes_per_datum) 164 goto flush_fifo; 165 166 result = kfifo_out(&st->timestamps, ×tamp, 1); 167 /* when there is no timestamp, put timestamp as 0 */ 168 if (0 == result) 169 timestamp = 0; 170 171 result = iio_push_to_buffers_with_timestamp(indio_dev, data, 172 timestamp); 173 if (result) 174 goto flush_fifo; 175 fifo_count -= bytes_per_datum; 176 } 177 178 end_session: 179 mutex_unlock(&indio_dev->mlock); 180 iio_trigger_notify_done(indio_dev->trig); 181 182 return IRQ_HANDLED; 183 184 flush_fifo: 185 /* Flush HW and SW FIFOs. */ 186 inv_reset_fifo(indio_dev); 187 inv_clear_kfifo(st); 188 mutex_unlock(&indio_dev->mlock); 189 iio_trigger_notify_done(indio_dev->trig); 190 191 return IRQ_HANDLED; 192 } 193