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);
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);
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);
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, 0);
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);
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);
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);
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);
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, 0);
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;
500 * inv_irq_handler() - Cache a timestamp at each data ready interrupt.
502 static irqreturn_t inv_irq_handler(int irq, void *dev_id)
504 struct inv_mpu_iio_s *st;
507 u64 time_since_last_irq;
509 st = (struct inv_mpu_iio_s *)dev_id;
510 timestamp = get_time_ns();
511 time_since_last_irq = timestamp - st->last_isr_time;
512 spin_lock(&st->time_stamp_lock);
514 while ((time_since_last_irq > st->irq_dur_ns * 2) &&
515 (catch_up < MAX_CATCH_UP) &&
516 (!st->chip_config.lpa_mode) &&
517 (!st->chip_config.dmp_on)) {
518 st->last_isr_time += st->irq_dur_ns;
519 kfifo_in(&st->timestamps,
520 &st->last_isr_time, 1);
521 time_since_last_irq = timestamp - st->last_isr_time;
524 kfifo_in(&st->timestamps, ×tamp, 1);
525 st->last_isr_time = timestamp;
526 spin_unlock(&st->time_stamp_lock);
528 return IRQ_WAKE_THREAD;
531 static int put_scan_to_buf(struct iio_dev *indio_dev, u8 *d,
532 short *s, int scan_index, int d_ind) {
533 struct iio_buffer *ring = indio_dev->buffer;
536 for (i = 0; i < 3; i++) {
537 st = iio_scan_mask_query(indio_dev, ring, scan_index + i);
539 memcpy(&d[d_ind], &s[i], sizeof(s[i]));
540 d_ind += sizeof(s[i]);
546 static void inv_report_data_3050(struct iio_dev *indio_dev, s64 t,
547 int has_footer, u8 *data)
549 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
550 struct iio_buffer *ring = indio_dev->buffer;
552 struct inv_chip_config_s *conf;
553 short g[THREE_AXIS], a[THREE_AXIS];
556 int bytes_per_datum, scan_count;
557 conf = &st->chip_config;
559 scan_count = bitmap_weight(indio_dev->active_scan_mask,
560 indio_dev->masklength);
561 bytes_per_datum = scan_count * 2;
569 if (conf->gyro_fifo_enable) {
570 for (i = 0; i < ARRAY_SIZE(g); i++) {
571 g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
572 st->raw_gyro[i] = g[i];
574 ind += BYTES_PER_SENSOR;
576 if (conf->accl_fifo_enable) {
577 st->mpu_slave->combine_data(&data[ind], a);
578 for (i = 0; i < ARRAY_SIZE(a); i++)
579 st->raw_accel[i] = a[i];
581 ind += BYTES_PER_SENSOR;
582 d_ind = put_scan_to_buf(indio_dev, tmp, a,
583 INV_MPU_SCAN_ACCL_X, d_ind);
585 if (conf->gyro_fifo_enable)
586 d_ind = put_scan_to_buf(indio_dev, tmp, g,
587 INV_MPU_SCAN_GYRO_X, d_ind);
589 i = (bytes_per_datum + 7) / 8;
590 if (ring->scan_timestamp)
592 #ifdef INV_KERNEL_3_10
593 ring->access->store_to(indio_dev->buffer, (u8 *)buf);
595 ring->access->store_to(indio_dev->buffer, (u8 *)buf, t);
600 * inv_read_fifo_mpu3050() - Transfer data from FIFO to ring buffer for
603 irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id)
606 struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
607 struct iio_dev *indio_dev = iio_priv_to_dev(st);
611 short fifo_count, byte_read;
614 struct inv_reg_map_s *reg;
616 /* It is impossible that chip is asleep or enable is
617 zero when interrupt is on
618 * because interrupt is now connected with enable */
619 if (st->chip_config.dmp_on)
620 bytes_per_datum = BYTES_FOR_DMP;
622 bytes_per_datum = (st->chip_config.accl_fifo_enable +
623 st->chip_config.gyro_fifo_enable)*BYTES_PER_SENSOR;
624 if (st->chip_config.has_footer)
625 byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
627 byte_read = bytes_per_datum;
630 if (byte_read != 0) {
631 result = inv_plat_read(st, reg->fifo_count_h,
632 FIFO_COUNT_BYTE, data);
635 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
636 if (fifo_count < byte_read)
640 if (fifo_count > FIFO_THRESHOLD)
642 /* Timestamp mismatch. */
643 if (kfifo_len(&st->timestamps) <
644 fifo_count / byte_read)
646 if (kfifo_len(&st->timestamps) >
647 fifo_count / byte_read + TIME_STAMP_TOR) {
648 if (st->chip_config.dmp_on) {
649 result = kfifo_to_user(&st->timestamps,
650 ×tamp, sizeof(timestamp), &copied);
658 while ((bytes_per_datum != 0) && (fifo_count >= byte_read)) {
659 result = inv_plat_read(st, reg->fifo_r_w, byte_read, data);
663 result = kfifo_to_user(&st->timestamps,
664 ×tamp, sizeof(timestamp), &copied);
667 inv_report_data_3050(indio_dev, timestamp,
668 st->chip_config.has_footer, data);
669 fifo_count -= byte_read;
670 if (st->chip_config.has_footer == 0) {
671 st->chip_config.has_footer = 1;
672 byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
680 /* Flush HW and SW FIFOs. */
681 inv_reset_fifo(indio_dev);
686 static int inv_report_gyro_accl_compass(struct iio_dev *indio_dev,
689 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
690 short g[THREE_AXIS], a[THREE_AXIS], c[THREE_AXIS];
694 u8 d[8], compass_divider;
698 struct inv_chip_config_s *conf;
700 conf = &st->chip_config;
703 if (conf->quaternion_on & conf->dmp_on) {
704 for (i = 0; i < ARRAY_SIZE(q); i++) {
705 q[i] = be32_to_cpup((__be32 *)(&data[ind + i * 4]));
706 st->raw_quaternion[i] = q[i];
707 memcpy(&buf[ind + i * sizeof(q[i])], &q[i],
710 ind += QUATERNION_BYTES;
713 if (conf->accl_fifo_enable) {
714 for (i = 0; i < ARRAY_SIZE(a); i++) {
715 a[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
716 memcpy(&buf[ind + i * sizeof(a[i])], &a[i],
719 ind += BYTES_PER_SENSOR;
722 if (conf->gyro_fifo_enable) {
723 for (i = 0; i < ARRAY_SIZE(g); i++) {
724 g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
725 memcpy(&buf[ind + i * sizeof(g[i])], &g[i],
728 ind += BYTES_PER_SENSOR;
731 if (conf->dmp_on && (conf->tap_on || conf->display_orient_on)) {
732 word = (u32)(be32_to_cpup((u32 *)&data[ind]));
733 source = ((word >> 16) & 0xff);
735 st->tap_data = (DMP_MASK_TAP & (word & 0xff));
736 st->display_orient_data =
737 ((DMP_MASK_DIS_ORIEN & (word & 0xff)) >>
738 DMP_DIS_ORIEN_SHIFT);
741 /* report tap information */
742 if (source & INT_SRC_TAP)
743 sysfs_notify(&indio_dev->dev.kobj, NULL, "event_tap");
744 /* report orientation information */
745 if (source & INT_SRC_DISPLAY_ORIENT)
746 sysfs_notify(&indio_dev->dev.kobj, NULL,
747 "event_display_orientation");
749 /*divider and counter is used to decrease the speed of read in
750 high frequency sample rate*/
751 if (conf->compass_fifo_enable) {
756 compass_divider = st->compass_dmp_divider;
758 compass_divider = st->compass_divider;
759 if (compass_divider <= st->compass_counter) {
760 /*read from external sensor data register */
761 result = inv_plat_read(st, REG_EXT_SENS_DATA_00,
762 NUM_BYTES_COMPASS_SLAVE, d);
763 /* d[7] is status 2 register */
764 /*for AKM8975, bit 2 and 3 should be all be zero*/
765 /* for AMK8963, bit 3 should be zero*/
766 if ((DATA_AKM_DRDY == d[0]) &&
767 (0 == (d[7] & DATA_AKM_STAT_MASK)) &&
770 sens = st->chip_info.compass_sens;
771 c[0] = (short)((d[2] << 8) | d[1]);
772 c[1] = (short)((d[4] << 8) | d[3]);
773 c[2] = (short)((d[6] << 8) | d[5]);
774 c[0] = (short)(((int)c[0] *
775 (sens[0] + 128)) >> 8);
776 c[1] = (short)(((int)c[1] *
777 (sens[1] + 128)) >> 8);
778 c[2] = (short)(((int)c[2] *
779 (sens[2] + 128)) >> 8);
780 st->raw_compass[0] = c[0];
781 st->raw_compass[1] = c[1];
782 st->raw_compass[2] = c[2];
784 st->compass_counter = 0;
785 } else if (compass_divider != 0) {
786 st->compass_counter++;
788 if (!conf->normal_compass_measure) {
792 conf->normal_compass_measure = 1;
794 for (i = 0; i < 3; i++)
795 memcpy(&buf[ind + i * sizeof(c[i])], &c[i],
797 ind += BYTES_PER_SENSOR;
800 tmp[DIV_ROUND_UP(ind, 8)] = t;
803 #ifdef INV_KERNEL_3_10
804 iio_push_to_buffers(indio_dev, buf);
806 iio_push_to_buffer(indio_dev->buffer, buf, t);
812 static void inv_process_motion(struct inv_mpu_iio_s *st)
814 struct iio_dev *indio_dev = iio_priv_to_dev(st);
815 s32 diff, true_motion;
820 /* motion interrupt */
821 result = inv_plat_read(st, REG_INT_STATUS, 1, data);
825 if (data[0] & BIT_MOT_INT) {
826 timestamp = get_time_ns();
827 diff = (int)(((timestamp - st->mpu6500_last_motion_time) >>
829 if (diff > st->mot_int.mot_dur) {
830 st->mpu6500_last_motion_time = timestamp;
836 sysfs_notify(&indio_dev->dev.kobj, NULL,
837 "event_accel_motion");
841 static int get_bytes_per_datum(struct inv_mpu_iio_s *st)
846 if (st->chip_config.dmp_on) {
847 if (st->chip_config.quaternion_on)
848 bytes_per_datum += QUATERNION_BYTES;
849 if (st->chip_config.tap_on ||
850 st->chip_config.display_orient_on)
851 bytes_per_datum += BYTES_FOR_EVENTS;
853 if (st->chip_config.accl_fifo_enable)
854 bytes_per_datum += BYTES_PER_SENSOR;
855 if (st->chip_config.gyro_fifo_enable)
856 bytes_per_datum += BYTES_PER_SENSOR;
858 return bytes_per_datum;
862 * inv_read_fifo() - Transfer data from FIFO to ring buffer.
864 irqreturn_t inv_read_fifo(int irq, void *dev_id)
867 struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
868 struct iio_dev *indio_dev = iio_priv_to_dev(st);
869 size_t bytes_per_datum;
871 u8 data[BYTES_FOR_DMP + QUATERNION_BYTES];
875 struct inv_reg_map_s *reg;
879 mutex_lock(&indio_dev->mlock);
880 if (!(iio_buffer_enabled(indio_dev)))
884 if (!(st->chip_config.accl_fifo_enable |
885 st->chip_config.gyro_fifo_enable |
886 st->chip_config.dmp_on |
887 st->chip_config.compass_fifo_enable |
890 if (st->mot_int.mot_on)
891 inv_process_motion(st);
892 if (st->chip_config.dmp_on && st->chip_config.smd_enable) {
893 /* dmp interrupt status */
894 result = inv_plat_read(st, REG_DMP_INT_STATUS, 1, data);
896 if (data[0] & SMD_INT_ON) {
897 sysfs_notify(&indio_dev->dev.kobj, NULL,
899 st->chip_config.smd_enable = 0;
902 if (st->chip_config.lpa_mode) {
903 result = inv_plat_read(st, reg->raw_accl,
904 BYTES_PER_SENSOR, data);
907 inv_report_gyro_accl_compass(indio_dev, data,
911 bytes_per_datum = get_bytes_per_datum(st);
913 if (bytes_per_datum != 0) {
914 result = inv_plat_read(st, reg->fifo_count_h,
915 FIFO_COUNT_BYTE, data);
918 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
921 if (fifo_count < bytes_per_datum)
923 /* fifo count can't be odd number */
926 if (fifo_count > FIFO_THRESHOLD)
928 /* timestamp mismatch. */
929 if (kfifo_len(&st->timestamps) <
930 fifo_count / bytes_per_datum)
932 if (kfifo_len(&st->timestamps) >
933 fifo_count / bytes_per_datum + TIME_STAMP_TOR) {
934 if (st->chip_config.dmp_on) {
935 result = kfifo_to_user(&st->timestamps,
936 ×tamp, sizeof(timestamp), &copied);
944 result = kfifo_to_user(&st->timestamps,
945 ×tamp, sizeof(timestamp), &copied);
950 while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
951 result = inv_plat_read(st, reg->fifo_r_w, bytes_per_datum,
956 result = kfifo_to_user(&st->timestamps,
957 ×tamp, sizeof(timestamp), &copied);
960 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
961 fifo_count -= bytes_per_datum;
963 if (bytes_per_datum == 0 && st->chip_config.compass_fifo_enable)
964 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
967 mutex_unlock(&indio_dev->mlock);
972 /* Flush HW and SW FIFOs. */
973 inv_reset_fifo(indio_dev);
975 mutex_unlock(&indio_dev->mlock);
980 void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev)
982 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
983 free_irq(st->irq, st);
984 iio_kfifo_free(indio_dev->buffer);
987 static int inv_postenable(struct iio_dev *indio_dev)
989 return set_inv_enable(indio_dev, true);
992 static int inv_predisable(struct iio_dev *indio_dev)
994 return set_inv_enable(indio_dev, false);
997 static void inv_scan_query(struct iio_dev *indio_dev)
999 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1000 struct iio_buffer *ring = indio_dev->buffer;
1003 if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_X) ||
1004 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Y) ||
1005 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Z))
1006 st->chip_config.gyro_fifo_enable = 1;
1008 st->chip_config.gyro_fifo_enable = 0;
1010 if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_X) ||
1011 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Y) ||
1012 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Z))
1013 st->chip_config.accl_fifo_enable = 1;
1015 st->chip_config.accl_fifo_enable = 0;
1017 if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_X) ||
1018 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Y) ||
1019 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Z))
1020 st->chip_config.compass_fifo_enable = 1;
1022 st->chip_config.compass_fifo_enable = 0;
1024 /* check to make sure engine is turned on if fifo is turned on */
1025 if (st->chip_config.gyro_fifo_enable &&
1026 (!st->chip_config.gyro_enable)) {
1027 result = st->switch_gyro_engine(st, true);
1030 st->chip_config.gyro_enable = true;
1032 if (st->chip_config.accl_fifo_enable &&
1033 (!st->chip_config.accl_enable)) {
1034 result = st->switch_accl_engine(st, true);
1037 st->chip_config.accl_enable = true;
1041 static int inv_check_quaternion(struct iio_dev *indio_dev)
1043 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1044 struct iio_buffer *ring = indio_dev->buffer;
1047 if (st->chip_config.dmp_on) {
1049 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_R) ||
1050 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_X) ||
1051 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Y) ||
1052 iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Z))
1053 st->chip_config.quaternion_on = 1;
1055 st->chip_config.quaternion_on = 0;
1057 result = inv_send_quaternion(st,
1058 st->chip_config.quaternion_on);
1062 st->chip_config.quaternion_on = 0;
1063 clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
1064 clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
1065 clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
1066 clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
1072 static int inv_check_conflict_sysfs(struct iio_dev *indio_dev)
1074 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1075 struct iio_buffer *ring = indio_dev->buffer;
1078 if (st->chip_config.lpa_mode) {
1079 /* dmp cannot run with low power mode on */
1080 st->chip_config.dmp_on = 0;
1081 result = st->gyro_en(st, ring, false);
1084 result = st->compass_en(st, ring, false);
1087 result = st->quaternion_en(st, ring, false);
1091 result = st->accl_en(st, ring, true);
1095 result = inv_check_quaternion(indio_dev);
1102 static int inv_preenable(struct iio_dev *indio_dev)
1105 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1107 result = st->set_power_state(st, true);
1111 result = inv_check_conflict_sysfs(indio_dev);
1114 inv_scan_query(indio_dev);
1119 static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = {
1120 .preenable = &inv_preenable,
1121 .postenable = &inv_postenable,
1122 .predisable = &inv_predisable,
1125 int inv_mpu_configure_ring(struct iio_dev *indio_dev)
1128 struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1129 struct iio_buffer *ring;
1131 ring = iio_kfifo_allocate();
1134 indio_dev->buffer = ring;
1135 /* setup ring buffer */
1136 ring->scan_timestamp = true;
1137 indio_dev->setup_ops = &inv_mpu_ring_setup_ops;
1138 /*scan count double count timestamp. should subtract 1. but
1139 number of channels still includes timestamp*/
1140 if (INV_MPU3050 == st->chip_type)
1141 ret = request_threaded_irq(st->irq, inv_irq_handler,
1142 inv_read_fifo_mpu3050,
1143 IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1145 ret = request_threaded_irq(st->irq, inv_irq_handler,
1147 IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1149 goto error_iio_sw_rb_free;
1151 indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
1153 error_iio_sw_rb_free:
1154 iio_kfifo_free(indio_dev->buffer);