NFC: Queue pn533 commands

[cascardo/linux.git] / drivers / nfc / pn533.c

diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index d606f52..7ceea11 100644 (file)
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -356,6 +356,7 @@ struct pn533 {
 
        struct workqueue_struct *wq;
        struct work_struct cmd_work;
+       struct work_struct cmd_complete_work;
        struct work_struct poll_work;
        struct work_struct mi_work;
        struct work_struct tg_work;
@@ -383,6 +384,19 @@ struct pn533 {
        u8 tgt_mode;
 
        u32 device_type;
+
+       struct list_head cmd_queue;
+       u8 cmd_pending;
+};
+
+struct pn533_cmd {
+       struct list_head queue;
+       struct pn533_frame *out_frame;
+       struct pn533_frame *in_frame;
+       int in_frame_len;
+       pn533_cmd_complete_t cmd_complete;
+       void *arg;
+       gfp_t flags;
 };
 
 struct pn533_frame {
@@ -487,7 +501,7 @@ static bool pn533_rx_frame_is_cmd_response(struct pn533_frame *frame, u8 cmd)
 
 static void pn533_wq_cmd_complete(struct work_struct *work)
 {
-       struct pn533 *dev = container_of(work, struct pn533, cmd_work);
+       struct pn533 *dev = container_of(work, struct pn533, cmd_complete_work);
        struct pn533_frame *in_frame;
        int rc;
 
@@ -502,7 +516,7 @@ static void pn533_wq_cmd_complete(struct work_struct *work)
                                        PN533_FRAME_CMD_PARAMS_LEN(in_frame));
 
        if (rc != -EINPROGRESS)
-               mutex_unlock(&dev->cmd_lock);
+               queue_work(dev->wq, &dev->cmd_work);
 }
 
 static void pn533_recv_response(struct urb *urb)
@@ -550,7 +564,7 @@ static void pn533_recv_response(struct urb *urb)
        dev->wq_in_frame = in_frame;
 
 sched_wq:
-       queue_work(dev->wq, &dev->cmd_work);
+       queue_work(dev->wq, &dev->cmd_complete_work);
 }
 
 static int pn533_submit_urb_for_response(struct pn533 *dev, gfp_t flags)
@@ -606,7 +620,7 @@ static void pn533_recv_ack(struct urb *urb)
 
 sched_wq:
        dev->wq_in_frame = NULL;
-       queue_work(dev->wq, &dev->cmd_work);
+       queue_work(dev->wq, &dev->cmd_complete_work);
 }
 
 static int pn533_submit_urb_for_ack(struct pn533 *dev, gfp_t flags)
@@ -669,6 +683,31 @@ error:
        return rc;
 }
 
+static void pn533_wq_cmd(struct work_struct *work)
+{
+       struct pn533 *dev = container_of(work, struct pn533, cmd_work);
+       struct pn533_cmd *cmd;
+
+       mutex_lock(&dev->cmd_lock);
+
+       if (list_empty(&dev->cmd_queue)) {
+               dev->cmd_pending = 0;
+               mutex_unlock(&dev->cmd_lock);
+               return;
+       }
+
+       cmd = list_first_entry(&dev->cmd_queue, struct pn533_cmd, queue);
+
+       mutex_unlock(&dev->cmd_lock);
+
+       __pn533_send_cmd_frame_async(dev, cmd->out_frame, cmd->in_frame,
+                                    cmd->in_frame_len, cmd->cmd_complete,
+                                    cmd->arg, cmd->flags);
+
+       list_del(&cmd->queue);
+       kfree(cmd);
+}
+
 static int pn533_send_cmd_frame_async(struct pn533 *dev,
                                        struct pn533_frame *out_frame,
                                        struct pn533_frame *in_frame,
@@ -676,22 +715,44 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
                                        pn533_cmd_complete_t cmd_complete,
                                        void *arg, gfp_t flags)
 {
+       struct pn533_cmd *cmd;
        int rc;
 
        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-       if (!mutex_trylock(&dev->cmd_lock))
-               return -EBUSY;
+       mutex_lock(&dev->cmd_lock);
 
-       rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
-                                       in_frame_len, cmd_complete, arg, flags);
-       if (rc)
-               goto error;
+       if (!dev->cmd_pending) {
+               rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
+                                                 in_frame_len, cmd_complete,
+                                                 arg, flags);
+               if (!rc)
+                       dev->cmd_pending = 1;
+
+               mutex_unlock(&dev->cmd_lock);
+
+               return rc;
+       }
+
+       nfc_dev_dbg(&dev->interface->dev, "%s Queueing command", __func__);
+
+       cmd = kzalloc(sizeof(struct pn533_cmd), flags);
+       if (!cmd)
+               return -ENOMEM;
+
+       INIT_LIST_HEAD(&cmd->queue);
+       cmd->out_frame = out_frame;
+       cmd->in_frame = in_frame;
+       cmd->in_frame_len = in_frame_len;
+       cmd->cmd_complete = cmd_complete;
+       cmd->arg = arg;
+       cmd->flags = flags;
+
+       list_add_tail(&cmd->queue, &dev->cmd_queue);
 
-       return 0;
-error:
        mutex_unlock(&dev->cmd_lock);
-       return rc;
+
+       return 0;
 }
 
 struct pn533_sync_cmd_response {
@@ -1305,8 +1366,6 @@ static void pn533_listen_mode_timer(unsigned long data)
 
        dev->cancel_listen = 1;
 
-       mutex_unlock(&dev->cmd_lock);
-
        pn533_poll_next_mod(dev);
 
        queue_work(dev->wq, &dev->poll_work);
@@ -2131,7 +2190,7 @@ error_cmd:
 
        kfree(arg);
 
-       mutex_unlock(&dev->cmd_lock);
+       queue_work(dev->wq, &dev->cmd_work);
 }
 
 static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
@@ -2330,7 +2389,8 @@ static int pn533_probe(struct usb_interface *interface,
                        NULL, 0,
                        pn533_send_complete, dev);
 
-       INIT_WORK(&dev->cmd_work, pn533_wq_cmd_complete);
+       INIT_WORK(&dev->cmd_work, pn533_wq_cmd);
+       INIT_WORK(&dev->cmd_complete_work, pn533_wq_cmd_complete);
        INIT_WORK(&dev->mi_work, pn533_wq_mi_recv);
        INIT_WORK(&dev->tg_work, pn533_wq_tg_get_data);
        INIT_WORK(&dev->poll_work, pn533_wq_poll);
@@ -2346,6 +2406,8 @@ static int pn533_probe(struct usb_interface *interface,
 
        skb_queue_head_init(&dev->resp_q);
 
+       INIT_LIST_HEAD(&dev->cmd_queue);
+
        usb_set_intfdata(interface, dev);
 
        pn533_tx_frame_init(dev->out_frame, PN533_CMD_GET_FIRMWARE_VERSION);
@@ -2417,6 +2479,7 @@ error:
 static void pn533_disconnect(struct usb_interface *interface)
 {
        struct pn533 *dev;
+       struct pn533_cmd *cmd, *n;
 
        dev = usb_get_intfdata(interface);
        usb_set_intfdata(interface, NULL);
@@ -2433,6 +2496,11 @@ static void pn533_disconnect(struct usb_interface *interface)
 
        del_timer(&dev->listen_timer);
 
+       list_for_each_entry_safe(cmd, n, &dev->cmd_queue, queue) {
+               list_del(&cmd->queue);
+               kfree(cmd);
+       }
+
        kfree(dev->in_frame);
        usb_free_urb(dev->in_urb);
        kfree(dev->out_frame);