drivers/staging/unisys/visorutil/charqueue.c

   1 /* charqueue.c
   2  *
   3  * Copyright (C) 2010 - 2013 UNISYS CORPORATION
   4  * All rights reserved.
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or (at
   9  * your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful, but
  12  * WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
  14  * NON INFRINGEMENT.  See the GNU General Public License for more
  15  * details.
  16  */
  17
  18 /*
  19  *  Simple character queue implementation for Linux kernel mode.
  20  */
  21
  22 #include "charqueue.h"
  23
  24 #define MYDRVNAME "charqueue"
  25
  26 #define IS_EMPTY(charqueue) (charqueue->head == charqueue->tail)
  27
  28 struct charqueue {
  29         int alloc_size;
  30         int nslots;
  31         spinlock_t lock; /* read/write lock for this structure */
  32         int head, tail;
  33         unsigned char buf[0];
  34 };
  35
  36 struct charqueue *visor_charqueue_create(ulong nslots)
  37 {
  38         int alloc_size = sizeof(struct charqueue) + nslots + 1;
  39         struct charqueue *cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
  40
  41         if (cq == NULL) {
  42                 ERRDRV("visor_charqueue_create allocation failed (alloc_size=%d)",
  43                        alloc_size);
  44                 return NULL;
  45         }
  46         cq->alloc_size = alloc_size;
  47         cq->nslots = nslots;
  48         cq->head = 0;
  49         cq->tail = 0;
  50         spin_lock_init(&cq->lock);
  51         return cq;
  52 }
  53 EXPORT_SYMBOL_GPL(visor_charqueue_create);
  54
  55 void visor_charqueue_enqueue(struct charqueue *charqueue, unsigned char c)
  56 {
  57         int alloc_slots = charqueue->nslots+1;  /* 1 slot is always empty */
  58
  59         spin_lock(&charqueue->lock);
  60         charqueue->head = (charqueue->head+1) % alloc_slots;
  61         if (charqueue->head == charqueue->tail)
  62                 /* overflow; overwrite the oldest entry */
  63                 charqueue->tail = (charqueue->tail+1) % alloc_slots;
  64         charqueue->buf[charqueue->head] = c;
  65         spin_unlock(&charqueue->lock);
  66 }
  67 EXPORT_SYMBOL_GPL(visor_charqueue_enqueue);
  68
  69 BOOL visor_charqueue_is_empty(struct charqueue *charqueue)
  70 {
  71         BOOL b;
  72
  73         spin_lock(&charqueue->lock);
  74         b = IS_EMPTY(charqueue);
  75         spin_unlock(&charqueue->lock);
  76         return b;
  77 }
  78 EXPORT_SYMBOL_GPL(visor_charqueue_is_empty);
  79
  80 static int charqueue_dequeue_1(struct charqueue *charqueue)
  81 {
  82         int alloc_slots = charqueue->nslots + 1;  /* 1 slot is always empty */
  83
  84         if (IS_EMPTY(charqueue))
  85                 return -1;
  86         charqueue->tail = (charqueue->tail+1) % alloc_slots;
  87         return charqueue->buf[charqueue->tail];
  88 }
  89
  90 int charqueue_dequeue(struct charqueue *charqueue)
  91 {
  92         int rc;
  93
  94         spin_lock(&charqueue->lock);
  95         rc = charqueue_dequeue_1(charqueue);
  96         spin_unlock(&charqueue->lock);
  97         return rc;
  98 }
  99
 100 int visor_charqueue_dequeue_n(struct charqueue *charqueue, unsigned char *buf,
 101                               int n)
 102 {
 103         int rc, counter = 0, c;
 104
 105         spin_lock(&charqueue->lock);
 106         for (;;) {
 107                 if (n <= 0)
 108                         break;  /* no more buffer space */
 109                 c = charqueue_dequeue_1(charqueue);
 110                 if (c < 0)
 111                         break;  /* no more input */
 112                 *buf = (unsigned char)(c);
 113                 buf++;
 114                 n--;
 115                 counter++;
 116         }
 117         rc = counter;
 118         spin_unlock(&charqueue->lock);
 119         return rc;
 120 }
 121 EXPORT_SYMBOL_GPL(visor_charqueue_dequeue_n);
 122
 123 void visor_charqueue_destroy(struct charqueue *charqueue)
 124 {
 125         if (charqueue == NULL)
 126                 return;
 127         kfree(charqueue);
 128 }
 129 EXPORT_SYMBOL_GPL(visor_charqueue_destroy);