NFC: pn533 Rx chaining support

When buffers on the receiption path exceed 262 bytes, the pn533 uses
a chaining mechanism where the initiator has to send NULL data frames
to fetch the remaining frames.
We do that from a workqueue context while holding the cmd lock. Once the
MI bit is gone, we aggregate the queued received skbs.

Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index f2ee06f059f9b6cc9ab33a00d20fb0dd143ec90c..e6ec16d92e65d2ee362119f3f015f6d9322d00d2 100644 (file)
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -266,8 +266,11 @@ struct pn533 {
        int in_maxlen;
        struct pn533_frame *in_frame;
 
+       struct sk_buff_head resp_q;
+
        struct workqueue_struct *wq;
        struct work_struct cmd_work;
+       struct work_struct mi_work;
        struct pn533_frame *wq_in_frame;
        int wq_in_error;
 
@@ -1256,6 +1259,8 @@ static void pn533_deactivate_target(struct nfc_dev *nfc_dev, u32 target_idx)
 
        dev->tgt_active_prot = 0;
 
+       skb_queue_purge(&dev->resp_q);
+
        pn533_tx_frame_init(dev->out_frame, PN533_CMD_IN_RELEASE);
 
        tg = 1;
@@ -1454,11 +1459,49 @@ struct pn533_data_exchange_arg {
        void *cb_context;
 };
 
+static struct sk_buff *pn533_build_response(struct pn533 *dev)
+{
+       struct sk_buff *skb, *tmp, *t;
+       unsigned int skb_len = 0, tmp_len = 0;
+
+       nfc_dev_dbg(&dev->interface->dev, "%s\n", __func__);
+
+       if (skb_queue_empty(&dev->resp_q))
+               return NULL;
+
+       if (skb_queue_len(&dev->resp_q) == 1) {
+               skb = skb_dequeue(&dev->resp_q);
+               goto out;
+       }
+
+       skb_queue_walk_safe(&dev->resp_q, tmp, t)
+               skb_len += tmp->len;
+
+       nfc_dev_dbg(&dev->interface->dev, "%s total length %d\n",
+                   __func__, skb_len);
+
+       skb = alloc_skb(skb_len, GFP_KERNEL);
+       if (skb == NULL)
+               goto out;
+
+       skb_put(skb, skb_len);
+
+       skb_queue_walk_safe(&dev->resp_q, tmp, t) {
+               memcpy(skb->data + tmp_len, tmp->data, tmp->len);
+               tmp_len += tmp->len;
+       }
+
+out:
+       skb_queue_purge(&dev->resp_q);
+
+       return skb;
+}
+
 static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
                                                u8 *params, int params_len)
 {
        struct pn533_data_exchange_arg *arg = _arg;
-       struct sk_buff *skb_resp = arg->skb_resp;
+       struct sk_buff *skb = NULL, *skb_resp = arg->skb_resp;
        struct pn533_frame *in_frame = (struct pn533_frame *) skb_resp->data;
        int err = 0;
        u8 status;
@@ -1466,15 +1509,13 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
 
        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-       dev_kfree_skb_irq(arg->skb_out);
+       dev_kfree_skb(arg->skb_out);
 
        if (params_len < 0) { /* error */
                err = params_len;
                goto error;
        }
 
-       skb_put(skb_resp, PN533_FRAME_SIZE(in_frame));
-
        status = params[0];
 
        cmd_ret = status & PN533_CMD_RET_MASK;
@@ -1485,25 +1526,27 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
                goto error;
        }
 
+       skb_put(skb_resp, PN533_FRAME_SIZE(in_frame));
+       skb_pull(skb_resp, PN533_CMD_DATAEXCH_HEAD_LEN);
+       skb_trim(skb_resp, skb_resp->len - PN533_FRAME_TAIL_SIZE);
+       skb_queue_tail(&dev->resp_q, skb_resp);
+
        if (status & PN533_CMD_MI_MASK) {
-               /* TODO: Implement support to multi-part data exchange */
-               nfc_dev_err(&dev->interface->dev, "Multi-part message not yet"
-                                                               " supported");
-               /* Prevent the other messages from controller */
-               pn533_send_ack(dev, GFP_ATOMIC);
-               err = -ENOSYS;
-               goto error;
+               queue_work(dev->wq, &dev->mi_work);
+               return -EINPROGRESS;
        }
 
