NFC: digital: Fix target DEP_REQ I-PDU handling after ATN PDU

[linux-2.6-block.git] / net / nfc / digital_dep.c

diff --git a/net/nfc/digital_dep.c b/net/nfc/digital_dep.c
index f72be7433df35fc1acfa3105a151b40876c0d47f..1778c23751d43b4dcce857c1636a0d535eb29cb7 100644 (file)
--- a/net/nfc/digital_dep.c
+++ b/net/nfc/digital_dep.c
@@ -190,8 +190,6 @@ digital_send_dep_data_prep(struct nfc_digital_dev *ddev, struct sk_buff *skb,
                        return ERR_PTR(-ENOMEM);
                }
 
-               skb_reserve(new_skb, ddev->tx_headroom + NFC_HEADER_SIZE +
-                                       DIGITAL_NFC_DEP_REQ_RES_HEADROOM);
                memcpy(skb_put(new_skb, ddev->remote_payload_max), skb->data,
                       ddev->remote_payload_max);
                skb_pull(skb, ddev->remote_payload_max);
@@ -524,8 +522,7 @@ static int digital_in_send_ack(struct nfc_digital_dev *ddev,
 
        ddev->skb_add_crc(skb);
 
-       ddev->saved_skb = skb_get(skb);
-       ddev->saved_skb_len = skb->len;
+       ddev->saved_skb = pskb_copy(skb, GFP_KERNEL);
 
        rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
                                 data_exch);
@@ -627,16 +624,10 @@ static int digital_in_send_rtox(struct nfc_digital_dev *ddev,
 
        ddev->skb_add_crc(skb);
 
-       ddev->saved_skb = skb_get(skb);
-       ddev->saved_skb_len = skb->len;
-
        rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
                                 data_exch);
-       if (rc) {
+       if (rc)
                kfree_skb(skb);
-               kfree_skb(ddev->saved_skb);
-               ddev->saved_skb = NULL;
-       }
 
        return rc;
 }
