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 128 mutex_lock(&indio_dev->mlock); 129 if (!(st->chip_config.accl_fifo_enable | 130 st->chip_config.gyro_fifo_enable)) 131 goto end_session; 132 bytes_per_datum = 0; 133 if (st->chip_config.accl_fifo_enable) 134 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 135 136 if (st->chip_config.gyro_fifo_enable) 137 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 138 139 /* 140 * read fifo_count register to know how many bytes inside FIFO 141 * right now 142 */ 143 result = i2c_smbus_read_i2c_block_data(st->client, 144 st->reg->fifo_count_h, 145 INV_MPU6050_FIFO_COUNT_BYTE, data); 146 if (result != INV_MPU6050_FIFO_COUNT_BYTE) 147 goto end_session; 148 fifo_count = be16_to_cpup((__be16 *)(&data[0])); 149 if (fifo_count < bytes_per_datum) 150 goto end_session; 151 /* fifo count can't be odd number, if it is odd, reset fifo*/ 152 if (fifo_count & 1) 153 goto flush_fifo; 154 if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) 155 goto flush_fifo; 156 /* Timestamp mismatch. */ 157 if (kfifo_len(&st->timestamps) > 158 fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR) 159 goto flush_fifo; 160 while (fifo_count >= bytes_per_datum) { 161 result = i2c_smbus_read_i2c_block_data(st->client, 162 st->reg->fifo_r_w, 163 bytes_per_datum, data); 164 if (result != bytes_per_datum) 165 goto flush_fifo; 166 167 result = kfifo_out(&st->timestamps, ×tamp, 1); 168 /* when there is no timestamp, put timestamp as 0 */ 169 if (0 == result) 170 timestamp = 0; 171 172 result = iio_push_to_buffers_with_timestamp(indio_dev, data, 173 timestamp); 174 if (result) 175 goto flush_fifo; 176 fifo_count -= bytes_per_datum; 177 } 178 179 end_session: 180 mutex_unlock(&indio_dev->mlock); 181 iio_trigger_notify_done(indio_dev->trig); 182 183 return IRQ_HANDLED; 184 185 flush_fifo: 186 /* Flush HW and SW FIFOs. */ 187 inv_reset_fifo(indio_dev); 188 inv_clear_kfifo(st); 189 mutex_unlock(&indio_dev->mlock); 190 iio_trigger_notify_done(indio_dev->trig); 191 192 return IRQ_HANDLED; 193 } 194