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