itg3200_buffer.c 3.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * itg3200_buffer.c -- support InvenSense ITG3200
  4. * Digital 3-Axis Gyroscope driver
  5. *
  6. * Copyright (c) 2011 Christian Strobel <christian.strobel@iis.fraunhofer.de>
  7. * Copyright (c) 2011 Manuel Stahl <manuel.stahl@iis.fraunhofer.de>
  8. * Copyright (c) 2012 Thorsten Nowak <thorsten.nowak@iis.fraunhofer.de>
  9. */
  10. #include <linux/slab.h>
  11. #include <linux/i2c.h>
  12. #include <linux/interrupt.h>
  13. #include <linux/iio/iio.h>
  14. #include <linux/iio/buffer.h>
  15. #include <linux/iio/trigger.h>
  16. #include <linux/iio/trigger_consumer.h>
  17. #include <linux/iio/triggered_buffer.h>
  18. #include <linux/iio/gyro/itg3200.h>
  19. static int itg3200_read_all_channels(struct i2c_client *i2c, __be16 *buf)
  20. {
  21. u8 tx = 0x80 | ITG3200_REG_TEMP_OUT_H;
  22. struct i2c_msg msg[2] = {
  23. {
  24. .addr = i2c->addr,
  25. .flags = i2c->flags,
  26. .len = 1,
  27. .buf = &tx,
  28. },
  29. {
  30. .addr = i2c->addr,
  31. .flags = i2c->flags | I2C_M_RD,
  32. .len = ITG3200_SCAN_ELEMENTS * sizeof(s16),
  33. .buf = (char *)&buf,
  34. },
  35. };
  36. return i2c_transfer(i2c->adapter, msg, 2);
  37. }
  38. static irqreturn_t itg3200_trigger_handler(int irq, void *p)
  39. {
  40. struct iio_poll_func *pf = p;
  41. struct iio_dev *indio_dev = pf->indio_dev;
  42. struct itg3200 *st = iio_priv(indio_dev);
  43. /*
  44. * Ensure correct alignment and padding including for the
  45. * timestamp that may be inserted.
  46. */
  47. struct {
  48. __be16 buf[ITG3200_SCAN_ELEMENTS];
  49. s64 ts __aligned(8);
  50. } scan;
  51. int ret = itg3200_read_all_channels(st->i2c, scan.buf);
  52. if (ret < 0)
  53. goto error_ret;
  54. iio_push_to_buffers_with_timestamp(indio_dev, &scan, pf->timestamp);
  55. error_ret:
  56. iio_trigger_notify_done(indio_dev->trig);
  57. return IRQ_HANDLED;
  58. }
  59. int itg3200_buffer_configure(struct iio_dev *indio_dev)
  60. {
  61. return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
  62. itg3200_trigger_handler, NULL);
  63. }
  64. void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
  65. {
  66. iio_triggered_buffer_cleanup(indio_dev);
  67. }
  68. static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
  69. bool state)
  70. {
  71. struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
  72. int ret;
  73. u8 msc;
  74. ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
  75. if (ret)
  76. goto error_ret;
  77. if (state)
  78. msc |= ITG3200_IRQ_DATA_RDY_ENABLE;
  79. else
  80. msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;
  81. ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
  82. if (ret)
  83. goto error_ret;
  84. error_ret:
  85. return ret;
  86. }
  87. static const struct iio_trigger_ops itg3200_trigger_ops = {
  88. .set_trigger_state = &itg3200_data_rdy_trigger_set_state,
  89. };
  90. int itg3200_probe_trigger(struct iio_dev *indio_dev)
  91. {
  92. int ret;
  93. struct itg3200 *st = iio_priv(indio_dev);
  94. st->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
  95. indio_dev->id);
  96. if (!st->trig)
  97. return -ENOMEM;
  98. ret = request_irq(st->i2c->irq,
  99. &iio_trigger_generic_data_rdy_poll,
  100. IRQF_TRIGGER_RISING,
  101. "itg3200_data_rdy",
  102. st->trig);
  103. if (ret)
  104. goto error_free_trig;
  105. st->trig->dev.parent = &st->i2c->dev;
  106. st->trig->ops = &itg3200_trigger_ops;
  107. iio_trigger_set_drvdata(st->trig, indio_dev);
  108. ret = iio_trigger_register(st->trig);
  109. if (ret)
  110. goto error_free_irq;
  111. /* select default trigger */
  112. indio_dev->trig = iio_trigger_get(st->trig);
  113. return 0;
  114. error_free_irq:
  115. free_irq(st->i2c->irq, st->trig);
  116. error_free_trig:
  117. iio_trigger_free(st->trig);
  118. return ret;
  119. }
  120. void itg3200_remove_trigger(struct iio_dev *indio_dev)
  121. {
  122. struct itg3200 *st = iio_priv(indio_dev);
  123. iio_trigger_unregister(st->trig);
  124. free_irq(st->i2c->irq, st->trig);
  125. iio_trigger_free(st->trig);
  126. }