LXR linux/drivers/staging/vc04_services/interface/vchiq_arm/vchiq

   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
  37static inline int is_pow2(int i)
  38{
  39        return i && !(i & (i - 1));
  40}
  41
  42int 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) {
  56                vchiu_queue_delete(queue);
  57                return 0;
  58        }
  59        return 1;
  60}
  61
  62void vchiu_queue_delete(VCHIU_QUEUE_T *queue)
  63{
  64        kfree(queue->storage);
  65}
  66
  67int vchiu_queue_is_empty(VCHIU_QUEUE_T *queue)
  68{
  69        return queue->read == queue->write;
  70}
  71
  72int vchiu_queue_is_full(VCHIU_QUEUE_T *queue)
  73{
  74        return queue->write == queue->read + queue->size;
  75}
  76
  77void 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
 106VCHIQ_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
 124VCHIQ_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}
 153