3 files changed, 57 insertions, 0 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index c943edb07b72..b75a9b3f9e89 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -174,6 +174,8 @@ struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
         sdreq->uri = sdreq->tlv + 3;
         memcpy(sdreq->uri, uri, uri_len);
 
+        sdreq->time = jiffies;
+
         INIT_HLIST_NODE(&sdreq->node);
 
         return sdreq;
@@ -571,6 +573,10 @@ int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
 
         mutex_lock(&local->sdreq_lock);
 
+        if (hlist_empty(&local->pending_sdreqs))
+                mod_timer(&local->sdreq_timer,
+                          jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
         hlist_for_each_entry_safe(sdreq, n, tlv_list, node) {
                 pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri);
 
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 99e911060422..3361170cb262 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -156,6 +156,8 @@ static void local_release(struct kref *ref)
         cancel_work_sync(&local->rx_work);
         cancel_work_sync(&local->timeout_work);
         kfree_skb(local->rx_pending);
+        del_timer_sync(&local->sdreq_timer);
+        cancel_work_sync(&local->sdreq_timeout_work);
         nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
         kfree(local);
 }
@@ -224,6 +226,47 @@ static void nfc_llcp_symm_timer(unsigned long data)
         schedule_work(&local->timeout_work);
 }
 
+static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
+{
+        unsigned long time;
+        HLIST_HEAD(nl_sdres_list);
+        struct hlist_node *n;
+        struct nfc_llcp_sdp_tlv *sdp;
+        struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                    sdreq_timeout_work);
+
+        mutex_lock(&local->sdreq_lock);
+
+        time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
+
+        hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
+                if (time_after(sdp->time, time))
+                        continue;
+
+                sdp->sap = LLCP_SDP_UNBOUND;
+
+                hlist_del(&sdp->node);
+
+                hlist_add_head(&sdp->node, &nl_sdres_list);
+        }
+
+        if (!hlist_empty(&local->pending_sdreqs))
+                mod_timer(&local->sdreq_timer,
+                          jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
+        mutex_unlock(&local->sdreq_lock);
+
+        if (!hlist_empty(&nl_sdres_list))
+                nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+}
+
+static void nfc_llcp_sdreq_timer(unsigned long data)
+{
+        struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+        schedule_work(&local->sdreq_timeout_work);
+}
+
 struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
 {
         struct nfc_llcp_local *local, *n;
@@ -1457,6 +1500,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 
         mutex_init(&local->sdreq_lock);
         INIT_HLIST_HEAD(&local->pending_sdreqs);
+        init_timer(&local->sdreq_timer);
+        local->sdreq_timer.data = (unsigned long) local;
+        local->sdreq_timer.function = nfc_llcp_sdreq_timer;
+        INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
 
         list_add(&local->list, &llcp_devices);
 
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index ca8c6d94ab85..7e87a66b02ec 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -54,6 +54,8 @@ struct nfc_llcp_sdp_tlv {
         u8 tid;
         u8 sap;
 
+        unsigned long time;
+
         struct hlist_node node;
 };
 
@@ -99,6 +101,8 @@ struct nfc_llcp_local {
 
         struct mutex sdreq_lock;
         struct hlist_head pending_sdreqs;
+        struct timer_list sdreq_timer;
+        struct work_struct sdreq_timeout_work;
         u8 sdreq_next_tid;
 
         /* sockets array */