drivers/staging/iio/kfifo_buf.c

   1 #include <linux/slab.h>
   2 #include <linux/kernel.h>
   3 #include <linux/module.h>
   4 #include <linux/device.h>
   5 #include <linux/workqueue.h>
   6 #include <linux/kfifo.h>
   7 #include <linux/mutex.h>
   8
   9 #include "kfifo_buf.h"
  10
  11 #define iio_to_kfifo(r) container_of(r, struct iio_kfifo, ring)
  12
  13 static inline int __iio_allocate_kfifo(struct iio_kfifo *buf,
  14                                 int bytes_per_datum, int length)
  15 {
  16         if ((length == 0) || (bytes_per_datum == 0))
  17                 return -EINVAL;
  18
  19         __iio_update_ring_buffer(&buf->ring, bytes_per_datum, length);
  20         return kfifo_alloc(&buf->kf, bytes_per_datum*length, GFP_KERNEL);
  21 }
  22
  23 static int iio_request_update_kfifo(struct iio_ring_buffer *r)
  24 {
  25         int ret = 0;
  26         struct iio_kfifo *buf = iio_to_kfifo(r);
  27
  28         mutex_lock(&buf->use_lock);
  29         if (!buf->update_needed)
  30                 goto error_ret;
  31         if (buf->use_count) {
  32                 ret = -EAGAIN;
  33                 goto error_ret;
  34         }
  35         kfifo_free(&buf->kf);
  36         ret = __iio_allocate_kfifo(buf, buf->ring.bytes_per_datum,
  37                                 buf->ring.length);
  38 error_ret:
  39         mutex_unlock(&buf->use_lock);
  40         return ret;
  41 }
  42
  43 static void iio_mark_kfifo_in_use(struct iio_ring_buffer *r)
  44 {
  45         struct iio_kfifo *buf = iio_to_kfifo(r);
  46         mutex_lock(&buf->use_lock);
  47         buf->use_count++;
  48         mutex_unlock(&buf->use_lock);
  49 }
  50
  51 static void iio_unmark_kfifo_in_use(struct iio_ring_buffer *r)
  52 {
  53         struct iio_kfifo *buf = iio_to_kfifo(r);
  54         mutex_lock(&buf->use_lock);
  55         buf->use_count--;
  56         mutex_unlock(&buf->use_lock);
  57 }
  58
  59 static int iio_get_length_kfifo(struct iio_ring_buffer *r)
  60 {
  61         return r->length;
  62 }
  63
  64 static inline void __iio_init_kfifo(struct iio_kfifo *kf)
  65 {
  66         mutex_init(&kf->use_lock);
  67 }
  68
  69 static IIO_RING_ENABLE_ATTR;
  70 static IIO_RING_BYTES_PER_DATUM_ATTR;
  71 static IIO_RING_LENGTH_ATTR;
  72
  73 static struct attribute *iio_kfifo_attributes[] = {
  74         &dev_attr_length.attr,
  75         &dev_attr_bytes_per_datum.attr,
  76         &dev_attr_enable.attr,
  77         NULL,
  78 };
  79
  80 static struct attribute_group iio_kfifo_attribute_group = {
  81         .attrs = iio_kfifo_attributes,
  82 };
  83
  84 static const struct attribute_group *iio_kfifo_attribute_groups[] = {
  85         &iio_kfifo_attribute_group,
  86         NULL
  87 };
  88
  89 static void iio_kfifo_release(struct device *dev)
  90 {
  91         struct iio_ring_buffer *r = to_iio_ring_buffer(dev);
  92         struct iio_kfifo *kf = iio_to_kfifo(r);
  93         kfifo_free(&kf->kf);
  94         kfree(kf);
  95 }
  96
  97 static struct device_type iio_kfifo_type = {
  98         .release = iio_kfifo_release,
  99         .groups = iio_kfifo_attribute_groups,
 100 };
 101
 102 struct iio_ring_buffer *iio_kfifo_allocate(struct iio_dev *indio_dev)
 103 {
 104         struct iio_kfifo *kf;
 105
 106         kf = kzalloc(sizeof *kf, GFP_KERNEL);
 107         if (!kf)
 108                 return NULL;
 109         kf->update_needed = true;
 110         iio_ring_buffer_init(&kf->ring, indio_dev);
 111         __iio_init_kfifo(kf);
 112         kf->ring.dev.type = &iio_kfifo_type;
 113         device_initialize(&kf->ring.dev);
 114         kf->ring.dev.parent = &indio_dev->dev;
 115         kf->ring.dev.bus = &iio_bus_type;
 116         dev_set_drvdata(&kf->ring.dev, (void *)&(kf->ring));
 117
 118         return &kf->ring;
 119 }
 120 EXPORT_SYMBOL(iio_kfifo_allocate);
 121
 122 static int iio_get_bytes_per_datum_kfifo(struct iio_ring_buffer *r)
 123 {
 124         return r->bytes_per_datum;
 125 }
 126
 127 static int iio_set_bytes_per_datum_kfifo(struct iio_ring_buffer *r, size_t bpd)
 128 {
 129         if (r->bytes_per_datum != bpd) {
 130                 r->bytes_per_datum = bpd;
 131                 if (r->access->mark_param_change)
 132                         r->access->mark_param_change(r);
 133         }
 134         return 0;
 135 }
 136
 137 static int iio_mark_update_needed_kfifo(struct iio_ring_buffer *r)
 138 {
 139         struct iio_kfifo *kf = iio_to_kfifo(r);
 140         kf->update_needed = true;
 141         return 0;
 142 }
 143
 144 static int iio_set_length_kfifo(struct iio_ring_buffer *r, int length)
 145 {
 146         if (r->length != length) {
 147                 r->length = length;
 148                 if (r->access->mark_param_change)
 149                         r->access->mark_param_change(r);
 150         }
 151         return 0;
 152 }
 153
 154 void iio_kfifo_free(struct iio_ring_buffer *r)
 155 {
 156         if (r)
 157                 iio_put_ring_buffer(r);
 158 }
 159 EXPORT_SYMBOL(iio_kfifo_free);
 160
 161 static int iio_store_to_kfifo(struct iio_ring_buffer *r,
 162                               u8 *data,
 163                               s64 timestamp)
 164 {
 165         int ret;
 166         struct iio_kfifo *kf = iio_to_kfifo(r);
 167         u8 *datal = kmalloc(r->bytes_per_datum, GFP_KERNEL);
 168         memcpy(datal, data, r->bytes_per_datum - sizeof(timestamp));
 169         memcpy(datal + r->bytes_per_datum - sizeof(timestamp),
 170                 &timestamp, sizeof(timestamp));
 171         ret = kfifo_in(&kf->kf, data, r->bytes_per_datum);
 172         if (ret != r->bytes_per_datum) {
 173                 kfree(datal);
 174                 return -EBUSY;
 175         }
 176         kfree(datal);
 177         return 0;
 178 }
 179
 180 static int iio_read_first_n_kfifo(struct iio_ring_buffer *r,
 181                            size_t n, char __user *buf)
 182 {
 183         int ret, copied;
 184         struct iio_kfifo *kf = iio_to_kfifo(r);
 185
 186         ret = kfifo_to_user(&kf->kf, buf, r->bytes_per_datum*n, &copied);
 187
 188         return copied;
 189 }
 190
 191 const struct iio_ring_access_funcs kfifo_access_funcs = {
 192         .mark_in_use = &iio_mark_kfifo_in_use,
 193         .unmark_in_use = &iio_unmark_kfifo_in_use,
 194         .store_to = &iio_store_to_kfifo,
 195         .read_first_n = &iio_read_first_n_kfifo,
 196         .mark_param_change = &iio_mark_update_needed_kfifo,
 197         .request_update = &iio_request_update_kfifo,
 198         .get_bytes_per_datum = &iio_get_bytes_per_datum_kfifo,
 199         .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo,
 200         .get_length = &iio_get_length_kfifo,
 201         .set_length = &iio_set_length_kfifo,
 202 };
 203 EXPORT_SYMBOL(kfifo_access_funcs);
 204
 205 MODULE_LICENSE("GPL");