drivers/staging/wilc1000/wilc_msgqueue.c

   1
   2 #include "wilc_msgqueue.h"
   3 #include <linux/spinlock.h>
   4 #include "linux_wlan_common.h"
   5 #include <linux/errno.h>
   6 #include <linux/slab.h>
   7
   8 /*!
   9  *  @author             syounan
  10  *  @date               1 Sep 2010
  11  *  @note               copied from FLO glue implementatuion
  12  *  @version            1.0
  13  */
  14 int wilc_mq_create(struct message_queue *mq)
  15 {
  16         spin_lock_init(&mq->lock);
  17         sema_init(&mq->sem, 0);
  18         mq->msg_list = NULL;
  19         mq->recv_count = 0;
  20         mq->exiting = false;
  21         return 0;
  22 }
  23
  24 /*!
  25  *  @author             syounan
  26  *  @date               1 Sep 2010
  27  *  @note               copied from FLO glue implementatuion
  28  *  @version            1.0
  29  */
  30 int wilc_mq_destroy(struct message_queue *mq)
  31 {
  32         mq->exiting = true;
  33
  34         /* Release any waiting receiver thread. */
  35         while (mq->recv_count > 0) {
  36                 up(&mq->sem);
  37                 mq->recv_count--;
  38         }
  39
  40         while (mq->msg_list) {
  41                 struct message *msg = mq->msg_list->next;
  42
  43                 kfree(mq->msg_list);
  44                 mq->msg_list = msg;
  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                 PRINT_ER("mq or send_buf is null\n");
  64                 return -EINVAL;
  65         }
  66
  67         if (mq->exiting) {
  68                 PRINT_ER("mq fail\n");
  69                 return -EFAULT;
  70         }
  71
  72         /* construct a new message */
  73         new_msg = kmalloc(sizeof(struct message), GFP_ATOMIC);
  74         if (!new_msg)
  75                 return -ENOMEM;
  76
  77         new_msg->len = send_buf_size;
  78         new_msg->next = NULL;
  79         new_msg->buf = kmemdup(send_buf, send_buf_size, GFP_ATOMIC);
  80         if (!new_msg->buf) {
  81                 kfree(new_msg);
  82                 return -ENOMEM;
  83         }
  84
  85         spin_lock_irqsave(&mq->lock, flags);
  86
  87         /* add it to the message queue */
  88         if (!mq->msg_list) {
  89                 mq->msg_list  = new_msg;
  90         } else {
  91                 struct message *tail_msg = mq->msg_list;
  92
  93                 while (tail_msg->next)
  94                         tail_msg = tail_msg->next;
  95
  96                 tail_msg->next = new_msg;
  97         }
  98
  99         spin_unlock_irqrestore(&mq->lock, flags);
 100
 101         up(&mq->sem);
 102
 103         return 0;
 104 }
 105
 106 /*!
 107  *  @author             syounan
 108  *  @date               1 Sep 2010
 109  *  @note               copied from FLO glue implementatuion
 110  *  @version            1.0
 111  */
 112 int wilc_mq_recv(struct message_queue *mq,
 113                  void *recv_buf, u32 recv_buf_size, u32 *recv_len)
 114 {
 115         struct message *pstrMessage;
 116         unsigned long flags;
 117
 118         if ((!mq) || (recv_buf_size == 0)
 119             || (!recv_buf) || (!recv_len)) {
 120                 PRINT_ER("mq or recv_buf is null\n");
 121                 return -EINVAL;
 122         }
 123
 124         if (mq->exiting) {
 125                 PRINT_ER("mq fail\n");
 126                 return -EFAULT;
 127         }
 128
 129         spin_lock_irqsave(&mq->lock, flags);
 130         mq->recv_count++;
 131         spin_unlock_irqrestore(&mq->lock, flags);
 132
 133         down(&mq->sem);
 134         spin_lock_irqsave(&mq->lock, flags);
 135
 136         pstrMessage = mq->msg_list;
 137         if (!pstrMessage) {
 138                 spin_unlock_irqrestore(&mq->lock, flags);
 139                 PRINT_ER("pstrMessage is null\n");
 140                 return -EFAULT;
 141         }
 142         /* check buffer size */
 143         if (recv_buf_size < pstrMessage->len) {
 144                 spin_unlock_irqrestore(&mq->lock, flags);
 145                 up(&mq->sem);
 146                 PRINT_ER("recv_buf_size overflow\n");
 147                 return -EOVERFLOW;
 148         }
 149
 150         /* consume the message */
 151         mq->recv_count--;
 152         memcpy(recv_buf, pstrMessage->buf, pstrMessage->len);
 153         *recv_len = pstrMessage->len;
 154
 155         mq->msg_list = pstrMessage->next;
 156
 157         kfree(pstrMessage->buf);
 158         kfree(pstrMessage);
 159
 160         spin_unlock_irqrestore(&mq->lock, flags);
 161
 162         return 0;
 163 }