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 queue->initialized = 1; 49 50 _sema_init(&queue->pop, 0); 51 _sema_init(&queue->push, 0); 52 53 queue->storage = kzalloc(size * sizeof(VCHIQ_HEADER_T *), GFP_KERNEL); 54 if (queue->storage == NULL) { 55 vchiu_queue_delete(queue); 56 return 0; 57 } 58 return 1; 59 } 60 61 void vchiu_queue_delete(VCHIU_QUEUE_T *queue) 62 { 63 if (queue->storage != NULL) 64 kfree(queue->storage); 65 } 66 67 int vchiu_queue_is_empty(VCHIU_QUEUE_T *queue) 68 { 69 return queue->read == queue->write; 70 } 71 72 int vchiu_queue_is_full(VCHIU_QUEUE_T *queue) 73 { 74 return queue->write == queue->read + queue->size; 75 } 76 77 void vchiu_queue_push(VCHIU_QUEUE_T *queue, VCHIQ_HEADER_T *header) 78 { 79 if (!queue->initialized) 80 return; 81 82 while (queue->write == queue->read + queue->size) { 83 if (down_interruptible(&queue->pop) != 0) { 84 flush_signals(current); 85 } 86 } 87 88 /* 89 * Write to queue->storage must be visible after read from 90 * queue->read 91 */ 92 smp_mb(); 93 94 queue->storage[queue->write & (queue->size - 1)] = header; 95 96 /* 97 * Write to queue->storage must be visible before write to 98 * queue->write 99 */ 100 smp_wmb(); 101 102 queue->write++; 103 104 up(&queue->push); 105 } 106 107 VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue) 108 { 109 while (queue->write == queue->read) { 110 if (down_interruptible(&queue->push) != 0) { 111 flush_signals(current); 112 } 113 } 114 115 up(&queue->push); // We haven't removed anything from the queue. 116 117 /* 118 * Read from queue->storage must be visible after read from 119 * queue->write 120 */ 121 smp_rmb(); 122 123 return queue->storage[queue->read & (queue->size - 1)]; 124 } 125 126 VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue) 127 { 128 VCHIQ_HEADER_T *header; 129 130 while (queue->write == queue->read) { 131 if (down_interruptible(&queue->push) != 0) { 132 flush_signals(current); 133 } 134 } 135 136 /* 137 * Read from queue->storage must be visible after read from 138 * queue->write 139 */ 140 smp_rmb(); 141 142 header = queue->storage[queue->read & (queue->size - 1)]; 143 144 /* 145 * Read from queue->storage must be visible before write to 146 * queue->read 147 */ 148 smp_mb(); 149 150 queue->read++; 151 152 up(&queue->pop); 153 154 return header; 155 } 156