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;
  40
  41         cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
  42         if (cq == NULL)
  43                 return NULL;
  44         cq->alloc_size = alloc_size;
  45         cq->nslots = nslots;
  46         cq->head = 0;
  47         cq->tail = 0;
  48         spin_lock_init(&cq->lock);
  49         return cq;
  50 }
  51 EXPORT_SYMBOL_GPL(visor_charqueue_create);
  52
  53 void visor_charqueue_enqueue(struct charqueue *charqueue, unsigned char c)
  54 {
  55         int alloc_slots = charqueue->nslots+1;  /* 1 slot is always empty */
  56
  57         spin_lock(&charqueue->lock);
  58         charqueue->head = (charqueue->head+1) % alloc_slots;
  59         if (charqueue->head == charqueue->tail)
  60                 /* overflow; overwrite the oldest entry */
  61                 charqueue->tail = (charqueue->tail+1) % alloc_slots;
  62         charqueue->buf[charqueue->head] = c;
  63         spin_unlock(&charqueue->lock);
  64 }
  65 EXPORT_SYMBOL_GPL(visor_charqueue_enqueue);
  66
  67 BOOL visor_charqueue_is_empty(struct charqueue *charqueue)
  68 {
  69         BOOL b;
  70
  71         spin_lock(&charqueue->lock);
  72         b = IS_EMPTY(charqueue);
  73         spin_unlock(&charqueue->lock);
  74         return b;
  75 }
  76 EXPORT_SYMBOL_GPL(visor_charqueue_is_empty);
  77
  78 static int charqueue_dequeue_1(struct charqueue *charqueue)
  79 {
  80         int alloc_slots = charqueue->nslots + 1;  /* 1 slot is always empty */
  81
  82         if (IS_EMPTY(charqueue))
  83                 return -1;
  84         charqueue->tail = (charqueue->tail+1) % alloc_slots;
  85         return charqueue->buf[charqueue->tail];
  86 }
  87
  88 int charqueue_dequeue(struct charqueue *charqueue)
  89 {
  90         int rc;
  91
  92         spin_lock(&charqueue->lock);
  93         rc = charqueue_dequeue_1(charqueue);
  94         spin_unlock(&charqueue->lock);
  95         return rc;
  96 }
  97
  98 int visor_charqueue_dequeue_n(struct charqueue *charqueue, unsigned char *buf,
  99                               int n)
 100 {
 101         int rc, counter = 0, c;
 102
 103         spin_lock(&charqueue->lock);
 104         for (;;) {
 105                 if (n <= 0)
 106                         break;  /* no more buffer space */
 107                 c = charqueue_dequeue_1(charqueue);
 108                 if (c < 0)
 109                         break;  /* no more input */
 110                 *buf = (unsigned char)(c);
 111                 buf++;
 112                 n--;
 113                 counter++;
 114         }
 115         rc = counter;
 116         spin_unlock(&charqueue->lock);
 117         return rc;
 118 }
 119 EXPORT_SYMBOL_GPL(visor_charqueue_dequeue_n);
 120
 121 void visor_charqueue_destroy(struct charqueue *charqueue)
 122 {
 123         if (charqueue == NULL)
 124                 return;
 125         kfree(charqueue);
 126 }
 127 EXPORT_SYMBOL_GPL(visor_charqueue_destroy);