root/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.c

DEFINITIONS

1. is_pow2
2. vchiu_queue_init
3. vchiu_queue_delete
4. vchiu_queue_is_empty
5. vchiu_queue_push
6. vchiu_queue_peek
7. vchiu_queue_pop

   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 
   6 static inline int is_pow2(int i)
   7 {
   8         return i && !(i & (i - 1));
   9 }
  10 
  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 
  32 void vchiu_queue_delete(struct vchiu_queue *queue)
  33 {
  34         kfree(queue->storage);
  35 }
  36 
  37 int vchiu_queue_is_empty(struct vchiu_queue *queue)
  38 {
  39         return queue->read == queue->write;
  40 }
  41 
  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 
  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 
  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 }