xref: /freebsd/sys/contrib/vchiq/interface/vchiq_arm/vchiq_util.c (revision bdd1243df58e60e85101c09001d9812a789b6bc4)
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