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 = regmap_write(st->map, st->reg->fifo_en, 0);
  69                 if (result)
  70                         return result;
  71
  72                 result = regmap_write(st->map, st->reg->int_enable, 0);
  73                 if (result)
  74                         return result;
  75
  76                 result = regmap_write(st->map, 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
  94         return 0;
  95 }
  96
  97 /**
  98  * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state
  99  * @trig: Trigger instance
 100  * @state: Desired trigger state
 101  */
 102 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
 103                                               bool state)
 104 {
 105         struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 106         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 107         int result;
 108
 109         mutex_lock(&st->lock);
 110         result = inv_mpu6050_set_enable(indio_dev, state);
 111         mutex_unlock(&st->lock);
 112
 113         return result;
 114 }
 115
 116 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
 117         .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
 118 };
 119
 120 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev)
 121 {
 122         int ret;
 123         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 124
 125         st->trig = devm_iio_trigger_alloc(&indio_dev->dev,
 126                                           "%s-dev%d",
 127                                           indio_dev->name,
 128                                           indio_dev->id);
 129         if (!st->trig)
 130                 return -ENOMEM;
 131
 132         ret = devm_request_irq(&indio_dev->dev, st->irq,
 133                                &iio_trigger_generic_data_rdy_poll,
 134                                IRQF_TRIGGER_RISING,
 135                                "inv_mpu",
 136                                st->trig);
 137         if (ret)
 138                 return ret;
 139
 140         st->trig->dev.parent = regmap_get_device(st->map);
 141         st->trig->ops = &inv_mpu_trigger_ops;
 142         iio_trigger_set_drvdata(st->trig, indio_dev);
 143
 144         ret = iio_trigger_register(st->trig);
 145         if (ret)
 146                 return ret;
 147
 148         indio_dev->trig = iio_trigger_get(st->trig);
 149
 150         return 0;
 151 }
 152
 153 void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st)
 154 {
 155         iio_trigger_unregister(st->trig);
 156 }