1 /** 2 * Copyright (c) 2010-2012 Broadcom. All rights reserved. 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 1. Redistributions of source code must retain the above copyright 8 * notice, this list of conditions, and the following disclaimer, 9 * without modification. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. The names of the above-listed copyright holders may not be used 14 * to endorse or promote products derived from this software without 15 * specific prior written permission. 16 * 17 * ALTERNATIVELY, this software may be distributed under the terms of the 18 * GNU General Public License ("GPL") version 2, as published by the Free 19 * Software Foundation. 20 * 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 22 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 23 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 25 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 28 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 29 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 30 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 */ 33 34 #include "vchiq_util.h" 35 36 static inline int is_pow2(int i) 37 { 38 return i && !(i & (i - 1)); 39 } 40 41 int vchiu_queue_init(VCHIU_QUEUE_T *queue, int size) 42 { 43 WARN_ON(!is_pow2(size)); 44 45 queue->size = size; 46 queue->read = 0; 47 queue->write = 0; 48 49 _sema_init(&queue->pop, 0); 50 _sema_init(&queue->push, 0); 51 52 queue->storage = kzalloc(size * sizeof(VCHIQ_HEADER_T *), GFP_KERNEL); 53 if (queue->storage == NULL) { 54 vchiu_queue_delete(queue); 55 return 0; 56 } 57 return 1; 58 } 59 60 void vchiu_queue_delete(VCHIU_QUEUE_T *queue) 61 { 62 if (queue->storage != NULL) 63 kfree(queue->storage); 64 } 65 66 int vchiu_queue_is_empty(VCHIU_QUEUE_T *queue) 67 { 68 return queue->read == queue->write; 69 } 70 71 int vchiu_queue_is_full(VCHIU_QUEUE_T *queue) 72 { 73 return queue->write == queue->read + queue->size; 74 } 75 76 void vchiu_queue_push(VCHIU_QUEUE_T *queue, VCHIQ_HEADER_T *header) 77 { 78 while (queue->write == queue->read + queue->size) { 79 if (down_interruptible(&queue->pop) != 0) { 80 flush_signals(current); 81 } 82 } 83 84 queue->storage[queue->write & (queue->size - 1)] = header; 85 86 queue->write++; 87 88 up(&queue->push); 89 } 90 91 VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue) 92 { 93 while (queue->write == queue->read) { 94 if (down_interruptible(&queue->push) != 0) { 95 flush_signals(current); 96 } 97 } 98 99 up(&queue->push); // We haven't removed anything from the queue. 100 return queue->storage[queue->read & (queue->size - 1)]; 101 } 102 103 VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue) 104 { 105 VCHIQ_HEADER_T *header; 106 107 while (queue->write == queue->read) { 108 if (down_interruptible(&queue->push) != 0) { 109 flush_signals(current); 110 } 111 } 112 113 header = queue->storage[queue->read & (queue->size - 1)]; 114 115 queue->read++; 116 117 up(&queue->pop); 118 119 return header; 120 } 121