Branch data     Line data    Source code

       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                 :            : #include "vchiq_killable.h"
       6                 :            : 
       7                 :            : static inline int is_pow2(int i)
       8                 :            : {
       9   [ +  -  -  + ]:       1449 :         return i && !(i & (i - 1));
      10                 :            : }
      11                 :            : 
      12                 :       1449 : int vchiu_queue_init(struct vchiu_queue *queue, int size)
      13                 :            : {
      14         [ -  + ]:       1449 :         WARN_ON(!is_pow2(size));
      15                 :            : 
      16                 :       1449 :         queue->size = size;
      17                 :       1449 :         queue->read = 0;
      18                 :       1449 :         queue->write = 0;
      19                 :       1449 :         queue->initialized = 1;
      20                 :            : 
      21                 :            :         init_completion(&queue->pop);
      22                 :            :         init_completion(&queue->push);
      23                 :            : 
      24                 :       2896 :         queue->storage = kcalloc(size, sizeof(struct vchiq_header *),
      25                 :            :                                  GFP_KERNEL);
      26         [ -  + ]:       1448 :         if (!queue->storage) {
      27                 :            :                 vchiu_queue_delete(queue);
      28                 :          0 :                 return 0;
      29                 :            :         }
      30                 :            :         return 1;
      31                 :            : }
      32                 :            : 
      33                 :        207 : void vchiu_queue_delete(struct vchiu_queue *queue)
      34                 :            : {
      35                 :        207 :         kfree(queue->storage);
      36                 :        207 : }
      37                 :            : 
      38                 :       8487 : int vchiu_queue_is_empty(struct vchiu_queue *queue)
      39                 :            : {
      40                 :       8487 :         return queue->read == queue->write;
      41                 :            : }
      42                 :            : 
      43                 :       8073 : void vchiu_queue_push(struct vchiu_queue *queue, struct vchiq_header *header)
      44                 :            : {
      45         [ +  - ]:       8073 :         if (!queue->initialized)
      46                 :       8073 :                 return;
      47                 :            : 
      48         [ -  + ]:       8073 :         while (queue->write == queue->read + queue->size) {
      49         [ #  # ]:          0 :                 if (wait_for_completion_interruptible(&queue->pop))
      50                 :          0 :                         flush_signals(current);
      51                 :            :         }
      52                 :            : 
      53                 :       8073 :         queue->storage[queue->write & (queue->size - 1)] = header;
      54                 :       8073 :         queue->write++;
      55                 :            : 
      56                 :       8073 :         complete(&queue->push);
      57                 :            : }
      58                 :            : 
      59                 :        207 : struct vchiq_header *vchiu_queue_peek(struct vchiu_queue *queue)
      60                 :            : {
      61         [ -  + ]:        414 :         while (queue->write == queue->read) {
      62         [ #  # ]:          0 :                 if (wait_for_completion_interruptible(&queue->push))
      63                 :          0 :                         flush_signals(current);
      64                 :            :         }
      65                 :            : 
      66                 :        207 :         complete(&queue->push); // We haven't removed anything from the queue.
      67                 :            : 
      68                 :        207 :         return queue->storage[queue->read & (queue->size - 1)];
      69                 :            : }
      70                 :            : 
      71                 :       8073 : struct vchiq_header *vchiu_queue_pop(struct vchiu_queue *queue)
      72                 :            : {
      73                 :            :         struct vchiq_header *header;
      74                 :            : 
      75         [ -  + ]:      16146 :         while (queue->write == queue->read) {
      76         [ #  # ]:          0 :                 if (wait_for_completion_interruptible(&queue->push))
      77                 :          0 :                         flush_signals(current);
      78                 :            :         }
      79                 :            : 
      80                 :       8073 :         header = queue->storage[queue->read & (queue->size - 1)];
      81                 :       8073 :         queue->read++;
      82                 :            : 
      83                 :       8073 :         complete(&queue->pop);
      84                 :            : 
      85                 :       8073 :         return header;
      86                 :            : }