NFC: llcp: Service Name Lookup netlink interface

This adds a netlink interface for service name lookup support.
Multiple URIs can be passed nested into the NFC_ATTR_LLC_SDP attribute
using the NFC_CMD_LLC_SDREQ netlink command.
When the SNL reply is received, a NFC_EVENT_LLC_SDRES event is sent to
the user space. URI and SAP tuples are passed back, nested into
NFC_ATTR_LLC_SDP attribute.

Signed-off-by: Thierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

5 files changed, 302 insertions, 0 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 59f7ffca783e..c943edb07b72 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -144,12 +144,59 @@ struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
         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);
+
+        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)
 {
@@ -511,6 +558,37 @@ int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
         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);
+
+        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;
+}
+
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
 {
         struct sk_buff *skb;
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 30b61c1aa984..99e911060422 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -156,6 +156,7 @@ static void local_release(struct kref *ref)
         cancel_work_sync(&local->rx_work);
         cancel_work_sync(&local->timeout_work);
         kfree_skb(local->rx_pending);
+        nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
         kfree(local);
 }
 
@@ -1147,6 +1148,7 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
         struct nfc_llcp_sdp_tlv *sdp;
         HLIST_HEAD(llc_sdres_list);
         size_t sdres_tlvs_len;
+        HLIST_HEAD(nl_sdres_list);
 
         dsap = nfc_llcp_dsap(skb);
         ssap = nfc_llcp_ssap(skb);
@@ -1229,6 +1231,30 @@ add_snl:
                         hlist_add_head(&sdp->node, &llc_sdres_list);
                         break;
 
+                case LLCP_TLV_SDRES:
+                        mutex_lock(&local->sdreq_lock);
+
+                        pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
+
+                        hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
+                                if (sdp->tid != tlv[2])
+                                        continue;
+
+                                sdp->sap = tlv[3];
+
+                                pr_debug("Found: uri=%s, sap=%d\n",
+                                         sdp->uri, sdp->sap);
+
+                                hlist_del(&sdp->node);
+
+                                hlist_add_head(&sdp->node, &nl_sdres_list);
+
+                                break;
+                        }
+
+                        mutex_unlock(&local->sdreq_lock);
+                        break;
+
                 default:
                         pr_err("Invalid SNL tlv value 0x%x\n", type);
                         break;
@@ -1239,6 +1265,9 @@ add_snl:
         }
 
 exit:
+        if (!hlist_empty(&nl_sdres_list))
+                nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+
         if (!hlist_empty(&llc_sdres_list))
                 nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
 }
@@ -1426,6 +1455,9 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
         local->remote_miu = LLCP_DEFAULT_MIU;
         local->remote_lto = LLCP_DEFAULT_LTO;
 
+        mutex_init(&local->sdreq_lock);
+        INIT_HLIST_HEAD(&local->pending_sdreqs);
+
         list_add(&local->list, &llcp_devices);
 
         return 0;
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index 465f5953e2d1..ca8c6d94ab85 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -97,6 +97,10 @@ struct nfc_llcp_local {
         u8  remote_opt;
         u16 remote_wks;
 
+        struct mutex sdreq_lock;
+        struct hlist_head pending_sdreqs;
+        u8 sdreq_next_tid;
+
         /* sockets array */
         struct llcp_sock_list sockets;
         struct llcp_sock_list connecting_sockets;
@@ -230,7 +234,10 @@ int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
 u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length);
 struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap);
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+                                                  size_t uri_len);
 void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head);
 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
 int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_symm(struct nfc_dev *dev);
@@ -238,6 +245,8 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
                             struct hlist_head *tlv_list, size_t tlvs_len);
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+                            struct hlist_head *tlv_list, size_t tlvs_len);
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
 int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 63975c039b85..73fd51098f4d 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -56,6 +56,12 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = {
         [NFC_ATTR_LLC_PARAM_LTO] = { .type = NLA_U8 },
         [NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 },
         [NFC_ATTR_LLC_PARAM_MIUX] = { .type = NLA_U16 },
+        [NFC_ATTR_LLC_SDP] = { .type = NLA_NESTED },
+};
+
+static const struct nla_policy nfc_sdp_genl_policy[NFC_SDP_ATTR_MAX + 1] = {
+        [NFC_SDP_ATTR_URI] = { .type = NLA_STRING },
+        [NFC_SDP_ATTR_SAP] = { .type = NLA_U8 },
 };
 
 static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target,
@@ -351,6 +357,74 @@ free_msg:
         return -EMSGSIZE;
 }
 
