NFC: Requeue lost LLCP frames

When receiving an I or RR frame telling us that some of the pending queues
were not received, we should requeue them before the currently pending ones.

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

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index b694313c2be5d5a6bf9bbda5bfa0f5fc6832e0ab..0fc60da6a0a1d02d764a75227a9a2cbc751ac387 100644 (file)
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -411,28 +411,6 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
                                     local->remote_gb_len - 3);
 }
 
-static void nfc_llcp_tx_work(struct work_struct *work)
-{
-       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                   tx_work);
-       struct sk_buff *skb;
-
-       skb = skb_dequeue(&local->tx_queue);
-       if (skb != NULL) {
-               pr_debug("Sending pending skb\n");
-               print_hex_dump(KERN_DEBUG, "LLCP Tx: ", DUMP_PREFIX_OFFSET,
-                              16, 1, skb->data, skb->len, true);
-
-               nfc_data_exchange(local->dev, local->target_idx,
-                                 skb, nfc_llcp_recv, local);
-       } else {
-               nfc_llcp_send_symm(local->dev);
-       }
-
-       mod_timer(&local->link_timer,
-                 jiffies + msecs_to_jiffies(local->remote_lto));
-}
-
 static u8 nfc_llcp_dsap(struct sk_buff *pdu)
 {
        return (pdu->data[0] & 0xfc) >> 2;
@@ -465,6 +443,45 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
        sock->recv_ack_n = (sock->recv_n - 1) % 16;
 }
 
+static void nfc_llcp_tx_work(struct work_struct *work)
+{
+       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                   tx_work);
+       struct sk_buff *skb;
+       struct sock *sk;
+       struct nfc_llcp_sock *llcp_sock;
+
+       skb = skb_dequeue(&local->tx_queue);
+       if (skb != NULL) {
+               sk = skb->sk;
+               llcp_sock = nfc_llcp_sock(sk);
+               if (llcp_sock != NULL) {
+                       int ret;
+
+                       pr_debug("Sending pending skb\n");
+                       print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
+                                      DUMP_PREFIX_OFFSET, 16, 1,
+                                      skb->data, skb->len, true);
+
+                       ret = nfc_data_exchange(local->dev, local->target_idx,
+                                               skb, nfc_llcp_recv, local);
+
+                       if (!ret && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+                               skb = skb_get(skb);
+                               skb_queue_tail(&llcp_sock->tx_pending_queue,
+                                              skb);
+                       }
+               } else {
+                       nfc_llcp_send_symm(local->dev);
+               }
+       } else {
+               nfc_llcp_send_symm(local->dev);
+       }
+
+       mod_timer(&local->link_timer,
+                 jiffies + msecs_to_jiffies(2 * local->remote_lto));
+}
+
 static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
                                                          u8 ssap)
 {
@@ -705,7 +722,7 @@ int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
        /* Try to queue some I frames for transmission */
        while (sock->remote_ready &&
               skb_queue_len(&sock->tx_pending_queue) < sock->rw) {
-               struct sk_buff *pdu, *pending_pdu;
+               struct sk_buff *pdu;
 
                pdu = skb_dequeue(&sock->tx_queue);
                if (pdu == NULL)
@@ -714,10 +731,7 @@ int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
                /* Update N(S)/N(R) */
                nfc_llcp_set_nrns(sock, pdu);
 
-               pending_pdu = skb_clone(pdu, GFP_KERNEL);
-
                skb_queue_tail(&local->tx_queue, pdu);
-               skb_queue_tail(&sock->tx_pending_queue, pending_pdu);
                nr_frames++;
        }
 
@@ -774,11 +788,21 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 
                llcp_sock->send_ack_n = nr;
 
-               skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp)
-                       if (nfc_llcp_ns(s) <= nr) {
-                               skb_unlink(s, &llcp_sock->tx_pending_queue);
-                               kfree_skb(s);
-                       }
+               /* Remove and free all skbs until ns == nr */
+               skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+                       skb_unlink(s, &llcp_sock->tx_pending_queue);
+                       kfree_skb(s);
+
+                       if (nfc_llcp_ns(s) == nr)
+                               break;
+               }
+
+               /* Re-queue the remaining skbs for transmission */
+               skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
+                                           s, tmp) {
+                       skb_unlink(s, &llcp_sock->tx_pending_queue);
+                       skb_queue_head(&local->tx_queue, s);
+               }
        }
 
        if (ptype == LLCP_PDU_RR)