drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3 * Copyright (C) 2012 Invensense, Inc.
   4 */
   5
   6 #include <linux/pm_runtime.h>
   7 #include "inv_mpu_iio.h"
   8
   9 static unsigned int inv_scan_query_mpu6050(struct iio_dev *indio_dev)
  10 {
  11         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
  12         unsigned int mask;
  13
  14         st->chip_config.gyro_fifo_enable =
  15                 test_bit(INV_MPU6050_SCAN_GYRO_X,
  16                          indio_dev->active_scan_mask) ||
  17                 test_bit(INV_MPU6050_SCAN_GYRO_Y,
  18                          indio_dev->active_scan_mask) ||
  19                 test_bit(INV_MPU6050_SCAN_GYRO_Z,
  20                          indio_dev->active_scan_mask);
  21
  22         st->chip_config.accl_fifo_enable =
  23                 test_bit(INV_MPU6050_SCAN_ACCL_X,
  24                          indio_dev->active_scan_mask) ||
  25                 test_bit(INV_MPU6050_SCAN_ACCL_Y,
  26                          indio_dev->active_scan_mask) ||
  27                 test_bit(INV_MPU6050_SCAN_ACCL_Z,
  28                          indio_dev->active_scan_mask);
  29
  30         st->chip_config.temp_fifo_enable =
  31                 test_bit(INV_MPU6050_SCAN_TEMP, indio_dev->active_scan_mask);
  32
  33         mask = 0;
  34         if (st->chip_config.gyro_fifo_enable)
  35                 mask |= INV_MPU6050_SENSOR_GYRO;
  36         if (st->chip_config.accl_fifo_enable)
  37                 mask |= INV_MPU6050_SENSOR_ACCL;
  38         if (st->chip_config.temp_fifo_enable)
  39                 mask |= INV_MPU6050_SENSOR_TEMP;
  40
  41         return mask;
  42 }
  43
  44 static unsigned int inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
  45 {
  46         struct inv_mpu6050_state *st = iio_priv(indio_dev);
  47         unsigned int mask;
  48
  49         mask = inv_scan_query_mpu6050(indio_dev);
  50
  51         /* no magnetometer if i2c auxiliary bus is used */
  52         if (st->magn_disabled)
  53                 return mask;
  54
  55         st->chip_config.magn_fifo_enable =
  56                 test_bit(INV_MPU9X50_SCAN_MAGN_X,
  57                          indio_dev->active_scan_mask) ||
  58                 test_bit(INV_MPU9X50_SCAN_MAGN_Y,
  59                          indio_dev->active_scan_mask) ||
  60                 test_bit(INV_MPU9X50_SCAN_MAGN_Z,
  61                          indio_dev->active_scan_mask);
  62         if (st->chip_config.magn_fifo_enable)
  63                 mask |= INV_MPU6050_SENSOR_MAGN;
  64
  65         return mask;
  66 }
  67
  68 static unsigned int inv_scan_query(struct iio_dev *indio_dev)
  69 {
  70         struct inv_mpu6050_state *st = iio_priv(indio_dev);
  71
  72         switch (st->chip_type) {
  73         case INV_MPU9150:
  74         case INV_MPU9250:
  75         case INV_MPU9255:
  76                 return inv_scan_query_mpu9x50(indio_dev);
  77         default:
  78                 return inv_scan_query_mpu6050(indio_dev);
  79         }
  80 }
  81
  82 static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
  83 {
  84         unsigned int gyro_skip = 0;
  85         unsigned int magn_skip = 0;
  86         unsigned int skip_samples;
  87
  88         /* gyro first sample is out of specs, skip it */
  89         if (st->chip_config.gyro_fifo_enable)
  90                 gyro_skip = 1;
  91
  92         /* mag first sample is always not ready, skip it */
  93         if (st->chip_config.magn_fifo_enable)
  94                 magn_skip = 1;
  95
  96         /* compute first samples to skip */
  97         skip_samples = gyro_skip;
  98         if (magn_skip > skip_samples)
  99                 skip_samples = magn_skip;
 100
 101         return skip_samples;
 102 }
 103
 104 int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
 105 {
 106         uint8_t d;
 107         int ret;
 108
 109         if (enable) {
 110                 st->it_timestamp = 0;
 111                 /* reset FIFO */
 112                 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
 113                 ret = regmap_write(st->map, st->reg->user_ctrl, d);
 114                 if (ret)
 115                         return ret;
 116                 /* enable sensor output to FIFO */
 117                 d = 0;
 118                 if (st->chip_config.gyro_fifo_enable)
 119                         d |= INV_MPU6050_BITS_GYRO_OUT;
 120                 if (st->chip_config.accl_fifo_enable)
 121                         d |= INV_MPU6050_BIT_ACCEL_OUT;
 122                 if (st->chip_config.temp_fifo_enable)
 123                         d |= INV_MPU6050_BIT_TEMP_OUT;
 124                 if (st->chip_config.magn_fifo_enable)
 125                         d |= INV_MPU6050_BIT_SLAVE_0;
 126                 ret = regmap_write(st->map, st->reg->fifo_en, d);
 127                 if (ret)
 128                         return ret;
 129                 /* enable FIFO reading */
 130                 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
 131                 ret = regmap_write(st->map, st->reg->user_ctrl, d);
 132                 if (ret)
 133                         return ret;
 134                 /* enable interrupt */
 135                 ret = regmap_write(st->map, st->reg->int_enable,
 136                                    INV_MPU6050_BIT_DATA_RDY_EN);
 137         } else {
 138                 ret = regmap_write(st->map, st->reg->int_enable, 0);
 139                 if (ret)
 140                         return ret;
 141                 ret = regmap_write(st->map, st->reg->fifo_en, 0);
 142                 if (ret)
 143                         return ret;
 144                 /* restore user_ctrl for disabling FIFO reading */
 145                 ret = regmap_write(st->map, st->reg->user_ctrl,
 146                                    st->chip_config.user_ctrl);
 147         }
 148
 149         return ret;
 150 }
 151
 152 /**
 153  *  inv_mpu6050_set_enable() - enable chip functions.
 154  *  @indio_dev: Device driver instance.
 155  *  @enable: enable/disable
 156  */
 157 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 158 {
 159         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 160         struct device *pdev = regmap_get_device(st->map);
 161         unsigned int scan;
 162         int result;
 163
 164         if (enable) {
 165                 scan = inv_scan_query(indio_dev);
 166                 result = pm_runtime_get_sync(pdev);
 167                 if (result < 0) {
 168                         pm_runtime_put_noidle(pdev);
 169                         return result;
 170                 }
 171                 /*
 172                  * In case autosuspend didn't trigger, turn off first not
 173                  * required sensors.
 174                  */
 175                 result = inv_mpu6050_switch_engine(st, false, ~scan);
 176                 if (result)
 177                         goto error_power_off;
 178                 result = inv_mpu6050_switch_engine(st, true, scan);
 179                 if (result)
 180                         goto error_power_off;
 181                 st->skip_samples = inv_compute_skip_samples(st);
 182                 result = inv_mpu6050_prepare_fifo(st, true);
 183                 if (result)
 184                         goto error_power_off;
 185         } else {
 186                 result = inv_mpu6050_prepare_fifo(st, false);
 187                 if (result)
 188                         goto error_power_off;
 189                 pm_runtime_mark_last_busy(pdev);
 190                 pm_runtime_put_autosuspend(pdev);
 191         }
 192
 193         return 0;
 194
 195 error_power_off:
 196         pm_runtime_put_autosuspend(pdev);
 197         return result;
 198 }
 199
 200 /**
 201  * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state
 202  * @trig: Trigger instance
 203  * @state: Desired trigger state
 204  */
 205 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
 206                                               bool state)
 207 {
 208         struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 209         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 210         int result;
 211
 212         mutex_lock(&st->lock);
 213         result = inv_mpu6050_set_enable(indio_dev, state);
 214         mutex_unlock(&st->lock);
 215
 216         return result;
 217 }
 218
 219 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
 220         .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
 221 };
 222
 223 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type)
 224 {
 225         int ret;
 226         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 227
 228         st->trig = devm_iio_trigger_alloc(&indio_dev->dev,
 229                                           "%s-dev%d",
 230                                           indio_dev->name,
 231                                           indio_dev->id);
 232         if (!st->trig)
 233                 return -ENOMEM;
 234
 235         ret = devm_request_irq(&indio_dev->dev, st->irq,
 236                                &iio_trigger_generic_data_rdy_poll,
 237                                irq_type,
 238                                "inv_mpu",
 239                                st->trig);
 240         if (ret)
 241                 return ret;
 242
 243         st->trig->dev.parent = regmap_get_device(st->map);
 244         st->trig->ops = &inv_mpu_trigger_ops;
 245         iio_trigger_set_drvdata(st->trig, indio_dev);
 246
 247         ret = devm_iio_trigger_register(&indio_dev->dev, st->trig);
 248         if (ret)
 249                 return ret;
 250
 251         indio_dev->trig = iio_trigger_get(st->trig);
 252
 253         return 0;
 254 }