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