Home | History | Annotate | Line # | Download | only in vchiq_arm
vchiq_util.c revision 1.2.4.2 
      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