1 /* 2 * Copyright (C) 2012 Invensense, Inc. 3 * 4 * This software is licensed under the terms of the GNU General Public 5 * License version 2, as published by the Free Software Foundation, and 6 * may be copied, distributed, and modified under those terms. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 */ 13 14 #include "inv_mpu_iio.h" 15 16 static void inv_scan_query(struct iio_dev *indio_dev) 17 { 18 struct inv_mpu6050_state *st = iio_priv(indio_dev); 19 20 st->chip_config.gyro_fifo_enable = 21 test_bit(INV_MPU6050_SCAN_GYRO_X, 22 indio_dev->active_scan_mask) || 23 test_bit(INV_MPU6050_SCAN_GYRO_Y, 24 indio_dev->active_scan_mask) || 25 test_bit(INV_MPU6050_SCAN_GYRO_Z, 26 indio_dev->active_scan_mask); 27 28 st->chip_config.accl_fifo_enable = 29 test_bit(INV_MPU6050_SCAN_ACCL_X, 30 indio_dev->active_scan_mask) || 31 test_bit(INV_MPU6050_SCAN_ACCL_Y, 32 indio_dev->active_scan_mask) || 33 test_bit(INV_MPU6050_SCAN_ACCL_Z, 34 indio_dev->active_scan_mask); 35 } 36 37 /** 38 * inv_mpu6050_set_enable() - enable chip functions. 39 * @indio_dev: Device driver instance. 40 * @enable: enable/disable 41 */ 42 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) 43 { 44 struct inv_mpu6050_state *st = iio_priv(indio_dev); 45 int result; 46 47 if (enable) { 48 result = inv_mpu6050_set_power_itg(st, true); 49 if (result) 50 return result; 51 inv_scan_query(indio_dev); 52 st->skip_samples = 0; 53 if (st->chip_config.gyro_fifo_enable) { 54 result = inv_mpu6050_switch_engine(st, true, 55 INV_MPU6050_BIT_PWR_GYRO_STBY); 56 if (result) 57 goto error_power_off; 58 /* gyro first sample is out of specs, skip it */ 59 st->skip_samples = 1; 60 } 61 if (st->chip_config.accl_fifo_enable) { 62 result = inv_mpu6050_switch_engine(st, true, 63 INV_MPU6050_BIT_PWR_ACCL_STBY); 64 if (result) 65 goto error_gyro_off; 66 } 67 result = inv_reset_fifo(indio_dev); 68 if (result) 69 goto error_accl_off; 70 } else { 71 result = regmap_write(st->map, st->reg->fifo_en, 0); 72 if (result) 73 goto error_accl_off; 74 75 result = regmap_write(st->map, st->reg->int_enable, 0); 76 if (result) 77 goto error_accl_off; 78 79 result = regmap_write(st->map, st->reg->user_ctrl, 80 st->chip_config.user_ctrl); 81 if (result) 82 goto error_accl_off; 83 84 result = inv_mpu6050_switch_engine(st, false, 85 INV_MPU6050_BIT_PWR_ACCL_STBY); 86 if (result) 87 goto error_accl_off; 88 89 result = inv_mpu6050_switch_engine(st, false, 90 INV_MPU6050_BIT_PWR_GYRO_STBY); 91 if (result) 92 goto error_gyro_off; 93 94 result = inv_mpu6050_set_power_itg(st, false); 95 if (result) 96 goto error_power_off; 97 } 98 99 return 0; 100 101 error_accl_off: 102 if (st->chip_config.accl_fifo_enable) 103 inv_mpu6050_switch_engine(st, false, 104 INV_MPU6050_BIT_PWR_ACCL_STBY); 105 error_gyro_off: 106 if (st->chip_config.gyro_fifo_enable) 107 inv_mpu6050_switch_engine(st, false, 108 INV_MPU6050_BIT_PWR_GYRO_STBY); 109 error_power_off: 110 inv_mpu6050_set_power_itg(st, false); 111 return result; 112 } 113 114 /** 115 * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state 116 * @trig: Trigger instance 117 * @state: Desired trigger state 118 */ 119 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, 120 bool state) 121 { 122 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); 123 struct inv_mpu6050_state *st = iio_priv(indio_dev); 124 int result; 125 126 mutex_lock(&st->lock); 127 result = inv_mpu6050_set_enable(indio_dev, state); 128 mutex_unlock(&st->lock); 129 130 return result; 131 } 132 133 static const struct iio_trigger_ops inv_mpu_trigger_ops = { 134 .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, 135 }; 136 137 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type) 138 { 139 int ret; 140 struct inv_mpu6050_state *st = iio_priv(indio_dev); 141 142 st->trig = devm_iio_trigger_alloc(&indio_dev->dev, 143 "%s-dev%d", 144 indio_dev->name, 145 indio_dev->id); 146 if (!st->trig) 147 return -ENOMEM; 148 149 ret = devm_request_irq(&indio_dev->dev, st->irq, 150 &iio_trigger_generic_data_rdy_poll, 151 irq_type, 152 "inv_mpu", 153 st->trig); 154 if (ret) 155 return ret; 156 157 st->trig->dev.parent = regmap_get_device(st->map); 158 st->trig->ops = &inv_mpu_trigger_ops; 159 iio_trigger_set_drvdata(st->trig, indio_dev); 160 161 ret = devm_iio_trigger_register(&indio_dev->dev, st->trig); 162 if (ret) 163 return ret; 164 165 indio_dev->trig = iio_trigger_get(st->trig); 166 167 return 0; 168 } 169