1 /* 2 * itg3200_buffer.c -- support InvenSense ITG3200 3 * Digital 3-Axis Gyroscope driver 4 * 5 * Copyright (c) 2011 Christian Strobel <christian.strobel@iis.fraunhofer.de> 6 * Copyright (c) 2011 Manuel Stahl <manuel.stahl@iis.fraunhofer.de> 7 * Copyright (c) 2012 Thorsten Nowak <thorsten.nowak@iis.fraunhofer.de> 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License version 2 as 11 * published by the Free Software Foundation. 12 */ 13 14 #include <linux/slab.h> 15 #include <linux/i2c.h> 16 #include <linux/interrupt.h> 17 18 #include <linux/iio/iio.h> 19 #include <linux/iio/buffer.h> 20 #include <linux/iio/trigger.h> 21 #include <linux/iio/trigger_consumer.h> 22 #include <linux/iio/triggered_buffer.h> 23 #include <linux/iio/gyro/itg3200.h> 24 25 26 static int itg3200_read_all_channels(struct i2c_client *i2c, __be16 *buf) 27 { 28 u8 tx = 0x80 | ITG3200_REG_TEMP_OUT_H; 29 struct i2c_msg msg[2] = { 30 { 31 .addr = i2c->addr, 32 .flags = i2c->flags, 33 .len = 1, 34 .buf = &tx, 35 }, 36 { 37 .addr = i2c->addr, 38 .flags = i2c->flags | I2C_M_RD, 39 .len = ITG3200_SCAN_ELEMENTS * sizeof(s16), 40 .buf = (char *)&buf, 41 }, 42 }; 43 44 return i2c_transfer(i2c->adapter, msg, 2); 45 } 46 47 static irqreturn_t itg3200_trigger_handler(int irq, void *p) 48 { 49 struct iio_poll_func *pf = p; 50 struct iio_dev *indio_dev = pf->indio_dev; 51 struct itg3200 *st = iio_priv(indio_dev); 52 __be16 buf[ITG3200_SCAN_ELEMENTS + sizeof(s64)/sizeof(u16)]; 53 54 int ret = itg3200_read_all_channels(st->i2c, buf); 55 if (ret < 0) 56 goto error_ret; 57 58 if (indio_dev->scan_timestamp) 59 memcpy(buf + indio_dev->scan_bytes - sizeof(s64), 60 &pf->timestamp, sizeof(pf->timestamp)); 61 62 iio_push_to_buffers(indio_dev, (u8 *)buf); 63 iio_trigger_notify_done(indio_dev->trig); 64 65 error_ret: 66 return IRQ_HANDLED; 67 } 68 69 int itg3200_buffer_configure(struct iio_dev *indio_dev) 70 { 71 return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time, 72 itg3200_trigger_handler, NULL); 73 } 74 75 void itg3200_buffer_unconfigure(struct iio_dev *indio_dev) 76 { 77 iio_triggered_buffer_cleanup(indio_dev); 78 } 79 80 81 static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig, 82 bool state) 83 { 84 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); 85 int ret; 86 u8 msc; 87 88 ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc); 89 if (ret) 90 goto error_ret; 91 92 if (state) 93 msc |= ITG3200_IRQ_DATA_RDY_ENABLE; 94 else 95 msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE; 96 97 ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc); 98 if (ret) 99 goto error_ret; 100 101 error_ret: 102 return ret; 103 104 } 105 106 static const struct iio_trigger_ops itg3200_trigger_ops = { 107 .owner = THIS_MODULE, 108 .set_trigger_state = &itg3200_data_rdy_trigger_set_state, 109 }; 110 111 int itg3200_probe_trigger(struct iio_dev *indio_dev) 112 { 113 int ret; 114 struct itg3200 *st = iio_priv(indio_dev); 115 116 st->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name, 117 indio_dev->id); 118 if (!st->trig) 119 return -ENOMEM; 120 121 ret = request_irq(st->i2c->irq, 122 &iio_trigger_generic_data_rdy_poll, 123 IRQF_TRIGGER_RISING, 124 "itg3200_data_rdy", 125 st->trig); 126 if (ret) 127 goto error_free_trig; 128 129 130 st->trig->dev.parent = &st->i2c->dev; 131 st->trig->ops = &itg3200_trigger_ops; 132 iio_trigger_set_drvdata(st->trig, indio_dev); 133 ret = iio_trigger_register(st->trig); 134 if (ret) 135 goto error_free_irq; 136 137 /* select default trigger */ 138 indio_dev->trig = st->trig; 139 140 return 0; 141 142 error_free_irq: 143 free_irq(st->i2c->irq, st->trig); 144 error_free_trig: 145 iio_trigger_free(st->trig); 146 return ret; 147 } 148 149 void itg3200_remove_trigger(struct iio_dev *indio_dev) 150 { 151 struct itg3200 *st = iio_priv(indio_dev); 152 153 iio_trigger_unregister(st->trig); 154 free_irq(st->i2c->irq, st->trig); 155 iio_trigger_free(st->trig); 156 } 157