-       skb_pull(skb_resp, PN533_CMD_DATAEXCH_HEAD_LEN);
-       skb_trim(skb_resp, skb_resp->len - PN533_FRAME_TAIL_SIZE);
+       skb = pn533_build_response(dev);
+       if (skb == NULL)
+               goto error;
 
-       arg->cb(arg->cb_context, skb_resp, 0);
+       arg->cb(arg->cb_context, skb, 0);
        kfree(arg);
        return 0;
 
 error:
-       dev_kfree_skb_irq(skb_resp);
+       skb_queue_purge(&dev->resp_q);
+       dev_kfree_skb(skb_resp);
        arg->cb(arg->cb_context, NULL, err);
        kfree(arg);
        return 0;
@@ -1578,6 +1621,68 @@ error:
        return rc;
 }
 
+static void pn533_wq_mi_recv(struct work_struct *work)
+{
+       struct pn533 *dev = container_of(work, struct pn533, mi_work);
+       struct sk_buff *skb_cmd;
+       struct pn533_data_exchange_arg *arg = dev->cmd_complete_arg;
+       struct pn533_frame *out_frame, *in_frame;
+       struct sk_buff *skb_resp;
+       int skb_resp_len;
+       int rc;
+
+       nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+       /* This is a zero payload size skb */
+       skb_cmd = alloc_skb(PN533_CMD_DATAEXCH_HEAD_LEN + PN533_FRAME_TAIL_SIZE,
+                           GFP_KERNEL);
+       if (skb_cmd == NULL)
+               goto error_cmd;
+
+       skb_reserve(skb_cmd, PN533_CMD_DATAEXCH_HEAD_LEN);
+
+       rc = pn533_data_exchange_tx_frame(dev, skb_cmd);
+       if (rc)
+               goto error_frame;
+
+       skb_resp_len = PN533_CMD_DATAEXCH_HEAD_LEN +
+                       PN533_CMD_DATAEXCH_DATA_MAXLEN +
+                       PN533_FRAME_TAIL_SIZE;
+       skb_resp = alloc_skb(skb_resp_len, GFP_KERNEL);
+       if (!skb_resp) {
+               rc = -ENOMEM;
+               goto error_frame;
+       }
+
+       in_frame = (struct pn533_frame *) skb_resp->data;
+       out_frame = (struct pn533_frame *) skb_cmd->data;
+
+       arg->skb_resp = skb_resp;
+       arg->skb_out = skb_cmd;
+
+       rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
+                                         skb_resp_len,
+                                         pn533_data_exchange_complete,
+                                         dev->cmd_complete_arg, GFP_KERNEL);
+       if (!rc)
+               return;
+
+       nfc_dev_err(&dev->interface->dev, "Error %d when trying to"
+                                               " perform data_exchange", rc);
+
+       kfree_skb(skb_resp);
+
+error_frame:
+       kfree_skb(skb_cmd);
+
+error_cmd:
+       pn533_send_ack(dev, GFP_KERNEL);
+
+       kfree(arg);
+
+       up(&dev->cmd_lock);
+}
+
 static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
                                                                u8 cfgdata_len)
 {
@@ -1676,10 +1781,15 @@ static int pn533_probe(struct usb_interface *interface,
                        pn533_send_complete, dev);
 
        INIT_WORK(&dev->cmd_work, pn533_wq_cmd_complete);
-       dev->wq = create_singlethread_workqueue("pn533");
+       INIT_WORK(&dev->mi_work, pn533_wq_mi_recv);
+       dev->wq = alloc_workqueue("pn533",
+                                 WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+                                 1);
        if (dev->wq == NULL)
                goto error;
 
+       skb_queue_head_init(&dev->resp_q);
+
        usb_set_intfdata(interface, dev);
 
        pn533_tx_frame_init(dev->out_frame, PN533_CMD_GET_FIRMWARE_VERSION);
@@ -1756,6 +1866,8 @@ static void pn533_disconnect(struct usb_interface *interface)
 
        destroy_workqueue(dev->wq);
 
+       skb_queue_purge(&dev->resp_q);
+
        kfree(dev->in_frame);
        usb_free_urb(dev->in_urb);
        kfree(dev->out_frame);