drivers/staging/wilc1000/wilc_msgqueue.c

   1
   2 #include "wilc_msgqueue.h"
   3 #include <linux/spinlock.h>
   4 #include <linux/errno.h>
   5 #include <linux/slab.h>
   6
   7 /*!
   8  *  @author             syounan
   9  *  @date               1 Sep 2010
  10  *  @note               copied from FLO glue implementatuion
  11  *  @version            1.0
  12  */
  13 int wilc_mq_create(struct message_queue *mq)
  14 {
  15         spin_lock_init(&mq->lock);
  16         sema_init(&mq->sem, 0);
  17         INIT_LIST_HEAD(&mq->msg_list);
  18         mq->recv_count = 0;
  19         mq->exiting = false;
  20         return 0;
  21 }
  22
  23 /*!
  24  *  @author             syounan
  25  *  @date               1 Sep 2010
  26  *  @note               copied from FLO glue implementatuion
  27  *  @version            1.0
  28  */
  29 int wilc_mq_destroy(struct message_queue *mq)
  30 {
  31         struct message *msg;
  32
  33         mq->exiting = true;
  34
  35         /* Release any waiting receiver thread. */
  36         while (mq->recv_count > 0) {
  37                 up(&mq->sem);
  38                 mq->recv_count--;
  39         }
  40
  41         while (!list_empty(&mq->msg_list)) {
  42                 msg = list_first_entry(&mq->msg_list, struct message, list);
  43                 list_del(&msg->list);
  44                 kfree(msg->buf);
  45         }
  46
  47         return 0;
  48 }
  49
  50 /*!
  51  *  @author             syounan
  52  *  @date               1 Sep 2010
  53  *  @note               copied from FLO glue implementatuion
  54  *  @version            1.0
  55  */
  56 int wilc_mq_send(struct message_queue *mq,
  57                  const void *send_buf, u32 send_buf_size)
  58 {
  59         unsigned long flags;
  60         struct message *new_msg = NULL;
  61
  62         if (!mq || (send_buf_size == 0) || !send_buf)
  63                 return -EINVAL;
  64
  65         if (mq->exiting)
  66                 return -EFAULT;
  67
  68         /* construct a new message */
  69         new_msg = kmalloc(sizeof(*new_msg), GFP_ATOMIC);
  70         if (!new_msg)
  71                 return -ENOMEM;
  72
  73         new_msg->len = send_buf_size;
  74         INIT_LIST_HEAD(&new_msg->list);
  75         new_msg->buf = kmemdup(send_buf, send_buf_size, GFP_ATOMIC);
  76         if (!new_msg->buf) {
  77                 kfree(new_msg);
  78                 return -ENOMEM;
  79         }
  80
  81         spin_lock_irqsave(&mq->lock, flags);
  82
  83         /* add it to the message queue */
  84         list_add_tail(&new_msg->list, &mq->msg_list);
  85
  86         spin_unlock_irqrestore(&mq->lock, flags);
  87
  88         up(&mq->sem);
  89
  90         return 0;
  91 }
  92
  93 /*!
  94  *  @author             syounan
  95  *  @date               1 Sep 2010
  96  *  @note               copied from FLO glue implementatuion
  97  *  @version            1.0
  98  */
  99 int wilc_mq_recv(struct message_queue *mq,
 100                  void *recv_buf, u32 recv_buf_size, u32 *recv_len)
 101 {
 102         struct message *msg;
 103         unsigned long flags;
 104
 105         if (!mq || (recv_buf_size == 0) || !recv_buf || !recv_len)
 106                 return -EINVAL;
 107
 108         if (mq->exiting)
 109                 return -EFAULT;
 110
 111         spin_lock_irqsave(&mq->lock, flags);
 112         mq->recv_count++;
 113         spin_unlock_irqrestore(&mq->lock, flags);
 114
 115         down(&mq->sem);
 116         spin_lock_irqsave(&mq->lock, flags);
 117
 118         if (list_empty(&mq->msg_list)) {
 119                 spin_unlock_irqrestore(&mq->lock, flags);
 120                 up(&mq->sem);
 121                 return -EFAULT;
 122         }
 123         /* check buffer size */
 124         msg = list_first_entry(&mq->msg_list, struct message, list);
 125         if (recv_buf_size < msg->len) {
 126                 spin_unlock_irqrestore(&mq->lock, flags);
 127                 up(&mq->sem);
 128                 return -EOVERFLOW;
 129         }
 130
 131         /* consume the message */
 132         mq->recv_count--;
 133         memcpy(recv_buf, msg->buf, msg->len);
 134         *recv_len = msg->len;
 135
 136         list_del(&msg->list);
 137
 138         kfree(msg->buf);
 139         kfree(msg);
 140
 141         spin_unlock_irqrestore(&mq->lock, flags);
 142
 143         return 0;
 144 }