Home | History | Annotate | Line # | Download | only in vchiq_arm 
      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