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 } 227