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 "inv_mpu_iio.h"
   7
   8 static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
   9 {
  10         struct inv_mpu6050_state  *st = iio_priv(indio_dev);
  11
  12         st->chip_config.gyro_fifo_enable =
  13                 test_bit(INV_MPU6050_SCAN_GYRO_X,
  14                          indio_dev->active_scan_mask) ||
  15                 test_bit(INV_MPU6050_SCAN_GYRO_Y,
  16                          indio_dev->active_scan_mask) ||
  17                 test_bit(INV_MPU6050_SCAN_GYRO_Z,
  18                          indio_dev->active_scan_mask);
  19
  20         st->chip_config.accl_fifo_enable =
  21                 test_bit(INV_MPU6050_SCAN_ACCL_X,
  22                          indio_dev->active_scan_mask) ||
  23                 test_bit(INV_MPU6050_SCAN_ACCL_Y,
  24                          indio_dev->active_scan_mask) ||
  25                 test_bit(INV_MPU6050_SCAN_ACCL_Z,
  26                          indio_dev->active_scan_mask);
  27 }
  28
  29 static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
  30 {
  31         struct inv_mpu6050_state *st = iio_priv(indio_dev);
  32
  33         inv_scan_query_mpu6050(indio_dev);
  34
  35         /* no magnetometer if i2c auxiliary bus is used */
  36         if (st->magn_disabled)
  37                 return;
  38
  39         st->chip_config.magn_fifo_enable =
  40                 test_bit(INV_MPU9X50_SCAN_MAGN_X,
  41                          indio_dev->active_scan_mask) ||
  42                 test_bit(INV_MPU9X50_SCAN_MAGN_Y,
  43                          indio_dev->active_scan_mask) ||
  44                 test_bit(INV_MPU9X50_SCAN_MAGN_Z,
  45                          indio_dev->active_scan_mask);
  46 }
  47
  48 static void inv_scan_query(struct iio_dev *indio_dev)
  49 {
  50         struct inv_mpu6050_state *st = iio_priv(indio_dev);
  51
  52         switch (st->chip_type) {
  53         case INV_MPU9250:
  54         case INV_MPU9255:
  55                 return inv_scan_query_mpu9x50(indio_dev);
  56         default:
  57                 return inv_scan_query_mpu6050(indio_dev);
  58         }
  59 }
  60
  61 static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
  62 {
  63         unsigned int gyro_skip = 0;
  64         unsigned int magn_skip = 0;
  65         unsigned int skip_samples;
  66
  67         /* gyro first sample is out of specs, skip it */
  68         if (st->chip_config.gyro_fifo_enable)
  69                 gyro_skip = 1;
  70
  71         /* mag first sample is always not ready, skip it */
  72         if (st->chip_config.magn_fifo_enable)
  73                 magn_skip = 1;
  74
  75         /* compute first samples to skip */
  76         skip_samples = gyro_skip;
  77         if (magn_skip > skip_samples)
  78                 skip_samples = magn_skip;
  79
  80         return skip_samples;
  81 }
  82
  83 /**
  84  *  inv_mpu6050_set_enable() - enable chip functions.
  85  *  @indio_dev: Device driver instance.
  86  *  @enable: enable/disable
  87  */
  88 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
  89 {
  90         struct inv_mpu6050_state *st = iio_priv(indio_dev);
  91         uint8_t d;
  92         int result;
  93
  94         if (enable) {
  95                 result = inv_mpu6050_set_power_itg(st, true);
  96                 if (result)
  97                         return result;
  98                 inv_scan_query(indio_dev);
  99                 if (st->chip_config.gyro_fifo_enable) {
 100                         result = inv_mpu6050_switch_engine(st, true,
 101                                         INV_MPU6050_BIT_PWR_GYRO_STBY);
 102                         if (result)
 103                                 goto error_power_off;
 104                 }
 105                 if (st->chip_config.accl_fifo_enable) {
 106                         result = inv_mpu6050_switch_engine(st, true,
 107                                         INV_MPU6050_BIT_PWR_ACCL_STBY);
 108                         if (result)
 109                                 goto error_gyro_off;
 110                 }
 111                 if (st->chip_config.magn_fifo_enable) {
 112                         d = st->chip_config.user_ctrl |
 113                                         INV_MPU6050_BIT_I2C_MST_EN;
 114                         result = regmap_write(st->map, st->reg->user_ctrl, d);
 115                         if (result)
 116                                 goto error_accl_off;
 117                         st->chip_config.user_ctrl = d;
 118                 }
 119                 st->skip_samples = inv_compute_skip_samples(st);
 120                 result = inv_reset_fifo(indio_dev);
 121                 if (result)
 122                         goto error_magn_off;
 123         } else {
 124                 result = regmap_write(st->map, st->reg->fifo_en, 0);
 125                 if (result)
 126                         goto error_magn_off;
 127
 128                 result = regmap_write(st->map, st->reg->int_enable, 0);
 129                 if (result)
 130                         goto error_magn_off;
 131
 132                 d = st->chip_config.user_ctrl & ~INV_MPU6050_BIT_I2C_MST_EN;
 133                 result = regmap_write(st->map, st->reg->user_ctrl, d);
 134                 if (result)
 135                         goto error_magn_off;
 136                 st->chip_config.user_ctrl = d;
 137
 138                 result = inv_mpu6050_switch_engine(st, false,
 139                                         INV_MPU6050_BIT_PWR_ACCL_STBY);
 140                 if (result)
 141                         goto error_accl_off;
 142
 143                 result = inv_mpu6050_switch_engine(st, false,
 144                                         INV_MPU6050_BIT_PWR_GYRO_STBY);
 145                 if (result)
 146                         goto error_gyro_off;
 147
 148                 result = inv_mpu6050_set_power_itg(st, false);
 149                 if (result)
 150                         goto error_power_off;
 151         }
 152
 153         return 0;
 154
 155 error_magn_off:
 156         /* always restore user_ctrl to disable fifo properly */
 157         st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;
 158         regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
 159 error_accl_off:
 160         if (st->chip_config.accl_fifo_enable)
 161                 inv_mpu6050_switch_engine(st, false,
 162                                           INV_MPU6050_BIT_PWR_ACCL_STBY);
 163 error_gyro_off:
 164         if (st->chip_config.gyro_fifo_enable)
 165                 inv_mpu6050_switch_engine(st, false,
 166                                           INV_MPU6050_BIT_PWR_GYRO_STBY);
 167 error_power_off:
 168         inv_mpu6050_set_power_itg(st, false);
 169         return result;
 170 }
 171
 172 /**
 173  * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state
 174  * @trig: Trigger instance
 175  * @state: Desired trigger state
 176  */
 177 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
 178                                               bool state)
 179 {
 180         struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 181         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 182         int result;
 183
 184         mutex_lock(&st->lock);
 185         result = inv_mpu6050_set_enable(indio_dev, state);
 186         mutex_unlock(&st->lock);
 187
 188         return result;
 189 }
 190
 191 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
 192         .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
 193 };
 194
 195 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type)
 196 {
 197         int ret;
 198         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 199
 200         st->trig = devm_iio_trigger_alloc(&indio_dev->dev,
 201                                           "%s-dev%d",
 202                                           indio_dev->name,
 203                                           indio_dev->id);
 204         if (!st->trig)
 205                 return -ENOMEM;
 206
 207         ret = devm_request_irq(&indio_dev->dev, st->irq,
 208                                &iio_trigger_generic_data_rdy_poll,
 209                                irq_type,
 210                                "inv_mpu",
 211                                st->trig);
 212         if (ret)
 213                 return ret;
 214
 215         st->trig->dev.parent = regmap_get_device(st->map);
 216         st->trig->ops = &inv_mpu_trigger_ops;
 217         iio_trigger_set_drvdata(st->trig, indio_dev);
 218
 219         ret = devm_iio_trigger_register(&indio_dev->dev, st->trig);
 220         if (ret)
 221                 return ret;
 222
 223         indio_dev->trig = iio_trigger_get(st->trig);
 224
 225         return 0;
 226 }