1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
2 /* Copyright (c) 2010-2012 Broadcom. All rights reserved. */
3 
4 #include "vchiq_util.h"
5 
is_pow2(int i)6 static inline int is_pow2(int i)
7 {
8 	return i && !(i & (i - 1));
9 }
10 
vchiu_queue_init(struct vchiu_queue * queue,int size)11 int vchiu_queue_init(struct vchiu_queue *queue, int size)
12 {
13 	WARN_ON(!is_pow2(size));
14 
15 	queue->size = size;
16 	queue->read = 0;
17 	queue->write = 0;
18 	queue->initialized = 1;
19 
20 	init_completion(&queue->pop);
21 	init_completion(&queue->push);
22 
23 	queue->storage = kcalloc(size, sizeof(struct vchiq_header *),
24 				 GFP_KERNEL);
25 	if (!queue->storage) {
26 		vchiu_queue_delete(queue);
27 		return 0;
28 	}
29 	return 1;
30 }
31 
vchiu_queue_delete(struct vchiu_queue * queue)32 void vchiu_queue_delete(struct vchiu_queue *queue)
33 {
34 	kfree(queue->storage);
35 }
36 
vchiu_queue_is_empty(struct vchiu_queue * queue)37 int vchiu_queue_is_empty(struct vchiu_queue *queue)
38 {
39 	return queue->read == queue->write;
40 }
41 
vchiu_queue_push(struct vchiu_queue * queue,struct vchiq_header * header)42 void vchiu_queue_push(struct vchiu_queue *queue, struct vchiq_header *header)
43 {
44 	if (!queue->initialized)
45 		return;
46 
47 	while (queue->write == queue->read + queue->size) {
48 		if (wait_for_completion_interruptible(&queue->pop))
49 			flush_signals(current);
50 	}
51 
52 	queue->storage[queue->write & (queue->size - 1)] = header;
53 	queue->write++;
54 
55 	complete(&queue->push);
56 }
57 
vchiu_queue_peek(struct vchiu_queue * queue)58 struct vchiq_header *vchiu_queue_peek(struct vchiu_queue *queue)
59 {
60 	while (queue->write == queue->read) {
61 		if (wait_for_completion_interruptible(&queue->push))
62 			flush_signals(current);
63 	}
64 
65 	complete(&queue->push); // We haven't removed anything from the queue.
66 
67 	return queue->storage[queue->read & (queue->size - 1)];
68 }
69 
vchiu_queue_pop(struct vchiu_queue * queue)70 struct vchiq_header *vchiu_queue_pop(struct vchiu_queue *queue)
71 {
72 	struct vchiq_header *header;
73 
74 	while (queue->write == queue->read) {
75 		if (wait_for_completion_interruptible(&queue->push))
76 			flush_signals(current);
77 	}
78 
79 	header = queue->storage[queue->read & (queue->size - 1)];
80 	queue->read++;
81 
82 	complete(&queue->pop);
83 
84 	return header;
85 }
86