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