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