NFC: Queue pn533 commands

Instead of returning EBUSY when getting a command while another one is
running, we queue them. Upon completion of the pending command, the next
one is processed.
Besides the fact that it simplifies the pn533 locking scheme, it also
comes with the nice side effect of fixing the following warning:

[   82.274297] =====================================
[   82.274297] [ BUG: bad unlock balance detected! ]
[   82.274298] 3.5.0-rc1+ #1 Not tainted
[   82.274299] -------------------------------------
[   82.274300] kworker/u:1/16 is trying to release lock (&dev->cmd_lock) at:
[   82.274305] [<ffffffff8144f246>] mutex_unlock+0x9/0xb
[   82.274305] but there are no more locks to release!
[   82.274306]
[   82.274306] other info that might help us debug this:
[   82.274306] 2 locks held by kworker/u:1/16:
[   82.274311]  #0:  (pn533){.+.+..}, at: [<ffffffff8103a67d>]
+process_one_work+0x145/0x2e2
[   82.274314]  #1:  ((&dev->cmd_work)){+.+...}, at: [<ffffffff8103a67d>]
+process_one_work+0x145/0x2e2
[   82.274314]
[   82.274314] stack backtrace:
[   82.274315] Pid: 16, comm: kworker/u:1 Not tainted 3.5.0-rc1+ #1
[   82.274315] Call Trace:
[   82.274317]  [<ffffffff8144f246>] ? mutex_unlock+0x9/0xb
[   82.274321]  [<ffffffff81059841>] print_unlock_inbalance_bug+0xda/0xe4
[   82.274323]  [<ffffffff8105c74c>] lock_release_non_nested+0xb2/0x232
[   82.274325]  [<ffffffff8105a61e>] ? mark_held_locks+0x6d/0x95
[   82.274326]  [<ffffffff8144f246>] ? mutex_unlock+0x9/0xb
[   82.274328]  [<ffffffff81451105>] ? _raw_spin_unlock_irqrestore+0x40/0x5c
[   82.274329]  [<ffffffff8144f246>] ? mutex_unlock+0x9/0xb
[   82.274330]  [<ffffffff8105ca42>] lock_release+0x176/0x1ac
[   82.274333]  [<ffffffff8123de14>] ? pn533_send_complete+0xa8/0xa8
[   82.274334]  [<ffffffff8144f1d6>] __mutex_unlock_slowpath+0xb0/0x117
[   82.274336]  [<ffffffff8144f246>] mutex_unlock+0x9/0xb
[   82.274337]  [<ffffffff8123de65>] pn533_wq_cmd_complete+0x51/0x55
[   82.274338]  [<ffffffff8103a6db>] process_one_work+0x1a3/0x2e2
[   82.274340]  [<ffffffff8103a67d>] ? process_one_work+0x145/0x2e2
[   82.274341]  [<ffffffff8103b119>] worker_thread+0xcf/0x153
[   82.274343]  [<ffffffff8103b04a>] ? manage_workers.isra.22+0x16b/0x16b
[   82.274344]  [<ffffffff8103b04a>] ? manage_workers.isra.22+0x16b/0x16b
[   82.274346]  [<ffffffff8103eb11>] kthread+0x95/0x9d
[   82.274348]  [<ffffffff81452ef4>] kernel_thread_helper+0x4/0x10
[   82.274351]  [<ffffffff81046561>] ? finish_task_switch+0x45/0xc3
[   82.274352]  [<ffffffff814514f0>] ? retint_restore_args+0x13/0x13
[   82.274353]  [<ffffffff8103ea7c>] ? __init_kthread_worker+0x55/0x55
[   82.274354]  [<ffffffff81452ef0>] ? gs_change+0x13/0x13

Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

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))
+	mutex_lock(&dev->cmd_lock);
-		return -EBUSY;
 
-	rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
+	if (!dev->cmd_pending) {
-					in_frame_len, cmd_complete, arg, flags);
+		rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
-	if (rc)
+						  in_frame_len, cmd_complete,
-		goto error;
+						  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);
+
+	mutex_unlock(&dev->cmd_lock);
 
 	return 0;
-error:
-	mutex_unlock(&dev->cmd_lock);
-	return rc;
 }
 
 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);