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 39 static inline struct SimpleQueueEntry *simpleQueueGetNth(struct SimpleQueue* sq, uint32_t n) 40 { 41 return (struct SimpleQueueEntry*)(sq->data + n * sq->entrySz); 42 } 43 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 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 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 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 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 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