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