drivers/staging/vc04_services/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 #include "vchiq_killable.h"
  36
  37 static inline int is_pow2(int i)
  38 {
  39         return i && !(i & (i - 1));
  40 }
  41
  42 int vchiu_queue_init(VCHIU_QUEUE_T *queue, int size)
  43 {
  44         WARN_ON(!is_pow2(size));
  45
  46         queue->size = size;
  47         queue->read = 0;
  48         queue->write = 0;
  49         queue->initialized = 1;
  50
  51         sema_init(&queue->pop, 0);
  52         sema_init(&queue->push, 0);
  53
  54         queue->storage = kcalloc(size, sizeof(VCHIQ_HEADER_T *), GFP_KERNEL);
  55         if (!queue->storage) {
  56                 vchiu_queue_delete(queue);
  57                 return 0;
  58         }
  59         return 1;
  60 }
  61
  62 void vchiu_queue_delete(VCHIU_QUEUE_T *queue)
  63 {
  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          * Write to queue->storage must be visible after read from
  89          * queue->read
  90          */
  91         smp_mb();
  92
  93         queue->storage[queue->write & (queue->size - 1)] = header;
  94
  95         /*
  96          * Write to queue->storage must be visible before write to
  97          * queue->write
  98          */
  99         smp_wmb();
 100
 101         queue->write++;
 102
 103         up(&queue->push);
 104 }
 105
 106 VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue)
 107 {
 108         while (queue->write == queue->read) {
 109                 if (down_interruptible(&queue->push) != 0)
 110                         flush_signals(current);
 111         }
 112
 113         up(&queue->push); // We haven't removed anything from the queue.
 114
 115         /*
 116          * Read from queue->storage must be visible after read from
 117          * queue->write
 118          */
 119         smp_rmb();
 120
 121         return queue->storage[queue->read & (queue->size - 1)];
 122 }
 123
 124 VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue)
 125 {
 126         VCHIQ_HEADER_T *header;
 127
 128         while (queue->write == queue->read) {
 129                 if (down_interruptible(&queue->push) != 0)
 130                         flush_signals(current);
 131         }
 132
 133         /*
 134          * Read from queue->storage must be visible after read from
 135          * queue->write
 136          */
 137         smp_rmb();
 138
 139         header = queue->storage[queue->read & (queue->size - 1)];
 140
 141         /*
 142          * Read from queue->storage must be visible before write to
 143          * queue->read
 144          */
 145         smp_mb();
 146
 147         queue->read++;
 148
 149         up(&queue->pop);
 150
 151         return header;
 152 }