kernel/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c

   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 "inv_mpu_iio.h"
  15
  16 static void inv_scan_query(struct iio_dev *indio_dev)
  17 {
  18         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
  19
  20         st->chip_config.gyro_fifo_enable =
  21                 test_bit(INV_MPU6050_SCAN_GYRO_X,
  22                         indio_dev->active_scan_mask) ||
  23                         test_bit(INV_MPU6050_SCAN_GYRO_Y,
  24                         indio_dev->active_scan_mask) ||
  25                         test_bit(INV_MPU6050_SCAN_GYRO_Z,
  26                         indio_dev->active_scan_mask);
  27
  28         st->chip_config.accl_fifo_enable =
  29                 test_bit(INV_MPU6050_SCAN_ACCL_X,
  30                         indio_dev->active_scan_mask) ||
  31                         test_bit(INV_MPU6050_SCAN_ACCL_Y,
  32                         indio_dev->active_scan_mask) ||
  33                         test_bit(INV_MPU6050_SCAN_ACCL_Z,
  34                         indio_dev->active_scan_mask);
  35 }
  36
  37 /**
  38  *  inv_mpu6050_set_enable() - enable chip functions.
  39  *  @indio_dev: Device driver instance.
  40  *  @enable: enable/disable
  41  */
  42 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
  43 {
  44         struct inv_mpu6050_state *st = iio_priv(indio_dev);
  45         int result;
  46
  47         if (enable) {
  48                 result = inv_mpu6050_set_power_itg(st, true);
  49                 if (result)
  50                         return result;
  51                 inv_scan_query(indio_dev);
  52                 if (st->chip_config.gyro_fifo_enable) {
  53                         result = inv_mpu6050_switch_engine(st, true,
  54                                         INV_MPU6050_BIT_PWR_GYRO_STBY);
  55                         if (result)
  56                                 return result;
  57                 }
  58                 if (st->chip_config.accl_fifo_enable) {
  59                         result = inv_mpu6050_switch_engine(st, true,
  60                                         INV_MPU6050_BIT_PWR_ACCL_STBY);
  61                         if (result)
  62                                 return result;
  63                 }
  64                 result = inv_reset_fifo(indio_dev);
  65                 if (result)
  66                         return result;
  67         } else {
  68                 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
  69                 if (result)
  70                         return result;
  71
  72                 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
  73                 if (result)
  74                         return result;
  75
  76                 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
  77                 if (result)
  78                         return result;
  79
  80                 result = inv_mpu6050_switch_engine(st, false,
  81                                         INV_MPU6050_BIT_PWR_GYRO_STBY);
  82                 if (result)
  83                         return result;
  84
  85                 result = inv_mpu6050_switch_engine(st, false,
  86                                         INV_MPU6050_BIT_PWR_ACCL_STBY);
  87                 if (result)
  88                         return result;
  89                 result = inv_mpu6050_set_power_itg(st, false);
  90                 if (result)
  91                         return result;
  92         }
  93         st->chip_config.enable = enable;
  94
  95         return 0;
  96 }
  97
  98 /**
  99  * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state
 100  * @trig: Trigger instance
 101  * @state: Desired trigger state
 102  */
 103 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
 104                                                 bool state)
 105 {
 106         return inv_mpu6050_set_enable(iio_trigger_get_drvdata(trig), state);
 107 }
 108
 109 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
 110         .owner = THIS_MODULE,
 111         .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
 112 };
 113
 114 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev)
 115 {
 116         int ret;
 117         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 118
 119         st->trig = devm_iio_trigger_alloc(&indio_dev->dev,
 120                                           "%s-dev%d",
 121                                           indio_dev->name,
 122                                           indio_dev->id);
 123         if (!st->trig)
 124                 return -ENOMEM;
 125
 126         ret = devm_request_irq(&indio_dev->dev, st->client->irq,
 127                                &iio_trigger_generic_data_rdy_poll,
 128                                IRQF_TRIGGER_RISING,
 129                                "inv_mpu",
 130                                st->trig);
 131         if (ret)
 132                 return ret;
 133
 134         st->trig->dev.parent = &st->client->dev;
 135         st->trig->ops = &inv_mpu_trigger_ops;
 136         iio_trigger_set_drvdata(st->trig, indio_dev);
 137
 138         ret = iio_trigger_register(st->trig);
 139         if (ret)
 140                 return ret;
 141
 142         indio_dev->trig = iio_trigger_get(st->trig);
 143
 144         return 0;
 145 }
 146
 147 void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st)
 148 {
 149         iio_trigger_unregister(st->trig);
 150 }