sys/contrib/vchiq/interface/vchiq_arm/vchiq_util.c

   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 }