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 /* 85 * Write to queue->storage must be visible after read from 86 * queue->read 87 */ 88 smp_mb(); 89 90 queue->storage[queue->write & (queue->size - 1)] = header; 91 92 /* 93 * Write to queue->storage must be visible before write to 94 * queue->write 95 */ 96 smp_wmb(); 97 98 queue->write++; 99 100 up(&queue->push); 101 } 102 103 VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue) 104 { 105 while (queue->write == queue->read) { 106 if (down_interruptible(&queue->push) != 0) { 107 flush_signals(current); 108 } 109 } 110 111 up(&queue->push); // We haven't removed anything from the queue. 112 113 /* 114 * Read from queue->storage must be visible after read from 115 * queue->write 116 */ 117 smp_rmb(); 118 119 return queue->storage[queue->read & (queue->size - 1)]; 120 } 121 122 VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue) 123 { 124 VCHIQ_HEADER_T *header; 125 126 while (queue->write == queue->read) { 127 if (down_interruptible(&queue->push) != 0) { 128 flush_signals(current); 129 } 130 } 131 132 /* 133 * Read from queue->storage must be visible after read from 134 * queue->write 135 */ 136 smp_rmb(); 137 138 header = queue->storage[queue->read & (queue->size - 1)]; 139 140 /* 141 * Read from queue->storage must be visible before write to 142 * queue->read 143 */ 144 smp_mb(); 145 146 queue->read++; 147 148 up(&queue->pop); 149 150 return header; 151 } 152