diff options
Diffstat (limited to 'drivers/staging/unisys/visorutil/charqueue.c')
-rw-r--r-- | drivers/staging/unisys/visorutil/charqueue.c | 141 |
1 files changed, 141 insertions, 0 deletions
diff --git a/drivers/staging/unisys/visorutil/charqueue.c b/drivers/staging/unisys/visorutil/charqueue.c new file mode 100644 index 00000000000..0ceede129e8 --- /dev/null +++ b/drivers/staging/unisys/visorutil/charqueue.c @@ -0,0 +1,141 @@ +/* charqueue.c + * + * Copyright © 2010 - 2013 UNISYS CORPORATION + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or (at + * your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or + * NON INFRINGEMENT. See the GNU General Public License for more + * details. + */ + +/* + * Simple character queue implementation for Linux kernel mode. + */ + +#include "charqueue.h" + +#define MYDRVNAME "charqueue" + +#define IS_EMPTY(charqueue) (charqueue->head == charqueue->tail) + + + +struct CHARQUEUE_Tag { + int alloc_size; + int nslots; + spinlock_t lock; + int head, tail; + unsigned char buf[0]; +}; + + + +CHARQUEUE *visor_charqueue_create(ulong nslots) +{ + int alloc_size = sizeof(CHARQUEUE) + nslots + 1; + CHARQUEUE *cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY); + if (cq == NULL) { + ERRDRV("visor_charqueue_create allocation failed (alloc_size=%d)", + alloc_size); + return NULL; + } + cq->alloc_size = alloc_size; + cq->nslots = nslots; + cq->head = cq->tail = 0; + spin_lock_init(&cq->lock); + return cq; +} +EXPORT_SYMBOL_GPL(visor_charqueue_create); + + + +void visor_charqueue_enqueue(CHARQUEUE *charqueue, unsigned char c) +{ + int alloc_slots = charqueue->nslots+1; /* 1 slot is always empty */ + + spin_lock(&charqueue->lock); + charqueue->head = (charqueue->head+1) % alloc_slots; + if (charqueue->head == charqueue->tail) + /* overflow; overwrite the oldest entry */ + charqueue->tail = (charqueue->tail+1) % alloc_slots; + charqueue->buf[charqueue->head] = c; + spin_unlock(&charqueue->lock); +} +EXPORT_SYMBOL_GPL(visor_charqueue_enqueue); + + + +BOOL visor_charqueue_is_empty(CHARQUEUE *charqueue) +{ + BOOL b; + spin_lock(&charqueue->lock); + b = IS_EMPTY(charqueue); + spin_unlock(&charqueue->lock); + return b; +} +EXPORT_SYMBOL_GPL(visor_charqueue_is_empty); + + + +static int charqueue_dequeue_1(CHARQUEUE *charqueue) +{ + int alloc_slots = charqueue->nslots + 1; /* 1 slot is always empty */ + + if (IS_EMPTY(charqueue)) + return -1; + charqueue->tail = (charqueue->tail+1) % alloc_slots; + return charqueue->buf[charqueue->tail]; +} + + + +int charqueue_dequeue(CHARQUEUE *charqueue) +{ + int rc; + + spin_lock(&charqueue->lock); + rc = charqueue_dequeue_1(charqueue); + spin_unlock(&charqueue->lock); + return rc; +} + + + +int visor_charqueue_dequeue_n(CHARQUEUE *charqueue, unsigned char *buf, int n) +{ + int rc, counter = 0, c; + + spin_lock(&charqueue->lock); + for (;;) { + if (n <= 0) + break; /* no more buffer space */ + c = charqueue_dequeue_1(charqueue); + if (c < 0) + break; /* no more input */ + *buf = (unsigned char)(c); + buf++; + n--; + counter++; + } + rc = counter; + spin_unlock(&charqueue->lock); + return rc; +} +EXPORT_SYMBOL_GPL(visor_charqueue_dequeue_n); + + + +void visor_charqueue_destroy(CHARQUEUE *charqueue) +{ + if (charqueue == NULL) + return; + kfree(charqueue); +} +EXPORT_SYMBOL_GPL(visor_charqueue_destroy); |