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