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