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