2 * Copyright (C) 2012 Invensense, Inc.
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.
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.
17 * @brief Hardware drivers.
20 * @file inv_mpu_ring.c
21 * @brief A sysfs device driver for Invensense gyroscopes.
22 * @details This file is part of inv mpu iio driver code
25 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
27 #include <linux/module.h>
28 #include <linux/init.h>
29 #include <linux/slab.h>
30 #include <linux/i2c.h>
31 #include <linux/err.h>
32 #include <linux/delay.h>
33 #include <linux/sysfs.h>
34 #include <linux/jiffies.h>
35 #include <linux/irq.h>
36 #include <linux/interrupt.h>
37 #include <linux/kfifo.h>
38 #include <linux/poll.h>
39 #include <linux/miscdevice.h>
40 #ifdef INV_KERNEL_3_10
41 #include <linux/iio/iio.h>
42 #include <linux/iio/kfifo_buf.h>
43 #include <linux/iio/trigger_consumer.h>
44 #include <linux/iio/sysfs.h>
47 #include "kfifo_buf.h"
48 #include "trigger_consumer.h"
52 #include "inv_mpu_iio.h"
55 * reset_fifo_mpu3050() - Reset FIFO related registers
56 * @st: Device driver instance.
58 static int reset_fifo_mpu3050(struct iio_dev *indio_dev)
60 struct inv_reg_map_s *reg;
63 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
66 /* disable interrupt */
67 result = inv_plat_single_write(st, reg->int_enable,
68 st->plat_data.int_config);
71 /* disable the sensor output to FIFO */
72 result = inv_plat_single_write(st, reg->fifo_en, 0);
75 result = inv_plat_read(st, reg->user_ctrl, 1, &user_ctrl);
78 /* disable fifo reading */
79 user_ctrl &= ~BIT_FIFO_EN;
80 st->chip_config.has_footer = 0;
82 val = (BIT_3050_FIFO_RST | user_ctrl);
83 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
86 st->last_isr_time = get_time_ns();
87 if (st->chip_config.dmp_on) {
88 /* enable interrupt when DMP is done */
89 result = inv_plat_single_write(st, reg->int_enable,
90 st->plat_data.int_config | BIT_DMP_INT_EN);
94 result = inv_plat_single_write(st, reg->user_ctrl,
95 BIT_FIFO_EN | user_ctrl | st->i2c_dis);
99 /* enable interrupt */
100 if (st->chip_config.accl_fifo_enable ||
101 st->chip_config.gyro_fifo_enable) {
102 result = inv_plat_single_write(st, reg->int_enable,
103 st->plat_data.int_config | BIT_DATA_RDY_EN);
107 /* enable FIFO reading and I2C master interface*/
108 result = inv_plat_single_write(st, reg->user_ctrl,
109 BIT_FIFO_EN | user_ctrl | st->i2c_dis);
112 /* enable sensor output to FIFO and FIFO footer*/
114 if (st->chip_config.accl_fifo_enable)
115 val |= BITS_3050_ACCL_OUT;
116 if (st->chip_config.gyro_fifo_enable)
117 val |= BITS_GYRO_OUT;
118 result = inv_plat_single_write(st, reg->fifo_en, val);
125 if (st->chip_config.dmp_on)
126 val = BIT_DMP_INT_EN;
128 val = BIT_DATA_RDY_EN;
129 inv_plat_single_write(st, reg->int_enable,
130 st->plat_data.int_config | val);
131 pr_err("reset fifo failed\n");
137 * inv_set_lpf() - set low pass filer based on fifo rate.
139 static int inv_set_lpf(struct inv_mpu_iio_s *st, int rate)
141 const short hz[] = {188, 98, 42, 20, 10, 5};
142 const int d[] = {INV_FILTER_188HZ, INV_FILTER_98HZ,
143 INV_FILTER_42HZ, INV_FILTER_20HZ,
144 INV_FILTER_10HZ, INV_FILTER_5HZ};
145 int i, h, data, result;
146 struct inv_reg_map_s *reg;
150 while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
153 if (INV_MPU3050 == st->chip_type) {
154 if (st->mpu_slave != NULL) {
155 result = st->mpu_slave->set_lpf(st, rate);
159 result = inv_plat_single_write(st, reg->lpf, data |
160 (st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT));
162 result = inv_plat_single_write(st, reg->lpf, data);
166 st->chip_config.lpf = data;
172 * set_fifo_rate_reg() - Set fifo rate in hardware register
174 static int set_fifo_rate_reg(struct inv_mpu_iio_s *st)
179 struct inv_reg_map_s *reg;
182 fifo_rate = st->chip_config.new_fifo_rate;
183 data = ONE_K_HZ / fifo_rate - 1;
184 result = inv_plat_single_write(st, reg->sample_rate_div, data);
187 result = inv_set_lpf(st, fifo_rate);
190 st->chip_config.fifo_rate = fifo_rate;
196 * inv_lpa_mode() - store current low power mode settings
198 static int inv_lpa_mode(struct inv_mpu_iio_s *st, int lpa_mode)
200 unsigned long result;
202 struct inv_reg_map_s *reg;
205 result = inv_plat_read(st, reg->pwr_mgmt_1, 1, &d);
213 result = inv_plat_single_write(st, reg->pwr_mgmt_1, d);
216 if (INV_MPU6500 == st->chip_type) {
218 d = BIT_ACCEL_FCHOCIE_B;
221 result = inv_plat_single_write(st, REG_6500_ACCEL_CONFIG2, d);
230 * reset_fifo_itg() - Reset FIFO related registers.
231 * @st: Device driver instance.
233 static int reset_fifo_itg(struct iio_dev *indio_dev)
235 struct inv_reg_map_s *reg;
238 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
241 if (st->chip_config.lpa_mode) {
242 result = inv_lpa_mode(st, 0);
244 pr_err("reset lpa mode failed\n");
248 /* disable interrupt */
249 result = inv_plat_single_write(st, reg->int_enable, 0);
251 pr_err("int_enable write failed\n");
254 /* disable the sensor output to FIFO */
255 result = inv_plat_single_write(st, reg->fifo_en, 0);
257 goto reset_fifo_fail;
258 /* disable fifo reading */
259 result = inv_plat_single_write(st, reg->user_ctrl, st->i2c_dis);
261 goto reset_fifo_fail;
264 /* MPU6500's BIT_6500_WOM_EN is the same as BIT_MOT_EN */
265 if (st->mot_int.mot_on)
266 int_word |= BIT_MOT_EN;
268 if (st->chip_config.dmp_on) {
269 val = (BIT_FIFO_RST | BIT_DMP_RST);
270 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
272 goto reset_fifo_fail;
273 st->last_isr_time = get_time_ns();
274 if (st->chip_config.dmp_int_on) {
275 int_word |= BIT_DMP_INT_EN;
276 result = inv_plat_single_write(st, reg->int_enable,
281 val = (BIT_DMP_EN | BIT_FIFO_EN);
282 if (st->chip_config.compass_enable &
283 (!st->chip_config.dmp_event_int_on))
284 val |= BIT_I2C_MST_EN;
285 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
287 goto reset_fifo_fail;
289 if (st->chip_config.compass_enable) {
290 /* I2C_MST_DLY is set according to sample rate,
291 slow down the power*/
292 data = max(COMPASS_RATE_SCALE *
293 st->chip_config.new_fifo_rate / ONE_K_HZ,
294 st->chip_config.new_fifo_rate /
295 st->chip_config.dmp_output_rate);
298 result = inv_plat_single_write(st, REG_I2C_SLV4_CTRL,
304 if (st->chip_config.accl_fifo_enable)
305 val |= INV_ACCL_MASK;
306 if (st->chip_config.gyro_fifo_enable)
307 val |= INV_GYRO_MASK;
308 result = inv_send_sensor_data(st, val);
311 if (st->chip_config.display_orient_on || st->chip_config.tap_on)
312 result = inv_send_interrupt_word(st, true);
314 result = inv_send_interrupt_word(st, false);
316 /* reset FIFO and possibly reset I2C*/
318 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
320 goto reset_fifo_fail;
321 st->last_isr_time = get_time_ns();
322 /* enable interrupt */
323 if (st->chip_config.accl_fifo_enable ||
324 st->chip_config.gyro_fifo_enable ||
325 st->chip_config.compass_enable) {
326 int_word |= BIT_DATA_RDY_EN;
328 result = inv_plat_single_write(st, reg->int_enable, int_word);
331 /* enable FIFO reading and I2C master interface*/
333 if (st->chip_config.compass_enable)
334 val |= BIT_I2C_MST_EN;
335 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
337 goto reset_fifo_fail;
338 if (st->chip_config.compass_enable) {
339 /* I2C_MST_DLY is set according to sample rate,
340 slow down the power*/
341 data = COMPASS_RATE_SCALE *
342 st->chip_config.new_fifo_rate / ONE_K_HZ;
345 result = inv_plat_single_write(st, REG_I2C_SLV4_CTRL,
350 /* enable sensor output to FIFO */
352 if (st->chip_config.gyro_fifo_enable)
353 val |= BITS_GYRO_OUT;
354 if (st->chip_config.accl_fifo_enable)
355 val |= BIT_ACCEL_OUT;
356 result = inv_plat_single_write(st, reg->fifo_en, val);
358 goto reset_fifo_fail;
360 st->chip_config.normal_compass_measure = 0;
361 result = inv_lpa_mode(st, st->chip_config.lpa_mode);
363 goto reset_fifo_fail;
368 if (st->chip_config.dmp_on)
369 val = BIT_DMP_INT_EN;
371 val = BIT_DATA_RDY_EN;
372 inv_plat_single_write(st, reg->int_enable, val);
373 pr_err("reset fifo failed\n");
379 * inv_clear_kfifo() - clear time stamp fifo
380 * @st: Device driver instance.
382 static void inv_clear_kfifo(struct inv_mpu_iio_s *st)
386 spin_lock_irqsave(&st->time_stamp_lock, flags);
387 kfifo_reset(&st->timestamps);
388 spin_unlock_irqrestore(&st->time_stamp_lock, flags);
392 * inv_reset_fifo() - Reset FIFO related registers.
393 * @st: Device driver instance.
395 static int inv_reset_fifo(struct iio_dev *indio_dev)
397 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
400 if (INV_MPU3050 == st->chip_type)
401 return reset_fifo_mpu3050(indio_dev);
403 return reset_fifo_itg(indio_dev);
406 static int inv_set_dmp_sysfs(struct inv_mpu_iio_s *st)
410 result = inv_set_fifo_rate(st, st->chip_config.dmp_output_rate);
413 result = inv_set_interrupt_on_gesture_event(st,
414 st->chip_config.dmp_event_int_on);
420 * set_inv_enable() - Reset FIFO related registers.
421 * This also powers on the chip if needed.
422 * @st: Device driver instance.
423 * @fifo_enable: enable/disable
425 static int set_inv_enable(struct iio_dev *indio_dev,
427 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
428 struct inv_reg_map_s *reg;
433 if (st->chip_config.new_fifo_rate !=
434 st->chip_config.fifo_rate) {
435 result = set_fifo_rate_reg(st);
439 if (st->chip_config.dmp_on) {
440 result = inv_set_dmp_sysfs(st);
445 if (st->chip_config.gyro_enable) {
446 result = st->switch_gyro_engine(st, true);
450 if (st->chip_config.accl_enable) {
451 result = st->switch_accl_engine(st, true);
456 result = inv_reset_fifo(indio_dev);
460 if ((INV_MPU3050 != st->chip_type)
461 && st->chip_config.lpa_mode) {
462 /* if the chip is in low power mode,
463 register write/read could fail */
464 result = inv_lpa_mode(st, 0);
468 result = inv_plat_single_write(st, reg->fifo_en, 0);
471 /* disable fifo reading */
472 if (INV_MPU3050 != st->chip_type) {
473 result = inv_plat_single_write(st, reg->int_enable, 0);
476 result = inv_plat_single_write(st, reg->user_ctrl, st->i2c_dis);
478 result = inv_plat_single_write(st, reg->int_enable,
479 st->plat_data.int_config);
483 /* turn off the gyro/accl engine during disable phase */
484 result = st->switch_gyro_engine(st, false);
487 result = st->switch_accl_engine(st, false);
490 result = st->set_power_state(st, false);
494 st->chip_config.enable = enable;
499 /* #define _RATE_DEBUG */
502 static s64 tm_irq_min = -1;
503 static s64 tm_irq_max = -1;
504 static s64 tm_irq_sum = -1;
505 static s64 tm_irq_last_print = -1;
506 static s64 tm_irq_count = -1;
510 * inv_irq_handler() - Cache a timestamp at each data ready interrupt.
512 static irqreturn_t inv_irq_handler(int irq, void *dev_id)
514 struct inv_mpu_iio_s *st;
517 u64 time_since_last_irq;
519 st = (struct inv_mpu_iio_s *)dev_id;
520 timestamp = get_time_ns();
521 time_since_last_irq = timestamp - st->last_isr_time;
522 spin_lock(&st->time_stamp_lock);
525 if (tm_irq_min <= 0 && tm_irq_max <= 0) {
526 tm_irq_min = time_since_last_irq;
527 tm_irq_max = time_since_last_irq;
530 } else if (time_since_last_irq < tm_irq_min) {
531 tm_irq_min = time_since_last_irq;
532 } else if (time_since_last_irq > tm_irq_max) {
533 tm_irq_max = time_since_last_irq;
535 tm_irq_sum += time_since_last_irq;
538 if (unlikely((timestamp - tm_irq_last_print) >= 1000000000)) {
539 pr_info("MPU Interrupt: %lld,%d,%lld\n", tm_irq_min, (u32)tm_irq_sum / (u32)tm_irq_count, tm_irq_max);
540 tm_irq_last_print = timestamp;
549 while ((time_since_last_irq > st->irq_dur_ns * 2) &&
550 (catch_up < MAX_CATCH_UP) &&
551 (!st->chip_config.lpa_mode) &&
552 (!st->chip_config.dmp_on)) {
553 st->last_isr_time += st->irq_dur_ns;
554 kfifo_in(&st->timestamps,
555 &st->last_isr_time, 1);
556 time_since_last_irq = timestamp - st->last_isr_time;
559 kfifo_in(&st->timestamps, ×tamp, 1);
560 st->last_isr_time = timestamp;
561 spin_unlock(&st->time_stamp_lock);
563 return IRQ_WAKE_THREAD;
566 static int put_scan_to_buf(struct iio_dev *indio_dev, u8 *d,
567 short *s, int scan_index, int d_ind) {
568 struct iio_buffer *ring = indio_dev->buffer;
571 for (i = 0; i < 3; i++) {
572 st = iio_scan_mask_query(indio_dev, ring, scan_index + i);
574 memcpy(&d[d_ind], &s[i], sizeof(s[i]));
575 d_ind += sizeof(s[i]);
581 static void inv_report_data_3050(struct iio_dev *indio_dev, s64 t,
582 int has_footer, u8 *data)
584 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
585 struct iio_buffer *ring = indio_dev->buffer;
587 struct inv_chip_config_s *conf;
588 short g[THREE_AXIS], a[THREE_AXIS];
591 int bytes_per_datum, scan_count;
592 conf = &st->chip_config;
594 scan_count = bitmap_weight(indio_dev->active_scan_mask,
595 indio_dev->masklength);
596 bytes_per_datum = scan_count * 2;
604 if (conf->gyro_fifo_enable) {
605 for (i = 0; i < ARRAY_SIZE(g); i++) {
606 g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
607 st->raw_gyro[i] = g[i];
609 ind += BYTES_PER_SENSOR;
611 if (conf->accl_fifo_enable) {
612 st->mpu_slave->combine_data(&data[ind], a);
613 for (i = 0; i < ARRAY_SIZE(a); i++)
614 st->raw_accel[i] = a[i];
616 ind += BYTES_PER_SENSOR;
617 d_ind = put_scan_to_buf(indio_dev, tmp, a,
618 INV_MPU_SCAN_ACCL_X, d_ind);
620 if (conf->gyro_fifo_enable)
621 d_ind = put_scan_to_buf(indio_dev, tmp, g,
622 INV_MPU_SCAN_GYRO_X, d_ind);
624 i = (bytes_per_datum + 7) / 8;
625 if (ring->scan_timestamp)
627 #ifdef INV_KERNEL_3_10
628 ring->access->store_to(indio_dev->buffer, (u8 *)buf);
630 ring->access->store_to(indio_dev->buffer, (u8 *)buf, t);
635 * inv_read_fifo_mpu3050() - Transfer data from FIFO to ring buffer for
638 irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id)
641 struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
642 struct iio_dev *indio_dev = iio_priv_to_dev(st);
646 short fifo_count, byte_read;
649 struct inv_reg_map_s *reg;
651 /* It is impossible that chip is asleep or enable is
652 zero when interrupt is on
653 * because interrupt is now connected with enable */
654 if (st->chip_config.dmp_on)
655 bytes_per_datum = BYTES_FOR_DMP;
657 bytes_per_datum = (st->chip_config.accl_fifo_enable +
658 st->chip_config.gyro_fifo_enable)*BYTES_PER_SENSOR;
659 if (st->chip_config.has_footer)
660 byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
662 byte_read = bytes_per_datum;
665 if (byte_read != 0) {
666 result = inv_plat_read(st, reg->fifo_count_h,
667 FIFO_COUNT_BYTE, data);
670 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
671 if (fifo_count < byte_read)
675 if (fifo_count > FIFO_THRESHOLD)
677 /* Timestamp mismatch. */
678 if (kfifo_len(&st->timestamps) <
679 fifo_count / byte_read)
681 if (kfifo_len(&st->timestamps) >
682 fifo_count / byte_read + TIME_STAMP_TOR) {
683 if (st->chip_config.dmp_on) {
684 result = kfifo_to_user(&st->timestamps,
685 ×tamp, sizeof(timestamp), &copied);
693 while ((bytes_per_datum != 0) && (fifo_count >= byte_read)) {
694 result = inv_plat_read(st, reg->fifo_r_w, byte_read, data);
698 result = kfifo_to_user(&st->timestamps,
699 ×tamp, sizeof(timestamp), &copied);
702 inv_report_data_3050(indio_dev, timestamp,
703 st->chip_config.has_footer, data);
704 fifo_count -= byte_read;
705 if (st->chip_config.has_footer == 0) {
706 st->chip_config.has_footer = 1;
707 byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
715 /* Flush HW and SW FIFOs. */
716 inv_reset_fifo(indio_dev);
722 static s64 tm_min = -1;
723 static s64 tm_max = -1;
724 static s64 tm_sum = -1;
725 static s64 tm_last_print = -1;
726 static s64 tm_count = -1;
729 static int inv_report_gyro_accl_compass(struct iio_dev *indio_dev,
732 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
733 short g[THREE_AXIS], a[THREE_AXIS], c[THREE_AXIS];
737 u8 d[8], compass_divider;
741 struct inv_chip_config_s *conf;
743 conf = &st->chip_config;
746 if (conf->quaternion_on & conf->dmp_on) {
747 for (i = 0; i < ARRAY_SIZE(q); i++) {
748 q[i] = be32_to_cpup((__be32 *)(&data[ind + i * 4]));
749 st->raw_quaternion[i] = q[i];
750 memcpy(&buf[ind + i * sizeof(q[i])], &q[i],
753 ind += QUATERNION_BYTES;
756 if (conf->accl_fifo_enable) {
757 for (i = 0; i < ARRAY_SIZE(a); i++) {
758 a[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
759 memcpy(&buf[ind + i * sizeof(a[i])], &a[i],
762 ind += BYTES_PER_SENSOR;
765 if (conf->gyro_fifo_enable) {
766 for (i = 0; i < ARRAY_SIZE(g); i++) {
767 g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
768 memcpy(&buf[ind + i * sizeof(g[i])], &g[i],
771 ind += BYTES_PER_SENSOR;
774 if (conf->dmp_on && (conf->tap_on || conf->display_orient_on)) {
775 word = (u32)(be32_to_cpup((u32 *)&data[ind]));
776 source = ((word >> 16) & 0xff);
778 st->tap_data = (DMP_MASK_TAP & (word & 0xff));
779 st->display_orient_data =
780 ((DMP_MASK_DIS_ORIEN & (word & 0xff)) >>
781 DMP_DIS_ORIEN_SHIFT);
784 /* report tap information */
785 if (source & INT_SRC_TAP)
786 sysfs_notify(&indio_dev->dev.kobj, NULL, "event_tap");
787 /* report orientation information */
788 if (source & INT_SRC_DISPLAY_ORIENT)
789 sysfs_notify(&indio_dev->dev.kobj, NULL,
790 "event_display_orientation");
792 /*divider and counter is used to decrease the speed of read in
793 high frequency sample rate*/
794 if (conf->compass_fifo_enable) {
799 compass_divider = st->compass_dmp_divider;
801 compass_divider = st->compass_divider;
802 if (compass_divider <= st->compass_counter) {
803 /*read from external sensor data register */
804 result = inv_plat_read(st, REG_EXT_SENS_DATA_00,
805 NUM_BYTES_COMPASS_SLAVE, d);
806 /* d[7] is status 2 register */
807 /*for AKM8975, bit 2 and 3 should be all be zero*/
808 /* for AMK8963, bit 3 should be zero*/
809 if ((DATA_AKM_DRDY == d[0]) &&
810 (0 == (d[7] & DATA_AKM_STAT_MASK)) &&
813 sens = st->chip_info.compass_sens;
814 c[0] = (short)((d[2] << 8) | d[1]);
815 c[1] = (short)((d[4] << 8) | d[3]);
816 c[2] = (short)((d[6] << 8) | d[5]);
817 c[0] = (short)(((int)c[0] *
818 (sens[0] + 128)) >> 8);
819 c[1] = (short)(((int)c[1] *
820 (sens[1] + 128)) >> 8);
821 c[2] = (short)(((int)c[2] *
822 (sens[2] + 128)) >> 8);
823 st->raw_compass[0] = c[0];
824 st->raw_compass[1] = c[1];
825 st->raw_compass[2] = c[2];
827 st->compass_counter = 0;
828 } else if (compass_divider != 0) {
829 st->compass_counter++;
831 if (!conf->normal_compass_measure) {
835 conf->normal_compass_measure = 1;
837 for (i = 0; i < 3; i++)
838 memcpy(&buf[ind + i * sizeof(c[i])], &c[i],
840 ind += BYTES_PER_SENSOR;
843 tmp[DIV_ROUND_UP(ind, 8)] = t;
847 s64 tm_cur = get_time_ns();
848 s64 tm_delta = tm_cur - t;
850 if (tm_min <= 0 && tm_max <= 0) {
855 } else if (tm_delta < tm_min) {
857 } else if (tm_delta > tm_max) {
863 if (unlikely((tm_cur - tm_last_print) >= 1000000000)) {
864 pr_info("MPU6050 report size: %d rate: %lld\n", ind, tm_count);
865 pr_info("MPU KRNL report: [%lld] %lld,%d,%lld\n", t, tm_min, (u32)tm_sum / (u32)tm_count, tm_max);
866 tm_last_print = tm_cur;
874 #ifdef INV_KERNEL_3_10
875 iio_push_to_buffers(indio_dev, buf);
877 iio_push_to_buffer(indio_dev->buffer, buf, t);
884 static void inv_process_motion(struct inv_mpu_iio_s *st)
886 struct iio_dev *indio_dev = iio_priv_to_dev(st);
887 s32 diff, true_motion;
892 /* motion interrupt */
893 result = inv_plat_read(st, REG_INT_STATUS, 1, data);
897 if (data[0] & BIT_MOT_INT) {
898 timestamp = get_time_ns();
899 diff = (int)(((timestamp - st->mpu6500_last_motion_time) >>
901 if (diff > st->mot_int.mot_dur) {
902 st->mpu6500_last_motion_time = timestamp;
908 sysfs_notify(&indio_dev->dev.kobj, NULL,
909 "event_accel_motion");
913 static int get_bytes_per_datum(struct inv_mpu_iio_s *st)
918 if (st->chip_config.dmp_on) {
919 if (st->chip_config.quaternion_on)
920 bytes_per_datum += QUATERNION_BYTES;
921 if (st->chip_config.tap_on ||
922 st->chip_config.display_orient_on)
923 bytes_per_datum += BYTES_FOR_EVENTS;
925 if (st->chip_config.accl_fifo_enable)
926 bytes_per_datum += BYTES_PER_SENSOR;
927 if (st->chip_config.gyro_fifo_enable)
928 bytes_per_datum += BYTES_PER_SENSOR;
930 return bytes_per_datum;
934 * inv_read_fifo() - Transfer data from FIFO to ring buffer.
936 irqreturn_t inv_read_fifo(int irq, void *dev_id)
939 struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
940 struct iio_dev *indio_dev = iio_priv_to_dev(st);
941 size_t bytes_per_datum;
943 u8 data[BYTES_FOR_DMP + QUATERNION_BYTES];
947 struct inv_reg_map_s *reg;
954 s64 tm_begin, tm_end;
956 tm_begin = get_time_ns();
959 mutex_lock(&indio_dev->mlock);
960 if (!(iio_buffer_enabled(indio_dev)))
964 if (!(st->chip_config.accl_fifo_enable |
965 st->chip_config.gyro_fifo_enable |
966 st->chip_config.dmp_on |
967 st->chip_config.compass_fifo_enable |
970 if (st->mot_int.mot_on)
971 inv_process_motion(st);
972 if (st->chip_config.dmp_on && st->chip_config.smd_enable) {
973 /* dmp interrupt status */
974 result = inv_plat_read(st, REG_DMP_INT_STATUS, 1, data);
976 if (data[0] & SMD_INT_ON) {
977 sysfs_notify(&indio_dev->dev.kobj, NULL,
979 st->chip_config.smd_enable = 0;
982 if (st->chip_config.lpa_mode) {
983 result = inv_plat_read(st, reg->raw_accl,
984 BYTES_PER_SENSOR, data);
987 inv_report_gyro_accl_compass(indio_dev, data,
991 bytes_per_datum = get_bytes_per_datum(st);
993 if (bytes_per_datum != 0) {
997 result = inv_plat_read(st, reg->fifo_count_h,
998 FIFO_COUNT_BYTE, data);
1000 tm_i2c_sum += get_time_ns() - tm;
1004 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
1005 if (fifo_count == 0)
1007 if (fifo_count < bytes_per_datum)
1009 /* fifo count can't be odd number */
1012 if (fifo_count > FIFO_THRESHOLD)
1014 /* timestamp mismatch. */
1015 if (kfifo_len(&st->timestamps) <
1016 fifo_count / bytes_per_datum)
1018 if (kfifo_len(&st->timestamps) >
1019 fifo_count / bytes_per_datum + TIME_STAMP_TOR) {
1020 if (st->chip_config.dmp_on) {
1021 result = kfifo_to_user(&st->timestamps,
1022 ×tamp, sizeof(timestamp), &copied);
1030 result = kfifo_to_user(&st->timestamps,
1031 ×tamp, sizeof(timestamp), &copied);
1036 while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
1040 result = inv_plat_read(st, reg->fifo_r_w, bytes_per_datum,
1043 tm_i2c_sum += get_time_ns() - tm;
1048 result = kfifo_to_user(&st->timestamps,
1049 ×tamp, sizeof(timestamp), &copied);
1052 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
1053 fifo_count -= bytes_per_datum;
1055 if (bytes_per_datum == 0 && st->chip_config.compass_fifo_enable)
1056 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
1059 mutex_unlock(&indio_dev->mlock);
1062 tm_end = get_time_ns();
1063 if (tm_end - tm_begin > 700000)
1064 pr_info("%s: [%lld] %lld %lld %lld\n", __func__, timestamp, tm_begin - timestamp, tm_i2c_sum, tm_end - tm_begin);
1070 /* Flush HW and SW FIFOs. */
1071 inv_reset_fifo(indio_dev);
1072 inv_clear_kfifo(st);
1073 mutex_unlock(&indio_dev->mlock);
1078 void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev)
1080 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1081 free_irq(st->irq, st);
1082 iio_kfifo_free(indio_dev->buffer);
1085 static int inv_postenable(struct iio_dev *indio_dev)
1087 return set_inv_enable(indio_dev, true);
1090 static int inv_predisable(struct iio_dev *indio_dev)
1092 return set_inv_enable(indio_dev, false);
1095 static void inv_scan_query(struct iio_dev *indio_dev)
1097 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1098 struct iio_buffer *ring = indio_dev->buffer;
1101 if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_X) ||
1102 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Y) ||
1103 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Z))
1104 st->chip_config.gyro_fifo_enable = 1;
1106 st->chip_config.gyro_fifo_enable = 0;
1108 if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_X) ||
1109 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Y) ||
1110 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Z))
1111 st->chip_config.accl_fifo_enable = 1;
1113 st->chip_config.accl_fifo_enable = 0;
1115 if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_X) ||
1116 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Y) ||
1117 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Z))
1118 st->chip_config.compass_fifo_enable = 1;
1120 st->chip_config.compass_fifo_enable = 0;
1122 /* check to make sure engine is turned on if fifo is turned on */
1123 if (st->chip_config.gyro_fifo_enable &&
1124 (!st->chip_config.gyro_enable)) {
1125 result = st->switch_gyro_engine(st, true);
1128 st->chip_config.gyro_enable = true;
1130 if (st->chip_config.accl_fifo_enable &&
1131 (!st->chip_config.accl_enable)) {
1132 result = st->switch_accl_engine(st, true);
1135 st->chip_config.accl_enable = true;
1139 static int inv_check_quaternion(struct iio_dev *indio_dev)
1141 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1142 struct iio_buffer *ring = indio_dev->buffer;
1145 if (st->chip_config.dmp_on) {
1147 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_R) ||
1148 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_X) ||
1149 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Y) ||
1150 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Z))
1151 st->chip_config.quaternion_on = 1;
1153 st->chip_config.quaternion_on = 0;
1155 result = inv_send_quaternion(st,
1156 st->chip_config.quaternion_on);
1160 st->chip_config.quaternion_on = 0;
1161 clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
1162 clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
1163 clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
1164 clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
1170 static int inv_check_conflict_sysfs(struct iio_dev *indio_dev)
1172 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1173 struct iio_buffer *ring = indio_dev->buffer;
1176 if (st->chip_config.lpa_mode) {
1177 /* dmp cannot run with low power mode on */
1178 st->chip_config.dmp_on = 0;
1179 result = st->gyro_en(st, ring, false);
1182 result = st->compass_en(st, ring, false);
1185 result = st->quaternion_en(st, ring, false);
1189 result = st->accl_en(st, ring, true);
1193 result = inv_check_quaternion(indio_dev);
1200 static int inv_preenable(struct iio_dev *indio_dev)
1203 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1205 result = st->set_power_state(st, true);
1209 result = inv_check_conflict_sysfs(indio_dev);
1212 inv_scan_query(indio_dev);
1217 static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = {
1218 .preenable = &inv_preenable,
1219 .postenable = &inv_postenable,
1220 .predisable = &inv_predisable,
1223 int inv_mpu_configure_ring(struct iio_dev *indio_dev)
1226 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1227 struct iio_buffer *ring;
1229 ring = iio_kfifo_allocate();
1232 indio_dev->buffer = ring;
1233 /* setup ring buffer */
1234 ring->scan_timestamp = true;
1235 indio_dev->setup_ops = &inv_mpu_ring_setup_ops;
1236 /*scan count double count timestamp. should subtract 1. but
1237 number of channels still includes timestamp*/
1238 if (INV_MPU3050 == st->chip_type)
1239 ret = request_threaded_irq(st->irq, inv_irq_handler,
1240 inv_read_fifo_mpu3050,
1241 IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1243 ret = request_threaded_irq(st->irq, inv_irq_handler,
1245 IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1247 goto error_iio_sw_rb_free;
1249 indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
1251 error_iio_sw_rb_free:
1252 iio_kfifo_free(indio_dev->buffer);