+int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list)
+{
+        struct sk_buff *msg;
+        struct nlattr *sdp_attr, *uri_attr;
+        struct nfc_llcp_sdp_tlv *sdres;
+        struct hlist_node *n;
+        void *hdr;
+        int rc = -EMSGSIZE;
+        int i;
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+        if (!msg)
+                return -ENOMEM;
+
+        hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+                          NFC_EVENT_LLC_SDRES);
+        if (!hdr)
+                goto free_msg;
+
+        if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+                goto nla_put_failure;
+
+        sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP);
+        if (sdp_attr == NULL) {
+                rc = -ENOMEM;
+                goto nla_put_failure;
+        }
+
+        i = 1;
+        hlist_for_each_entry_safe(sdres, n, sdres_list, node) {
+                pr_debug("uri: %s, sap: %d\n", sdres->uri, sdres->sap);
+
+                uri_attr = nla_nest_start(msg, i++);
+                if (uri_attr == NULL) {
+                        rc = -ENOMEM;
+                        goto nla_put_failure;
+                }
+
+                if (nla_put_u8(msg, NFC_SDP_ATTR_SAP, sdres->sap))
+                        goto nla_put_failure;
+
+                if (nla_put_string(msg, NFC_SDP_ATTR_URI, sdres->uri))
+                        goto nla_put_failure;
+
+                nla_nest_end(msg, uri_attr);
+
+                hlist_del(&sdres->node);
+
+                nfc_llcp_free_sdp_tlv(sdres);
+        }
+
+        nla_nest_end(msg, sdp_attr);
+
+        genlmsg_end(msg, hdr);
+
+        return genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC);
+
+nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+
+free_msg:
+        nlmsg_free(msg);
+
+        nfc_llcp_free_sdp_tlv_list(sdres_list);
+
+        return rc;
+}
+
 static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
                                 u32 portid, u32 seq,
                                 struct netlink_callback *cb,
@@ -862,6 +936,96 @@ exit:
         return rc;
 }
 
+static int nfc_genl_llc_sdreq(struct sk_buff *skb, struct genl_info *info)
+{
+        struct nfc_dev *dev;
+        struct nfc_llcp_local *local;
+        struct nlattr *attr, *sdp_attrs[NFC_SDP_ATTR_MAX+1];
+        u32 idx;
+        u8 tid;
+        char *uri;
+        int rc = 0, rem;
+        size_t uri_len, tlvs_len;
+        struct hlist_head sdreq_list;
+        struct nfc_llcp_sdp_tlv *sdreq;
+
+        if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+            !info->attrs[NFC_ATTR_LLC_SDP])
+                return -EINVAL;
+
+        idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+        dev = nfc_get_device(idx);
+        if (!dev) {
+                rc = -ENODEV;
+                goto exit;
+        }
+
+        device_lock(&dev->dev);
+
+        if (dev->dep_link_up == false) {
+                rc = -ENOLINK;
+                goto exit;
+        }
+
+        local = nfc_llcp_find_local(dev);
+        if (!local) {
+                nfc_put_device(dev);
+                rc = -ENODEV;
+                goto exit;
+        }
+
+        INIT_HLIST_HEAD(&sdreq_list);
+
+        tlvs_len = 0;
+
+        nla_for_each_nested(attr, info->attrs[NFC_ATTR_LLC_SDP], rem) {
+                rc = nla_parse_nested(sdp_attrs, NFC_SDP_ATTR_MAX, attr,
+                                      nfc_sdp_genl_policy);
+
+                if (rc != 0) {
+                        rc = -EINVAL;
+                        goto exit;
+                }
+
+                if (!sdp_attrs[NFC_SDP_ATTR_URI])
+                        continue;
+
+                uri_len = nla_len(sdp_attrs[NFC_SDP_ATTR_URI]);
+                if (uri_len == 0)
+                        continue;
+
+                uri = nla_data(sdp_attrs[NFC_SDP_ATTR_URI]);
+                if (uri == NULL || *uri == 0)
+                        continue;
+
+                tid = local->sdreq_next_tid++;
+
+                sdreq = nfc_llcp_build_sdreq_tlv(tid, uri, uri_len);
+                if (sdreq == NULL) {
+                        rc = -ENOMEM;
+                        goto exit;
+                }
+
+                tlvs_len += sdreq->tlv_len;
+
+                hlist_add_head(&sdreq->node, &sdreq_list);
+        }
+
+        if (hlist_empty(&sdreq_list)) {
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        rc = nfc_llcp_send_snl_sdreq(local, &sdreq_list, tlvs_len);
+exit:
+        device_unlock(&dev->dev);
+
+        nfc_put_device(dev);
+
+        return rc;
+}
+
 static struct genl_ops nfc_genl_ops[] = {
         {
                 .cmd = NFC_CMD_GET_DEVICE,
@@ -916,6 +1080,11 @@ static struct genl_ops nfc_genl_ops[] = {
                 .doit = nfc_genl_llc_set_params,
                 .policy = nfc_genl_policy,
         },
+        {
+                .cmd = NFC_CMD_LLC_SDREQ,
+                .doit = nfc_genl_llc_sdreq,
+                .policy = nfc_genl_policy,
+        },
 };
 
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index 87d914d2876a..94bfe19ba678 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -46,6 +46,8 @@ struct nfc_rawsock {
 #define to_rawsock_sk(_tx_work) \
         ((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work))
 
+struct nfc_llcp_sdp_tlv;
+
 #ifdef CONFIG_NFC_LLCP
 
 void nfc_llcp_mac_is_down(struct nfc_dev *dev);
@@ -59,6 +61,8 @@ int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb);
 struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
 int __init nfc_llcp_init(void);
 void nfc_llcp_exit(void);
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head);
 
 #else
 
@@ -112,6 +116,14 @@ static inline void nfc_llcp_exit(void)
 {
 }
 
+static inline void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
+{
+}
+
+static inline void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head)
+{
+}
+
 #endif
 
 int __init rawsock_init(void);
@@ -144,6 +156,8 @@ int nfc_genl_dep_link_down_event(struct nfc_dev *dev);
 int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol);
 int nfc_genl_tm_deactivated(struct nfc_dev *dev);
 
+int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list);
+
 struct nfc_dev *nfc_get_device(unsigned int idx);
 
 static inline void nfc_put_device(struct nfc_dev *dev)