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                 st->skip_samples = 0;
  53                 if (st->chip_config.gyro_fifo_enable) {
  54                         result = inv_mpu6050_switch_engine(st, true,
  55                                         INV_MPU6050_BIT_PWR_GYRO_STBY);
  56                         if (result)
  57                                 goto error_power_off;
  58                         /* gyro first sample is out of specs, skip it */
  59                         st->skip_samples = 1;
  60                 }
  61                 if (st->chip_config.accl_fifo_enable) {
  62                         result = inv_mpu6050_switch_engine(st, true,
  63                                         INV_MPU6050_BIT_PWR_ACCL_STBY);
  64                         if (result)
  65                                 goto error_gyro_off;
  66                 }
  67                 result = inv_reset_fifo(indio_dev);
  68                 if (result)
  69                         goto error_accl_off;
  70         } else {
  71                 result = regmap_write(st->map, st->reg->fifo_en, 0);
  72                 if (result)
  73                         goto error_accl_off;
  74
  75                 result = regmap_write(st->map, st->reg->int_enable, 0);
  76                 if (result)
  77                         goto error_accl_off;
  78
  79                 result = regmap_write(st->map, st->reg->user_ctrl,
  80                                       st->chip_config.user_ctrl);
  81                 if (result)
  82                         goto error_accl_off;
  83
  84                 result = inv_mpu6050_switch_engine(st, false,
  85                                         INV_MPU6050_BIT_PWR_ACCL_STBY);
  86                 if (result)
  87                         goto error_accl_off;
  88
  89                 result = inv_mpu6050_switch_engine(st, false,
  90                                         INV_MPU6050_BIT_PWR_GYRO_STBY);
  91                 if (result)
  92                         goto error_gyro_off;
  93
  94                 result = inv_mpu6050_set_power_itg(st, false);
  95                 if (result)
  96                         goto error_power_off;
  97         }
  98
  99         return 0;
 100
 101 error_accl_off:
 102         if (st->chip_config.accl_fifo_enable)
 103                 inv_mpu6050_switch_engine(st, false,
 104                                           INV_MPU6050_BIT_PWR_ACCL_STBY);
 105 error_gyro_off:
 106         if (st->chip_config.gyro_fifo_enable)
 107                 inv_mpu6050_switch_engine(st, false,
 108                                           INV_MPU6050_BIT_PWR_GYRO_STBY);
 109 error_power_off:
 110         inv_mpu6050_set_power_itg(st, false);
 111         return result;
 112 }
 113
 114 /**
 115  * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state
 116  * @trig: Trigger instance
 117  * @state: Desired trigger state
 118  */
 119 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
 120                                               bool state)
 121 {
 122         struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 123         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 124         int result;
 125
 126         mutex_lock(&st->lock);
 127         result = inv_mpu6050_set_enable(indio_dev, state);
 128         mutex_unlock(&st->lock);
 129
 130         return result;
 131 }
 132
 133 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
 134         .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
 135 };
 136
 137 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type)
 138 {
 139         int ret;
 140         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 141
 142         st->trig = devm_iio_trigger_alloc(&indio_dev->dev,
 143                                           "%s-dev%d",
 144                                           indio_dev->name,
 145                                           indio_dev->id);
 146         if (!st->trig)
 147                 return -ENOMEM;
 148
 149         ret = devm_request_irq(&indio_dev->dev, st->irq,
 150                                &iio_trigger_generic_data_rdy_poll,
 151                                irq_type,
 152                                "inv_mpu",
 153                                st->trig);
 154         if (ret)
 155                 return ret;
 156
 157         st->trig->dev.parent = regmap_get_device(st->map);
 158         st->trig->ops = &inv_mpu_trigger_ops;
 159         iio_trigger_set_drvdata(st->trig, indio_dev);
 160
 161         ret = devm_iio_trigger_register(&indio_dev->dev, st->trig);
 162         if (ret)
 163                 return ret;
 164
 165         indio_dev->trig = iio_trigger_get(st->trig);
 166
 167         return 0;
 168 }