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 *pvRecvBuffer, u32 u32RecvBufferSize,
 114                  u32 *pu32ReceivedLength)
 115 {
 116         struct message *pstrMessage;
 117         unsigned long flags;
 118
 119         if ((!mq) || (u32RecvBufferSize == 0)
 120             || (!pvRecvBuffer) || (!pu32ReceivedLength)) {
 121                 PRINT_ER("mq or pvRecvBuffer is null\n");
 122                 return -EINVAL;
 123         }
 124
 125         if (mq->exiting) {
 126                 PRINT_ER("mq fail\n");
 127                 return -EFAULT;
 128         }
 129
 130         spin_lock_irqsave(&mq->lock, flags);
 131         mq->recv_count++;
 132         spin_unlock_irqrestore(&mq->lock, flags);
 133
 134         down(&mq->sem);
 135         spin_lock_irqsave(&mq->lock, flags);
 136
 137         pstrMessage = mq->msg_list;
 138         if (!pstrMessage) {
 139                 spin_unlock_irqrestore(&mq->lock, flags);
 140                 PRINT_ER("pstrMessage is null\n");
 141                 return -EFAULT;
 142         }
 143         /* check buffer size */
 144         if (u32RecvBufferSize < pstrMessage->len) {
 145                 spin_unlock_irqrestore(&mq->lock, flags);
 146                 up(&mq->sem);
 147                 PRINT_ER("u32RecvBufferSize overflow\n");
 148                 return -EOVERFLOW;
 149         }
 150
 151         /* consume the message */
 152         mq->recv_count--;
 153         memcpy(pvRecvBuffer, pstrMessage->buf, pstrMessage->len);
 154         *pu32ReceivedLength = pstrMessage->len;
 155
 156         mq->msg_list = pstrMessage->next;
 157
 158         kfree(pstrMessage->buf);
 159         kfree(pstrMessage);
 160
 161         spin_unlock_irqrestore(&mq->lock, flags);
 162
 163         return 0;
 164 }