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