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_resume_and_get(pdev); 177 if (result) 178 return result; 179 /* 180 * In case autosuspend didn't trigger, turn off first not 181 * required sensors. 182 */ 183 result = inv_mpu6050_switch_engine(st, false, ~scan); 184 if (result) 185 goto error_power_off; 186 result = inv_mpu6050_switch_engine(st, true, scan); 187 if (result) 188 goto error_power_off; 189 st->skip_samples = inv_compute_skip_samples(st); 190 result = inv_mpu6050_prepare_fifo(st, true); 191 if (result) 192 goto error_power_off; 193 } else { 194 result = inv_mpu6050_prepare_fifo(st, false); 195 if (result) 196 goto error_power_off; 197 pm_runtime_mark_last_busy(pdev); 198 pm_runtime_put_autosuspend(pdev); 199 } 200 201 return 0; 202 203 error_power_off: 204 pm_runtime_put_autosuspend(pdev); 205 return result; 206 } 207 208 /** 209 * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state 210 * @trig: Trigger instance 211 * @state: Desired trigger state 212 */ 213 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, 214 bool state) 215 { 216 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); 217 struct inv_mpu6050_state *st = iio_priv(indio_dev); 218 int result; 219 220 mutex_lock(&st->lock); 221 result = inv_mpu6050_set_enable(indio_dev, state); 222 mutex_unlock(&st->lock); 223 224 return result; 225 } 226 227 static const struct iio_trigger_ops inv_mpu_trigger_ops = { 228 .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, 229 }; 230 231 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type) 232 { 233 int ret; 234 struct inv_mpu6050_state *st = iio_priv(indio_dev); 235 236 st->trig = devm_iio_trigger_alloc(&indio_dev->dev, 237 "%s-dev%d", 238 indio_dev->name, 239 iio_device_id(indio_dev)); 240 if (!st->trig) 241 return -ENOMEM; 242 243 ret = devm_request_irq(&indio_dev->dev, st->irq, 244 &iio_trigger_generic_data_rdy_poll, 245 irq_type, 246 "inv_mpu", 247 st->trig); 248 if (ret) 249 return ret; 250 251 st->trig->dev.parent = regmap_get_device(st->map); 252 st->trig->ops = &inv_mpu_trigger_ops; 253 iio_trigger_set_drvdata(st->trig, indio_dev); 254 255 ret = devm_iio_trigger_register(&indio_dev->dev, st->trig); 256 if (ret) 257 return ret; 258 259 indio_dev->trig = iio_trigger_get(st->trig); 260 261 return 0; 262 } 263