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 = iio_trigger_alloc("%s-dev%d",
 120                                         indio_dev->name,
 121                                         indio_dev->id);
 122         if (st->trig == NULL) {
 123                 ret = -ENOMEM;
 124                 goto error_ret;
 125         }
 126         ret = request_irq(st->client->irq, &iio_trigger_generic_data_rdy_poll,
 127                                 IRQF_TRIGGER_RISING,
 128                                 "inv_mpu",
 129                                 st->trig);
 130         if (ret)
 131                 goto error_free_trig;
 132         st->trig->dev.parent = &st->client->dev;
 133         st->trig->ops = &inv_mpu_trigger_ops;
 134         iio_trigger_set_drvdata(st->trig, indio_dev);
 135         ret = iio_trigger_register(st->trig);
 136         if (ret)
 137                 goto error_free_irq;
 138         indio_dev->trig = st->trig;
 139
 140         return 0;
 141
 142 error_free_irq:
 143         free_irq(st->client->irq, st->trig);
 144 error_free_trig:
 145         iio_trigger_free(st->trig);
 146 error_ret:
 147         return ret;
 148 }
 149
 150 void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st)
 151 {
 152         iio_trigger_unregister(st->trig);
 153         free_irq(st->client->irq, st->trig);
 154         iio_trigger_free(st->trig);
 155 }