@@ -644,11 +635,19 @@ static int digital_in_send_rtox(struct nfc_digital_dev *ddev,
 static int digital_in_send_saved_skb(struct nfc_digital_dev *ddev,
                                     struct digital_data_exch *data_exch)
 {
+       int rc;
+
+       if (!ddev->saved_skb)
+               return -EINVAL;
+
        skb_get(ddev->saved_skb);
-       skb_push(ddev->saved_skb, ddev->saved_skb_len);
 
-       return digital_in_send_cmd(ddev, ddev->saved_skb, 1500,
-                                  digital_in_recv_dep_res, data_exch);
+       rc = digital_in_send_cmd(ddev, ddev->saved_skb, 1500,
+                                digital_in_recv_dep_res, data_exch);
+       if (rc)
+               kfree_skb(ddev->saved_skb);
+
+       return rc;
 }
 
 static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
@@ -664,7 +663,7 @@ static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
                rc = PTR_ERR(resp);
                resp = NULL;
 
-               if (((rc != -ETIMEDOUT) || ddev->nack_count) &&
+               if ((rc == -EIO || (rc == -ETIMEDOUT && ddev->nack_count)) &&
                    (ddev->nack_count++ < DIGITAL_NFC_DEP_N_RETRY_NACK)) {
                        ddev->atn_count = 0;
 
@@ -812,17 +811,12 @@ static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
        case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
                if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) { /* ATN */
                        rc = digital_in_send_saved_skb(ddev, data_exch);
-                       if (rc) {
-                               kfree_skb(ddev->saved_skb);
+                       if (rc)
                                goto error;
-                       }
 
                        return;
                }
 
-               kfree_skb(ddev->saved_skb);
-               ddev->saved_skb = NULL;
-
                rc = digital_in_send_rtox(ddev, data_exch, resp->data[0]);
                if (rc)
                        goto error;
@@ -876,8 +870,7 @@ int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
 
        ddev->skb_add_crc(tmp_skb);
 
-       ddev->saved_skb = skb_get(tmp_skb);
-       ddev->saved_skb_len = tmp_skb->len;
+       ddev->saved_skb = pskb_copy(tmp_skb, GFP_KERNEL);
 
        rc = digital_in_send_cmd(ddev, tmp_skb, 1500, digital_in_recv_dep_res,
                                 data_exch);
@@ -956,8 +949,7 @@ static int digital_tg_send_ack(struct nfc_digital_dev *ddev,
 
        ddev->skb_add_crc(skb);
 
-       ddev->saved_skb = skb_get(skb);
-       ddev->saved_skb_len = skb->len;
+       ddev->saved_skb = pskb_copy(skb, GFP_KERNEL);
 
        rc = digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
                                 data_exch);
@@ -1009,11 +1001,19 @@ static int digital_tg_send_atn(struct nfc_digital_dev *ddev)
 
 static int digital_tg_send_saved_skb(struct nfc_digital_dev *ddev)
 {
+       int rc;
+
+       if (!ddev->saved_skb)
+               return -EINVAL;
+
        skb_get(ddev->saved_skb);
-       skb_push(ddev->saved_skb, ddev->saved_skb_len);
 
-       return digital_tg_send_cmd(ddev, ddev->saved_skb, 1500,
-                                  digital_tg_recv_dep_req, NULL);
+       rc = digital_tg_send_cmd(ddev, ddev->saved_skb, 1500,
+                                digital_tg_recv_dep_req, NULL);
+       if (rc)
+               kfree_skb(ddev->saved_skb);
+
+       return rc;
 }
 
 static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
@@ -1086,22 +1086,38 @@ static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
        case DIGITAL_NFC_DEP_PFB_I_PDU:
                pr_debug("DIGITAL_NFC_DEP_PFB_I_PDU\n");
 
-               if ((ddev->atn_count && (DIGITAL_NFC_DEP_PFB_PNI(pfb - 1) !=
-                                               ddev->curr_nfc_dep_pni)) ||
-                   (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni)) {
-                       PROTOCOL_ERR("14.12.3.4");
-                       rc = -EIO;
-                       goto exit;
-               }
-
                if (ddev->atn_count) {
+                       /* The target has received (and replied to) at least one
+                        * ATN DEP_REQ.
+                        */
                        ddev->atn_count = 0;
 
-                       rc = digital_tg_send_saved_skb(ddev);
-                       if (rc)
-                               goto exit;
+                       /* pni of resp PDU equal to the target current pni - 1
+                        * means resp is the previous DEP_REQ PDU received from
+                        * the initiator so the target replies with saved_skb
+                        * which is the previous DEP_RES saved in
+                        * digital_tg_send_dep_res().
+                        */
+                       if (DIGITAL_NFC_DEP_PFB_PNI(pfb) ==
+                         DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni - 1)) {
+                               rc = digital_tg_send_saved_skb(ddev);
+                               if (rc)
+                                       goto exit;
 
-                       return;
+                               goto free_resp;
+                       }
+
+                       /* atn_count > 0 and PDU pni != curr_nfc_dep_pni - 1
+                        * means the target probably did not received the last
+                        * DEP_REQ PDU sent by the initiator. The target
+                        * fallbacks to normal processing then.
+                        */
+               }
+
+               if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
+                       PROTOCOL_ERR("14.12.3.4");
+                       rc = -EIO;
+                       goto exit;
                }
 
                kfree_skb(ddev->saved_skb);
@@ -1163,10 +1179,8 @@ static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
                        ddev->atn_count = 0;
 
                        rc = digital_tg_send_saved_skb(ddev);
-                       if (rc) {
-                               kfree_skb(ddev->saved_skb);
+                       if (rc)
                                goto exit;
-                       }
                }
 
                return;
@@ -1199,6 +1213,11 @@ exit:
 
        if (rc)
                kfree_skb(resp);
+
+       return;
+
+free_resp:
+       dev_kfree_skb(resp);
 }
 
 int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb)
@@ -1235,8 +1254,7 @@ int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb)
 
        ddev->skb_add_crc(tmp_skb);
 
-       ddev->saved_skb = skb_get(tmp_skb);
-       ddev->saved_skb_len = tmp_skb->len;
+       ddev->saved_skb = pskb_copy(tmp_skb, GFP_KERNEL);
 
        rc = digital_tg_send_cmd(ddev, tmp_skb, 1500, digital_tg_recv_dep_req,
                                 NULL);