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         kf->ring.dev.parent = &indio_dev->dev;
 114         dev_set_drvdata(&kf->ring.dev, (void *)&(kf->ring));
 115
 116         return &kf->ring;
 117 }
 118 EXPORT_SYMBOL(iio_kfifo_allocate);
 119
 120 static int iio_get_bytes_per_datum_kfifo(struct iio_ring_buffer *r)
 121 {
 122         return r->bytes_per_datum;
 123 }
 124
 125 static int iio_set_bytes_per_datum_kfifo(struct iio_ring_buffer *r, size_t bpd)
 126 {
 127         if (r->bytes_per_datum != bpd) {
 128                 r->bytes_per_datum = bpd;
 129                 if (r->access->mark_param_change)
 130                         r->access->mark_param_change(r);
 131         }
 132         return 0;
 133 }
 134
 135 static int iio_mark_update_needed_kfifo(struct iio_ring_buffer *r)
 136 {
 137         struct iio_kfifo *kf = iio_to_kfifo(r);
 138         kf->update_needed = true;
 139         return 0;
 140 }
 141
 142 static int iio_set_length_kfifo(struct iio_ring_buffer *r, int length)
 143 {
 144         if (r->length != length) {
 145                 r->length = length;
 146                 if (r->access->mark_param_change)
 147                         r->access->mark_param_change(r);
 148         }
 149         return 0;
 150 }
 151
 152 void iio_kfifo_free(struct iio_ring_buffer *r)
 153 {
 154         if (r)
 155                 iio_put_ring_buffer(r);
 156 }
 157 EXPORT_SYMBOL(iio_kfifo_free);
 158
 159 static int iio_store_to_kfifo(struct iio_ring_buffer *r,
 160                               u8 *data,
 161                               s64 timestamp)
 162 {
 163         int ret;
 164         struct iio_kfifo *kf = iio_to_kfifo(r);
 165         u8 *datal = kmalloc(r->bytes_per_datum, GFP_KERNEL);
 166         memcpy(datal, data, r->bytes_per_datum - sizeof(timestamp));
 167         memcpy(datal + r->bytes_per_datum - sizeof(timestamp),
 168                 &timestamp, sizeof(timestamp));
 169         ret = kfifo_in(&kf->kf, data, r->bytes_per_datum);
 170         if (ret != r->bytes_per_datum) {
 171                 kfree(datal);
 172                 return -EBUSY;
 173         }
 174         kfree(datal);
 175         return 0;
 176 }
 177
 178 static int iio_read_first_n_kfifo(struct iio_ring_buffer *r,
 179                            size_t n, char __user *buf)
 180 {
 181         int ret, copied;
 182         struct iio_kfifo *kf = iio_to_kfifo(r);
 183
 184         ret = kfifo_to_user(&kf->kf, buf, r->bytes_per_datum*n, &copied);
 185
 186         return copied;
 187 }
 188
 189 const struct iio_ring_access_funcs kfifo_access_funcs = {
 190         .mark_in_use = &iio_mark_kfifo_in_use,
 191         .unmark_in_use = &iio_unmark_kfifo_in_use,
 192         .store_to = &iio_store_to_kfifo,
 193         .read_first_n = &iio_read_first_n_kfifo,
 194         .mark_param_change = &iio_mark_update_needed_kfifo,
 195         .request_update = &iio_request_update_kfifo,
 196         .get_bytes_per_datum = &iio_get_bytes_per_datum_kfifo,
 197         .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo,
 198         .get_length = &iio_get_length_kfifo,
 199         .set_length = &iio_set_length_kfifo,
 200 };
 201 EXPORT_SYMBOL(kfifo_access_funcs);
 202
 203 MODULE_LICENSE("GPL");