drivers/staging/comedi/comedi_buf.c

   1 /*
   2  * comedi_buf.c
   3  *
   4  * COMEDI - Linux Control and Measurement Device Interface
   5  * Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  */
  17
  18 #include "comedidev.h"
  19 #include "comedi_internal.h"
  20
  21 #ifdef PAGE_KERNEL_NOCACHE
  22 #define COMEDI_PAGE_PROTECTION          PAGE_KERNEL_NOCACHE
  23 #else
  24 #define COMEDI_PAGE_PROTECTION          PAGE_KERNEL
  25 #endif
  26
  27 static void __comedi_buf_free(struct comedi_device *dev,
  28                               struct comedi_subdevice *s,
  29                               unsigned n_pages)
  30 {
  31         struct comedi_async *async = s->async;
  32         struct comedi_buf_page *buf;
  33         unsigned i;
  34
  35         if (async->prealloc_buf) {
  36                 vunmap(async->prealloc_buf);
  37                 async->prealloc_buf = NULL;
  38                 async->prealloc_bufsz = 0;
  39         }
  40
  41         if (!async->buf_page_list)
  42                 return;
  43
  44         for (i = 0; i < n_pages; ++i) {
  45                 buf = &async->buf_page_list[i];
  46                 if (buf->virt_addr) {
  47                         clear_bit(PG_reserved,
  48                                   &(virt_to_page(buf->virt_addr)->flags));
  49                         if (s->async_dma_dir != DMA_NONE) {
  50 #ifdef CONFIG_HAS_DMA
  51                                 dma_free_coherent(dev->hw_dev,
  52                                                   PAGE_SIZE,
  53                                                   buf->virt_addr,
  54                                                   buf->dma_addr);
  55 #endif
  56                         } else {
  57                                 free_page((unsigned long)buf->virt_addr);
  58                         }
  59                 }
  60         }
  61         vfree(async->buf_page_list);
  62         async->buf_page_list = NULL;
  63         async->n_buf_pages = 0;
  64 }
  65
  66 static void __comedi_buf_alloc(struct comedi_device *dev,
  67                                struct comedi_subdevice *s,
  68                                unsigned n_pages)
  69 {
  70         struct comedi_async *async = s->async;
  71         struct page **pages = NULL;
  72         struct comedi_buf_page *buf;
  73         unsigned i;
  74
  75         if (!IS_ENABLED(CONFIG_HAS_DMA) && s->async_dma_dir != DMA_NONE) {
  76                 dev_err(dev->class_dev,
  77                         "dma buffer allocation not supported\n");
  78                 return;
  79         }
  80
  81         async->buf_page_list = vzalloc(sizeof(*buf) * n_pages);
  82         if (async->buf_page_list)
  83                 pages = vmalloc(sizeof(struct page *) * n_pages);
  84
  85         if (!pages)
  86                 return;
  87
  88         for (i = 0; i < n_pages; i++) {
  89                 buf = &async->buf_page_list[i];
  90                 if (s->async_dma_dir != DMA_NONE)
  91 #ifdef CONFIG_HAS_DMA
  92                         buf->virt_addr = dma_alloc_coherent(dev->hw_dev,
  93                                                             PAGE_SIZE,
  94                                                             &buf->dma_addr,
  95                                                             GFP_KERNEL |
  96                                                             __GFP_COMP);
  97 #else
  98                         break;
  99 #endif
 100                 else
 101                         buf->virt_addr = (void *)get_zeroed_page(GFP_KERNEL);
 102                 if (!buf->virt_addr)
 103                         break;
 104
 105                 set_bit(PG_reserved, &(virt_to_page(buf->virt_addr)->flags));
 106
 107                 pages[i] = virt_to_page(buf->virt_addr);
 108         }
 109
 110         /* vmap the prealloc_buf if all the pages were allocated */
 111         if (i == n_pages)
 112                 async->prealloc_buf = vmap(pages, n_pages, VM_MAP,
 113                                            COMEDI_PAGE_PROTECTION);
 114
 115         vfree(pages);
 116 }
 117
 118 int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 119                      unsigned long new_size)
 120 {
 121         struct comedi_async *async = s->async;
 122
 123         /* Round up new_size to multiple of PAGE_SIZE */
 124         new_size = (new_size + PAGE_SIZE - 1) & PAGE_MASK;
 125
 126         /* if no change is required, do nothing */
 127         if (async->prealloc_buf && async->prealloc_bufsz == new_size)
 128                 return 0;
 129
 130         /* deallocate old buffer */
 131         __comedi_buf_free(dev, s, async->n_buf_pages);
 132
 133         /* allocate new buffer */
 134         if (new_size) {
 135                 unsigned n_pages = new_size >> PAGE_SHIFT;
 136
 137                 __comedi_buf_alloc(dev, s, n_pages);
 138
 139                 if (!async->prealloc_buf) {
 140                         /* allocation failed */
 141                         __comedi_buf_free(dev, s, n_pages);
 142                         return -ENOMEM;
 143                 }
 144                 async->n_buf_pages = n_pages;
 145         }
 146         async->prealloc_bufsz = new_size;
 147
 148         return 0;
 149 }
 150
 151 void comedi_buf_reset(struct comedi_async *async)
 152 {
 153         async->buf_write_alloc_count = 0;
 154         async->buf_write_count = 0;
 155         async->buf_read_alloc_count = 0;
 156         async->buf_read_count = 0;
 157
 158         async->buf_write_ptr = 0;
 159         async->buf_read_ptr = 0;
 160
 161         async->cur_chan = 0;
 162         async->scan_progress = 0;
 163         async->munge_chan = 0;
 164         async->munge_count = 0;
 165         async->munge_ptr = 0;
 166
 167         async->events = 0;
 168 }
 169
 170 static unsigned int comedi_buf_write_n_available(struct comedi_async *async)
 171 {
 172         unsigned int free_end = async->buf_read_count + async->prealloc_bufsz;
 173
 174         return free_end - async->buf_write_alloc_count;
 175 }
 176
 177 static unsigned int __comedi_buf_write_alloc(struct comedi_async *async,
 178                                              unsigned int nbytes,
 179                                              int strict)
 180 {
 181         unsigned int available = comedi_buf_write_n_available(async);
 182
 183         if (nbytes > available)
 184                 nbytes = strict ? 0 : available;
 185
 186         async->buf_write_alloc_count += nbytes;
 187
 188         /*
 189          * ensure the async buffer 'counts' are read and updated
 190          * before we write data to the write-alloc'ed buffer space
 191          */
 192         smp_mb();
 193
 194         return nbytes;
 195 }
 196
 197 /* allocates chunk for the writer from free buffer space */
 198 unsigned int comedi_buf_write_alloc(struct comedi_async *async,
 199                                     unsigned int nbytes)
 200 {
 201         return __comedi_buf_write_alloc(async, nbytes, 0);
 202 }
 203 EXPORT_SYMBOL_GPL(comedi_buf_write_alloc);
 204
 205 /*
 206  * munging is applied to data by core as it passes between user
 207  * and kernel space
 208  */
 209 static unsigned int comedi_buf_munge(struct comedi_async *async,
 210                                      unsigned int num_bytes)
 211 {
 212         struct comedi_subdevice *s = async->subdevice;
 213         unsigned int count = 0;
 214         const unsigned num_sample_bytes = bytes_per_sample(s);
 215
 216         if (!s->munge || (async->cmd.flags & CMDF_RAWDATA)) {
 217                 async->munge_count += num_bytes;
 218                 count = num_bytes;
 219         } else {
 220                 /* don't munge partial samples */
 221                 num_bytes -= num_bytes % num_sample_bytes;
 222                 while (count < num_bytes) {
 223                         int block_size = num_bytes - count;
 224                         unsigned int buf_end;
 225
 226                         buf_end = async->prealloc_bufsz - async->munge_ptr;
 227                         if (block_size > buf_end)
 228                                 block_size = buf_end;
 229
 230                         s->munge(s->device, s,
 231                                  async->prealloc_buf + async->munge_ptr,
 232                                  block_size, async->munge_chan);
 233
 234                         /*
 235                          * ensure data is munged in buffer before the
 236                          * async buffer munge_count is incremented
 237                          */
 238                         smp_wmb();
 239
 240                         async->munge_chan += block_size / num_sample_bytes;
 241                         async->munge_chan %= async->cmd.chanlist_len;
 242                         async->munge_count += block_size;
 243                         async->munge_ptr += block_size;
 244                         async->munge_ptr %= async->prealloc_bufsz;
 245                         count += block_size;
 246                 }
 247         }
 248
 249         return count;
 250 }
 251
 252 unsigned int comedi_buf_write_n_allocated(struct comedi_async *async)
 253 {
 254         return async->buf_write_alloc_count - async->buf_write_count;
 255 }
 256
 257 /* transfers a chunk from writer to filled buffer space */
 258 unsigned int comedi_buf_write_free(struct comedi_async *async,
 259                                    unsigned int nbytes)
 260 {
 261         unsigned int allocated = comedi_buf_write_n_allocated(async);
 262
 263         if (nbytes > allocated)
 264                 nbytes = allocated;
 265
 266         async->buf_write_count += nbytes;
 267         async->buf_write_ptr += nbytes;
 268         comedi_buf_munge(async, async->buf_write_count - async->munge_count);
 269         if (async->buf_write_ptr >= async->prealloc_bufsz)
 270                 async->buf_write_ptr %= async->prealloc_bufsz;
 271
 272         return nbytes;
 273 }
 274 EXPORT_SYMBOL_GPL(comedi_buf_write_free);
 275
 276 unsigned int comedi_buf_read_n_available(struct comedi_async *async)
 277 {
 278         unsigned num_bytes;
 279
 280         if (!async)
 281                 return 0;
 282
 283         num_bytes = async->munge_count - async->buf_read_count;
 284
 285         /*
 286          * ensure the async buffer 'counts' are read before we
 287          * attempt to read data from the buffer
 288          */
 289         smp_rmb();
 290
 291         return num_bytes;
 292 }
 293 EXPORT_SYMBOL_GPL(comedi_buf_read_n_available);
 294
 295 /* allocates a chunk for the reader from filled (and munged) buffer space */
 296 unsigned int comedi_buf_read_alloc(struct comedi_async *async,
 297                                    unsigned int nbytes)
 298 {
 299         unsigned int available;
 300
 301         available = async->munge_count - async->buf_read_alloc_count;
 302         if (nbytes > available)
 303                 nbytes = available;
 304
 305         async->buf_read_alloc_count += nbytes;
 306
 307         /*
 308          * ensure the async buffer 'counts' are read before we
 309          * attempt to read data from the read-alloc'ed buffer space
 310          */
 311         smp_rmb();
 312
 313         return nbytes;
 314 }
 315 EXPORT_SYMBOL_GPL(comedi_buf_read_alloc);
 316
 317 static unsigned int comedi_buf_read_n_allocated(struct comedi_async *async)
 318 {
 319         return async->buf_read_alloc_count - async->buf_read_count;
 320 }
 321
 322 /* transfers control of a chunk from reader to free buffer space */
 323 unsigned int comedi_buf_read_free(struct comedi_async *async,
 324                                   unsigned int nbytes)
 325 {
 326         unsigned int allocated;
 327
 328         /*
 329          * ensure data has been read out of buffer before
 330          * the async read count is incremented
 331          */
 332         smp_mb();
 333
 334         allocated = comedi_buf_read_n_allocated(async);
 335         if (nbytes > allocated)
 336                 nbytes = allocated;
 337
 338         async->buf_read_count += nbytes;
 339         async->buf_read_ptr += nbytes;
 340         async->buf_read_ptr %= async->prealloc_bufsz;
 341         return nbytes;
 342 }
 343 EXPORT_SYMBOL_GPL(comedi_buf_read_free);
 344
 345 int comedi_buf_put(struct comedi_async *async, short x)
 346 {
 347         unsigned int n = __comedi_buf_write_alloc(async, sizeof(short), 1);
 348
 349         if (n < sizeof(short)) {
 350                 async->events |= COMEDI_CB_ERROR;
 351                 return 0;
 352         }
 353         *(short *)(async->prealloc_buf + async->buf_write_ptr) = x;
 354         comedi_buf_write_free(async, sizeof(short));
 355         return 1;
 356 }
 357 EXPORT_SYMBOL_GPL(comedi_buf_put);
 358
 359 int comedi_buf_get(struct comedi_async *async, short *x)
 360 {
 361         unsigned int n = comedi_buf_read_n_available(async);
 362
 363         if (n < sizeof(short))
 364                 return 0;
 365         comedi_buf_read_alloc(async, sizeof(short));
 366         *x = *(short *)(async->prealloc_buf + async->buf_read_ptr);
 367         comedi_buf_read_free(async, sizeof(short));
 368         return 1;
 369 }
 370 EXPORT_SYMBOL_GPL(comedi_buf_get);
 371
 372 void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
 373                           const void *data, unsigned int num_bytes)
 374 {
 375         unsigned int write_ptr = async->buf_write_ptr + offset;
 376
 377         if (write_ptr >= async->prealloc_bufsz)
 378                 write_ptr %= async->prealloc_bufsz;
 379
 380         while (num_bytes) {
 381                 unsigned int block_size;
 382
 383                 if (write_ptr + num_bytes > async->prealloc_bufsz)
 384                         block_size = async->prealloc_bufsz - write_ptr;
 385                 else
 386                         block_size = num_bytes;
 387
 388                 memcpy(async->prealloc_buf + write_ptr, data, block_size);
 389
 390                 data += block_size;
 391                 num_bytes -= block_size;
 392
 393                 write_ptr = 0;
 394         }
 395 }
 396 EXPORT_SYMBOL_GPL(comedi_buf_memcpy_to);
 397
 398 void comedi_buf_memcpy_from(struct comedi_async *async, unsigned int offset,
 399                             void *dest, unsigned int nbytes)
 400 {
 401         void *src;
 402         unsigned int read_ptr = async->buf_read_ptr + offset;
 403
 404         if (read_ptr >= async->prealloc_bufsz)
 405                 read_ptr %= async->prealloc_bufsz;
 406
 407         while (nbytes) {
 408                 unsigned int block_size;
 409
 410                 src = async->prealloc_buf + read_ptr;
 411
 412                 if (nbytes >= async->prealloc_bufsz - read_ptr)
 413                         block_size = async->prealloc_bufsz - read_ptr;
 414                 else
 415                         block_size = nbytes;
 416
 417                 memcpy(dest, src, block_size);
 418                 nbytes -= block_size;
 419                 dest += block_size;
 420                 read_ptr = 0;
 421         }
 422 }
 423 EXPORT_SYMBOL_GPL(comedi_buf_memcpy_from);