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(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 /** 30 * inv_mpu6050_set_enable() - enable chip functions. 31 * @indio_dev: Device driver instance. 32 * @enable: enable/disable 33 */ 34 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) 35 { 36 struct inv_mpu6050_state *st = iio_priv(indio_dev); 37 int result; 38 39 if (enable) { 40 result = inv_mpu6050_set_power_itg(st, true); 41 if (result) 42 return result; 43 inv_scan_query(indio_dev); 44 st->skip_samples = 0; 45 if (st->chip_config.gyro_fifo_enable) { 46 result = inv_mpu6050_switch_engine(st, true, 47 INV_MPU6050_BIT_PWR_GYRO_STBY); 48 if (result) 49 goto error_power_off; 50 /* gyro first sample is out of specs, skip it */ 51 st->skip_samples = 1; 52 } 53 if (st->chip_config.accl_fifo_enable) { 54 result = inv_mpu6050_switch_engine(st, true, 55 INV_MPU6050_BIT_PWR_ACCL_STBY); 56 if (result) 57 goto error_gyro_off; 58 } 59 result = inv_reset_fifo(indio_dev); 60 if (result) 61 goto error_accl_off; 62 } else { 63 result = regmap_write(st->map, st->reg->fifo_en, 0); 64 if (result) 65 goto error_accl_off; 66 67 result = regmap_write(st->map, st->reg->int_enable, 0); 68 if (result) 69 goto error_accl_off; 70 71 result = regmap_write(st->map, st->reg->user_ctrl, 72 st->chip_config.user_ctrl); 73 if (result) 74 goto error_accl_off; 75 76 result = inv_mpu6050_switch_engine(st, false, 77 INV_MPU6050_BIT_PWR_ACCL_STBY); 78 if (result) 79 goto error_accl_off; 80 81 result = inv_mpu6050_switch_engine(st, false, 82 INV_MPU6050_BIT_PWR_GYRO_STBY); 83 if (result) 84 goto error_gyro_off; 85 86 result = inv_mpu6050_set_power_itg(st, false); 87 if (result) 88 goto error_power_off; 89 } 90 91 return 0; 92 93 error_accl_off: 94 if (st->chip_config.accl_fifo_enable) 95 inv_mpu6050_switch_engine(st, false, 96 INV_MPU6050_BIT_PWR_ACCL_STBY); 97 error_gyro_off: 98 if (st->chip_config.gyro_fifo_enable) 99 inv_mpu6050_switch_engine(st, false, 100 INV_MPU6050_BIT_PWR_GYRO_STBY); 101 error_power_off: 102 inv_mpu6050_set_power_itg(st, false); 103 return result; 104 } 105 106 /** 107 * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state 108 * @trig: Trigger instance 109 * @state: Desired trigger state 110 */ 111 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, 112 bool state) 113 { 114 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); 115 struct inv_mpu6050_state *st = iio_priv(indio_dev); 116 int result; 117 118 mutex_lock(&st->lock); 119 result = inv_mpu6050_set_enable(indio_dev, state); 120 mutex_unlock(&st->lock); 121 122 return result; 123 } 124 125 static const struct iio_trigger_ops inv_mpu_trigger_ops = { 126 .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, 127 }; 128 129 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type) 130 { 131 int ret; 132 struct inv_mpu6050_state *st = iio_priv(indio_dev); 133 134 st->trig = devm_iio_trigger_alloc(&indio_dev->dev, 135 "%s-dev%d", 136 indio_dev->name, 137 indio_dev->id); 138 if (!st->trig) 139 return -ENOMEM; 140 141 ret = devm_request_irq(&indio_dev->dev, st->irq, 142 &iio_trigger_generic_data_rdy_poll, 143 irq_type, 144 "inv_mpu", 145 st->trig); 146 if (ret) 147 return ret; 148 149 st->trig->dev.parent = regmap_get_device(st->map); 150 st->trig->ops = &inv_mpu_trigger_ops; 151 iio_trigger_set_drvdata(st->trig, indio_dev); 152 153 ret = devm_iio_trigger_register(&indio_dev->dev, st->trig); 154 if (ret) 155 return ret; 156 157 indio_dev->trig = iio_trigger_get(st->trig); 158 159 return 0; 160 } 161