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