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