123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193 |
- #include <linux/kernel.h>
- #include <linux/export.h>
- #include <linux/slab.h>
- #include <linux/interrupt.h>
- #include <linux/irq.h>
- #include <linux/bitmap.h>
- #include <linux/iio/iio.h>
- #include <linux/iio/trigger_consumer.h>
- #include <linux/iio/kfifo_buf.h>
- #include "iio_simple_dummy.h"
- static const s16 fakedata[] = {
- [DUMMY_INDEX_VOLTAGE_0] = 7,
- [DUMMY_INDEX_DIFFVOLTAGE_1M2] = -33,
- [DUMMY_INDEX_DIFFVOLTAGE_3M4] = -2,
- [DUMMY_INDEX_ACCELX] = 344,
- };
- static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
- {
- struct iio_poll_func *pf = p;
- struct iio_dev *indio_dev = pf->indio_dev;
- int len = 0;
- u16 *data;
- data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
- if (!data)
- goto done;
- if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
-
- int i, j;
- for (i = 0, j = 0;
- i < bitmap_weight(indio_dev->active_scan_mask,
- indio_dev->masklength);
- i++, j++) {
- j = find_next_bit(indio_dev->active_scan_mask,
- indio_dev->masklength, j);
-
- data[i] = fakedata[j];
- len += 2;
- }
- }
- iio_push_to_buffers_with_timestamp(indio_dev, data,
- iio_get_time_ns(indio_dev));
- kfree(data);
- done:
-
- iio_trigger_notify_done(indio_dev->trig);
- return IRQ_HANDLED;
- }
- static const struct iio_buffer_setup_ops iio_simple_dummy_buffer_setup_ops = {
-
- .postenable = &iio_triggered_buffer_postenable,
-
- .predisable = &iio_triggered_buffer_predisable,
- };
- int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
- {
- int ret;
- struct iio_buffer *buffer;
-
- buffer = iio_kfifo_allocate();
- if (!buffer) {
- ret = -ENOMEM;
- goto error_ret;
- }
- iio_device_attach_buffer(indio_dev, buffer);
-
- buffer->scan_timestamp = true;
-
- indio_dev->setup_ops = &iio_simple_dummy_buffer_setup_ops;
-
- indio_dev->pollfunc = iio_alloc_pollfunc(NULL,
- &iio_simple_dummy_trigger_h,
- IRQF_ONESHOT,
- indio_dev,
- "iio_simple_dummy_consumer%d",
- indio_dev->id);
- if (!indio_dev->pollfunc) {
- ret = -ENOMEM;
- goto error_free_buffer;
- }
-
- indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
- return 0;
- error_free_buffer:
- iio_kfifo_free(indio_dev->buffer);
- error_ret:
- return ret;
- }
- void iio_simple_dummy_unconfigure_buffer(struct iio_dev *indio_dev)
- {
- iio_dealloc_pollfunc(indio_dev->pollfunc);
- iio_kfifo_free(indio_dev->buffer);
- }
|