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 if (st->chip_config.gyro_fifo_enable) { 53 result = inv_mpu6050_switch_engine(st, true, 54 INV_MPU6050_BIT_PWR_GYRO_STBY); 55 if (result) 56 return result; 57 } 58 if (st->chip_config.accl_fifo_enable) { 59 result = inv_mpu6050_switch_engine(st, true, 60 INV_MPU6050_BIT_PWR_ACCL_STBY); 61 if (result) 62 return result; 63 } 64 result = inv_reset_fifo(indio_dev); 65 if (result) 66 return result; 67 } else { 68 result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0); 69 if (result) 70 return result; 71 72 result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0); 73 if (result) 74 return result; 75 76 result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0); 77 if (result) 78 return result; 79 80 result = inv_mpu6050_switch_engine(st, false, 81 INV_MPU6050_BIT_PWR_GYRO_STBY); 82 if (result) 83 return result; 84 85 result = inv_mpu6050_switch_engine(st, false, 86 INV_MPU6050_BIT_PWR_ACCL_STBY); 87 if (result) 88 return result; 89 result = inv_mpu6050_set_power_itg(st, false); 90 if (result) 91 return result; 92 } 93 st->chip_config.enable = enable; 94 95 return 0; 96 } 97 98 /** 99 * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state 100 * @trig: Trigger instance 101 * @state: Desired trigger state 102 */ 103 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, 104 bool state) 105 { 106 return inv_mpu6050_set_enable(iio_trigger_get_drvdata(trig), state); 107 } 108 109 static const struct iio_trigger_ops inv_mpu_trigger_ops = { 110 .owner = THIS_MODULE, 111 .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, 112 }; 113 114 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev) 115 { 116 int ret; 117 struct inv_mpu6050_state *st = iio_priv(indio_dev); 118 119 st->trig = iio_trigger_alloc("%s-dev%d", 120 indio_dev->name, 121 indio_dev->id); 122 if (st->trig == NULL) { 123 ret = -ENOMEM; 124 goto error_ret; 125 } 126 ret = request_irq(st->client->irq, &iio_trigger_generic_data_rdy_poll, 127 IRQF_TRIGGER_RISING, 128 "inv_mpu", 129 st->trig); 130 if (ret) 131 goto error_free_trig; 132 st->trig->dev.parent = &st->client->dev; 133 st->trig->ops = &inv_mpu_trigger_ops; 134 iio_trigger_set_drvdata(st->trig, indio_dev); 135 ret = iio_trigger_register(st->trig); 136 if (ret) 137 goto error_free_irq; 138 indio_dev->trig = st->trig; 139 140 return 0; 141 142 error_free_irq: 143 free_irq(st->client->irq, st->trig); 144 error_free_trig: 145 iio_trigger_free(st->trig); 146 error_ret: 147 return ret; 148 } 149 150 void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st) 151 { 152 iio_trigger_unregister(st->trig); 153 free_irq(st->client->irq, st->trig); 154 iio_trigger_free(st->trig); 155 } 156