1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <simpleQ.h>
18 #include <stddef.h>
19 #include <string.h>
20 #include <stdio.h>
21 #include <heap.h>
22
23
24 struct SimpleQueueEntry {
25 uint32_t nextIdx : 31;
26 uint32_t discardable : 1;
27 uint8_t data[];
28 };
29
30 struct SimpleQueue {
31 SimpleQueueForciblyDiscardCbkF discardCbk;
32 uint32_t head, tail, num, freeHead, entrySz;
33 uint8_t data[];
34 };
35
36 #define SIMPLE_QUEUE_IDX_NONE (SIMPLE_QUEUE_MAX_ELEMENTS + 1)
37
38 //no bounds checking
simpleQueueGetNth(struct SimpleQueue * sq,uint32_t n)39 static inline struct SimpleQueueEntry *simpleQueueGetNth(struct SimpleQueue* sq, uint32_t n)
40 {
41 return (struct SimpleQueueEntry*)(sq->data + n * sq->entrySz);
42 }
43
simpleQueueGetIdx(struct SimpleQueue * sq,const struct SimpleQueueEntry * e)44 static inline uint32_t simpleQueueGetIdx(struct SimpleQueue* sq, const struct SimpleQueueEntry *e)
45 {
46 return (((const uint8_t*)e) - sq->data) / sq->entrySz;
47 }
48
simpleQueueAlloc(uint32_t numEntries,uint32_t entrySz,SimpleQueueForciblyDiscardCbkF forceDiscardCbk)49 struct SimpleQueue* simpleQueueAlloc(uint32_t numEntries, uint32_t entrySz, SimpleQueueForciblyDiscardCbkF forceDiscardCbk)
50 {
51 uint32_t i, sz = sizeof(struct SimpleQueue) + (sizeof(struct SimpleQueueEntry) + entrySz) * numEntries;
52 struct SimpleQueue *sq;
53
54 if (numEntries > SIMPLE_QUEUE_MAX_ELEMENTS)
55 return NULL;
56
57 sq = heapAlloc(sz);
58 if (!sq)
59 return NULL;
60
61 memset(sq, 0, sz);
62
63 sq->discardCbk = forceDiscardCbk;
64 sq->head = SIMPLE_QUEUE_IDX_NONE;
65 sq->tail = SIMPLE_QUEUE_IDX_NONE;
66 sq->entrySz = entrySz + sizeof(struct SimpleQueueEntry);
67 sq->freeHead = 0;
68 sq->num = numEntries;
69
70 //init the freelist
71 for (i = 0; i < numEntries - 1; i++)
72 simpleQueueGetNth(sq, i)->nextIdx = i + 1;
73
74 simpleQueueGetNth(sq, numEntries - 1)->nextIdx = SIMPLE_QUEUE_IDX_NONE;
75
76 return sq;
77 }
78
simpleQueueDestroy(struct SimpleQueue * sq)79 void simpleQueueDestroy(struct SimpleQueue* sq)
80 {
81 SimpleQueueForciblyDiscardCbkF discard = sq->discardCbk;
82 struct SimpleQueueEntry *cur;
83 uint32_t i;
84
85 for (i = sq->head; i != SIMPLE_QUEUE_IDX_NONE; i = cur->nextIdx) {
86 cur = simpleQueueGetNth(sq, i);
87 discard(cur->data, true);
88 }
89
90 heapFree(sq);
91 }
92
simpleQueueDequeue(struct SimpleQueue * sq,void * data)93 bool simpleQueueDequeue(struct SimpleQueue* sq, void *data)
94 {
95 struct SimpleQueueEntry *e;
96 uint32_t head;
97
98 if (!sq || sq->head == SIMPLE_QUEUE_IDX_NONE)
99 return false;
100
101 head = sq->head;
102 e = simpleQueueGetNth(sq, head);
103
104 sq->head = e->nextIdx;
105 if (sq->tail == head)
106 sq->tail = SIMPLE_QUEUE_IDX_NONE;
107
108 memcpy(data, e->data, sq->entrySz - sizeof(struct SimpleQueueEntry));
109
110 e->nextIdx = sq->freeHead;
111 sq->freeHead = simpleQueueGetIdx(sq, e);
112
113 return true;
114 }
115
116 //if this is called, we need to discard at least one entry. we prefer to discard the oldest item
simpleQueueAllocWithDiscard(struct SimpleQueue * sq)117 static struct SimpleQueueEntry* simpleQueueAllocWithDiscard(struct SimpleQueue* sq)
118 {
119 struct SimpleQueueEntry* cur = NULL;
120 uint32_t idx, prev = SIMPLE_QUEUE_IDX_NONE;
121
122 for (idx = sq->head; idx != SIMPLE_QUEUE_IDX_NONE; prev=idx, idx = cur->nextIdx) {
123 cur = simpleQueueGetNth(sq, idx);
124
125 if (!cur->discardable)
126 continue;
127
128 //try to discard it
129 if (sq->discardCbk(cur->data, false)) {
130
131 //unlink
132 if (prev == SIMPLE_QUEUE_IDX_NONE)
133 sq->head = cur->nextIdx;
134 else
135 simpleQueueGetNth(sq, prev)->nextIdx = cur->nextIdx;
136 if (sq->tail == idx)
137 sq->tail = prev;
138
139 return cur;
140 }
141 }
142
143 return NULL;
144 }
145
simpleQueueEnqueue(struct SimpleQueue * sq,const void * data,int length,bool possiblyDiscardable)146 bool simpleQueueEnqueue(struct SimpleQueue* sq, const void *data, int length, bool possiblyDiscardable)
147 {
148 struct SimpleQueueEntry *e = NULL;
149
150 if (length > sq->entrySz - sizeof(struct SimpleQueueEntry))
151 return false;
152
153 //first try a simple alloc
154 if (sq->freeHead != SIMPLE_QUEUE_IDX_NONE) {
155 e = simpleQueueGetNth(sq, sq->freeHead);
156 sq->freeHead = e->nextIdx;
157 }
158
159 //if no luck, it gets complicated
160 if (!e)
161 e = simpleQueueAllocWithDiscard(sq);
162
163 //and we may have to give up
164 if (!e)
165 return false;
166
167 //link it in
168 e->nextIdx = SIMPLE_QUEUE_IDX_NONE;
169 if (sq->head == SIMPLE_QUEUE_IDX_NONE) // head = none implies tail = none
170 sq->head = simpleQueueGetIdx(sq, e);
171 else
172 simpleQueueGetNth(sq, sq->tail)->nextIdx = simpleQueueGetIdx(sq, e);
173 sq->tail = simpleQueueGetIdx(sq, e);
174
175 //fill in the data
176 memcpy(e->data, data, length);
177 e->discardable = possiblyDiscardable ? 1 : 0;
178
179 return true;
180 }
181