Merge git://git.kernel.org/pub/scm/linux/kernel/git/davem/net

[firefly-linux-kernel-4.4.55.git] / net / nfc / llcp / commands.c

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 5f61830a0e8650117b9d0ac66a536f3cd1c1b283..b75a9b3f9e8933ab0a2def3c2fde7248c14ed967 100644 (file)
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -117,6 +117,88 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
        return tlv;
 }
 
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
+{
+       struct nfc_llcp_sdp_tlv *sdres;
+       u8 value[2];
+
+       sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+       if (sdres == NULL)
+               return NULL;
+
+       value[0] = tid;
+       value[1] = sap;
+
+       sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
+                                       &sdres->tlv_len);
+       if (sdres->tlv == NULL) {
+               kfree(sdres);
+               return NULL;
+       }
+
+       sdres->tid = tid;
+       sdres->sap = sap;
+
+       INIT_HLIST_NODE(&sdres->node);
+
+       return sdres;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+                                                 size_t uri_len)
+{
+       struct nfc_llcp_sdp_tlv *sdreq;
+
+       pr_debug("uri: %s, len: %zu\n", uri, uri_len);
+
+       sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+       if (sdreq == NULL)
+               return NULL;
+
+       sdreq->tlv_len = uri_len + 3;
+
+       if (uri[uri_len - 1] == 0)
+               sdreq->tlv_len--;
+
+       sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
+       if (sdreq->tlv == NULL) {
+               kfree(sdreq);
+               return NULL;
+       }
+
+       sdreq->tlv[0] = LLCP_TLV_SDREQ;
+       sdreq->tlv[1] = sdreq->tlv_len - 2;
+       sdreq->tlv[2] = tid;
+
+       sdreq->tid = tid;
+       sdreq->uri = sdreq->tlv + 3;
+       memcpy(sdreq->uri, uri, uri_len);
+
+       sdreq->time = jiffies;
+
+       INIT_HLIST_NODE(&sdreq->node);
+
+       return sdreq;
+}
+
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
+{
+       kfree(sdp->tlv);
+       kfree(sdp);
+}
+
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
+{
+       struct nfc_llcp_sdp_tlv *sdp;
+       struct hlist_node *n;
+
+       hlist_for_each_entry_safe(sdp, n, head, node) {
+               hlist_del(&sdp->node);
+
+               nfc_llcp_free_sdp_tlv(sdp);
+       }
+}
+
 int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
                          u8 *tlv_array, u16 tlv_array_len)
 {
@@ -425,48 +507,90 @@ error_tlv:
        return err;
 }
 
-int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap)
+static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,
+                                            size_t tlv_length)
 {
        struct sk_buff *skb;
        struct nfc_dev *dev;
-       u8 *sdres_tlv = NULL, sdres_tlv_length, sdres[2];
        u16 size = 0;
 
-       pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid, sap);
-
        if (local == NULL)
-               return -ENODEV;
+               return ERR_PTR(-ENODEV);
 
        dev = local->dev;
        if (dev == NULL)
-               return -ENODEV;
-
-       sdres[0] = tid;
-       sdres[1] = sap;
-       sdres_tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, sdres, 0,
-                                      &sdres_tlv_length);
-       if (sdres_tlv == NULL)
-               return -ENOMEM;
+               return ERR_PTR(-ENODEV);
 
        size += LLCP_HEADER_SIZE;
        size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
-       size += sdres_tlv_length;
+       size += tlv_length;
 
        skb = alloc_skb(size, GFP_KERNEL);
-       if (skb == NULL) {
-               kfree(sdres_tlv);
-               return -ENOMEM;
-       }
+       if (skb == NULL)
+               return ERR_PTR(-ENOMEM);
 
        skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
 
        skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
 
-       memcpy(skb_put(skb, sdres_tlv_length), sdres_tlv, sdres_tlv_length);
+       return skb;
+}
+
+int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
+                           struct hlist_head *tlv_list, size_t tlvs_len)
+{
+       struct nfc_llcp_sdp_tlv *sdp;
+       struct hlist_node *n;
+       struct sk_buff *skb;
+
+       skb = nfc_llcp_allocate_snl(local, tlvs_len);
+       if (IS_ERR(skb))
+               return PTR_ERR(skb);
+
+       hlist_for_each_entry_safe(sdp, n, tlv_list, node) {
+               memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len);
+
+               hlist_del(&sdp->node);
+
+               nfc_llcp_free_sdp_tlv(sdp);
+       }
 
        skb_queue_tail(&local->tx_queue, skb);
 
-       kfree(sdres_tlv);
+       return 0;
+}
+
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+                           struct hlist_head *tlv_list, size_t tlvs_len)
+{
+       struct nfc_llcp_sdp_tlv *sdreq;
+       struct hlist_node *n;
+       struct sk_buff *skb;
+
+       skb = nfc_llcp_allocate_snl(local, tlvs_len);
+       if (IS_ERR(skb))
+               return PTR_ERR(skb);
+
+       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);
+
+               memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv,
+                      sdreq->tlv_len);
+
+               hlist_del(&sdreq->node);
+
+               hlist_add_head(&sdreq->node, &local->pending_sdreqs);
+       }
+
+       mutex_unlock(&local->sdreq_lock);
+
+       skb_queue_tail(&local->tx_queue, skb);
 
        return 0;
 }