diff options
Diffstat (limited to 'drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c')
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 168 |
1 files changed, 168 insertions, 0 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c new file mode 100644 index 000000000..6c3e1652a --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c @@ -0,0 +1,168 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +*/ + +#include "inv_mpu_iio.h" + +static void inv_scan_query(struct iio_dev *indio_dev) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + st->chip_config.gyro_fifo_enable = + test_bit(INV_MPU6050_SCAN_GYRO_X, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_GYRO_Y, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_GYRO_Z, + indio_dev->active_scan_mask); + + st->chip_config.accl_fifo_enable = + test_bit(INV_MPU6050_SCAN_ACCL_X, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_ACCL_Y, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_ACCL_Z, + indio_dev->active_scan_mask); +} + +/** + * inv_mpu6050_set_enable() - enable chip functions. + * @indio_dev: Device driver instance. + * @enable: enable/disable + */ +static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int result; + + if (enable) { + result = inv_mpu6050_set_power_itg(st, true); + if (result) + return result; + inv_scan_query(indio_dev); + st->skip_samples = 0; + if (st->chip_config.gyro_fifo_enable) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_BIT_PWR_GYRO_STBY); + if (result) + goto error_power_off; + /* gyro first sample is out of specs, skip it */ + st->skip_samples = 1; + } + if (st->chip_config.accl_fifo_enable) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_BIT_PWR_ACCL_STBY); + if (result) + goto error_gyro_off; + } + result = inv_reset_fifo(indio_dev); + if (result) + goto error_accl_off; + } else { + result = regmap_write(st->map, st->reg->fifo_en, 0); + if (result) + goto error_accl_off; + + result = regmap_write(st->map, st->reg->int_enable, 0); + if (result) + goto error_accl_off; + + result = regmap_write(st->map, st->reg->user_ctrl, + st->chip_config.user_ctrl); + if (result) + goto error_accl_off; + + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_ACCL_STBY); + if (result) + goto error_accl_off; + + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_GYRO_STBY); + if (result) + goto error_gyro_off; + + result = inv_mpu6050_set_power_itg(st, false); + if (result) + goto error_power_off; + } + + return 0; + +error_accl_off: + if (st->chip_config.accl_fifo_enable) + inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_ACCL_STBY); +error_gyro_off: + if (st->chip_config.gyro_fifo_enable) + inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_GYRO_STBY); +error_power_off: + inv_mpu6050_set_power_itg(st, false); + return result; +} + +/** + * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state + * @trig: Trigger instance + * @state: Desired trigger state + */ +static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, + bool state) +{ + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int result; + + mutex_lock(&st->lock); + result = inv_mpu6050_set_enable(indio_dev, state); + mutex_unlock(&st->lock); + + return result; +} + +static const struct iio_trigger_ops inv_mpu_trigger_ops = { + .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, +}; + +int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type) +{ + int ret; + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + st->trig = devm_iio_trigger_alloc(&indio_dev->dev, + "%s-dev%d", + indio_dev->name, + indio_dev->id); + if (!st->trig) + return -ENOMEM; + + ret = devm_request_irq(&indio_dev->dev, st->irq, + &iio_trigger_generic_data_rdy_poll, + irq_type, + "inv_mpu", + st->trig); + if (ret) + return ret; + + st->trig->dev.parent = regmap_get_device(st->map); + st->trig->ops = &inv_mpu_trigger_ops; + iio_trigger_set_drvdata(st->trig, indio_dev); + + ret = devm_iio_trigger_register(&indio_dev->dev, st->trig); + if (ret) + return ret; + + indio_dev->trig = iio_trigger_get(st->trig); + + return 0; +} |