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 gyro_skip = 0; 95 unsigned int magn_skip = 0; 96 unsigned int skip_samples; 97 98 /* gyro first sample is out of specs, skip it */ 99 if (st->chip_config.gyro_fifo_enable) 100 gyro_skip = 1; 101 102 /* mag first sample is always not ready, skip it */ 103 if (st->chip_config.magn_fifo_enable) 104 magn_skip = 1; 105 106 /* compute first samples to skip */ 107 skip_samples = gyro_skip; 108 if (magn_skip > skip_samples) 109 skip_samples = magn_skip; 110 111 return skip_samples; 112 } 113 114 int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable) 115 { 116 uint8_t d; 117 int ret; 118 119 if (enable) { 120 st->it_timestamp = 0; 121 /* reset FIFO */ 122 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST; 123 ret = regmap_write(st->map, st->reg->user_ctrl, d); 124 if (ret) 125 return ret; 126 /* enable sensor output to FIFO */ 127 d = 0; 128 if (st->chip_config.gyro_fifo_enable) 129 d |= INV_MPU6050_BITS_GYRO_OUT; 130 if (st->chip_config.accl_fifo_enable) 131 d |= INV_MPU6050_BIT_ACCEL_OUT; 132 if (st->chip_config.temp_fifo_enable) 133 d |= INV_MPU6050_BIT_TEMP_OUT; 134 if (st->chip_config.magn_fifo_enable) 135 d |= INV_MPU6050_BIT_SLAVE_0; 136 ret = regmap_write(st->map, st->reg->fifo_en, d); 137 if (ret) 138 return ret; 139 /* enable FIFO reading */ 140 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN; 141 ret = regmap_write(st->map, st->reg->user_ctrl, d); 142 if (ret) 143 return ret; 144 /* enable interrupt */ 145 ret = regmap_write(st->map, st->reg->int_enable, 146 INV_MPU6050_BIT_DATA_RDY_EN); 147 } else { 148 ret = regmap_write(st->map, st->reg->int_enable, 0); 149 if (ret) 150 return ret; 151 ret = regmap_write(st->map, st->reg->fifo_en, 0); 152 if (ret) 153 return ret; 154 /* restore user_ctrl for disabling FIFO reading */ 155 ret = regmap_write(st->map, st->reg->user_ctrl, 156 st->chip_config.user_ctrl); 157 } 158 159 return ret; 160 } 161 162 /** 163 * inv_mpu6050_set_enable() - enable chip functions. 164 * @indio_dev: Device driver instance. 165 * @enable: enable/disable 166 */ 167 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) 168 { 169 struct inv_mpu6050_state *st = iio_priv(indio_dev); 170 struct device *pdev = regmap_get_device(st->map); 171 unsigned int scan; 172 int result; 173 174 if (enable) { 175 scan = inv_scan_query(indio_dev); 176 result = pm_runtime_get_sync(pdev); 177 if (result < 0) { 178 pm_runtime_put_noidle(pdev); 179 return result; 180 } 181 /* 182 * In case autosuspend didn't trigger, turn off first not 183 * required sensors. 184 */ 185 result = inv_mpu6050_switch_engine(st, false, ~scan); 186 if (result) 187 goto error_power_off; 188 result = inv_mpu6050_switch_engine(st, true, scan); 189 if (result) 190 goto error_power_off; 191 st->skip_samples = inv_compute_skip_samples(st); 192 result = inv_mpu6050_prepare_fifo(st, true); 193 if (result) 194 goto error_power_off; 195 } else { 196 result = inv_mpu6050_prepare_fifo(st, false); 197 if (result) 198 goto error_power_off; 199 pm_runtime_mark_last_busy(pdev); 200 pm_runtime_put_autosuspend(pdev); 201 } 202 203 return 0; 204 205 error_power_off: 206 pm_runtime_put_autosuspend(pdev); 207 return result; 208 } 209 210 /** 211 * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state 212 * @trig: Trigger instance 213 * @state: Desired trigger state 214 */ 215 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, 216 bool state) 217 { 218 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); 219 struct inv_mpu6050_state *st = iio_priv(indio_dev); 220 int result; 221 222 mutex_lock(&st->lock); 223 result = inv_mpu6050_set_enable(indio_dev, state); 224 mutex_unlock(&st->lock); 225 226 return result; 227 } 228 229 static const struct iio_trigger_ops inv_mpu_trigger_ops = { 230 .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, 231 }; 232 233 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type) 234 { 235 int ret; 236 struct inv_mpu6050_state *st = iio_priv(indio_dev); 237 238 st->trig = devm_iio_trigger_alloc(&indio_dev->dev, 239 "%s-dev%d", 240 indio_dev->name, 241 indio_dev->id); 242 if (!st->trig) 243 return -ENOMEM; 244 245 ret = devm_request_irq(&indio_dev->dev, st->irq, 246 &iio_trigger_generic_data_rdy_poll, 247 irq_type, 248 "inv_mpu", 249 st->trig); 250 if (ret) 251 return ret; 252 253 st->trig->dev.parent = regmap_get_device(st->map); 254 st->trig->ops = &inv_mpu_trigger_ops; 255 iio_trigger_set_drvdata(st->trig, indio_dev); 256 257 ret = devm_iio_trigger_register(&indio_dev->dev, st->trig); 258 if (ret) 259 return ret; 260 261 indio_dev->trig = iio_trigger_get(st->trig); 262 263 return 0; 264 } 265