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 = kzalloc(size * sizeof(VCHIQ_HEADER_T *), GFP_KERNEL);
  55         if (queue->storage == NULL) {
  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         if (queue->storage != NULL)
  65                 kfree(queue->storage);
  66 }
  67
  68 int vchiu_queue_is_empty(VCHIU_QUEUE_T *queue)
  69 {
  70         return queue->read == queue->write;
  71 }
  72
  73 int vchiu_queue_is_full(VCHIU_QUEUE_T *queue)
  74 {
  75         return queue->write == queue->read + queue->size;
  76 }
  77
  78 void vchiu_queue_push(VCHIU_QUEUE_T *queue, VCHIQ_HEADER_T *header)
  79 {
  80         if (!queue->initialized)
  81                 return;
  82
  83         while (queue->write == queue->read + queue->size) {
  84                 if (down_interruptible(&queue->pop) != 0) {
  85                         flush_signals(current);
  86                 }
  87         }
  88
  89         /*
  90          * Write to queue->storage must be visible after read from
  91          * queue->read
  92          */
  93         smp_mb();
  94
  95         queue->storage[queue->write & (queue->size - 1)] = header;
  96
  97         /*
  98          * Write to queue->storage must be visible before write to
  99          * queue->write
 100          */
 101         smp_wmb();
 102
 103         queue->write++;
 104
 105         up(&queue->push);
 106 }
 107
 108 VCHIQ_HEADER_T *vchiu_queue_peek(VCHIU_QUEUE_T *queue)
 109 {
 110         while (queue->write == queue->read) {
 111                 if (down_interruptible(&queue->push) != 0) {
 112                         flush_signals(current);
 113                 }
 114         }
 115
 116         up(&queue->push); // We haven't removed anything from the queue.
 117
 118         /*
 119          * Read from queue->storage must be visible after read from
 120          * queue->write
 121          */
 122         smp_rmb();
 123
 124         return queue->storage[queue->read & (queue->size - 1)];
 125 }
 126
 127 VCHIQ_HEADER_T *vchiu_queue_pop(VCHIU_QUEUE_T *queue)
 128 {
 129         VCHIQ_HEADER_T *header;
 130
 131         while (queue->write == queue->read) {
 132                 if (down_interruptible(&queue->push) != 0) {
 133                         flush_signals(current);
 134                 }
 135         }
 136
 137         /*
 138          * Read from queue->storage must be visible after read from
 139          * queue->write
 140          */
 141         smp_rmb();
 142
 143         header = queue->storage[queue->read & (queue->size - 1)];
 144
 145         /*
 146          * Read from queue->storage must be visible before write to
 147          * queue->read
 148          */
 149         smp_mb();
 150
 151         queue->read++;
 152
 153         up(&queue->pop);
 154
 155         return header;
 156 }