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>

diff --git a/include/uapi/linux/nfc.h b/include/uapi/linux/nfc.h
index 855630fe731d64164f1d140c703048ef1c23a0fd..7440bc81a04b5d8af0a8ca109bb3f4662080389e 100644 (file)
--- a/include/uapi/linux/nfc.h
+++ b/include/uapi/linux/nfc.h
@@ -90,6 +90,8 @@ enum nfc_commands {
        NFC_CMD_LLC_SET_PARAMS,
        NFC_CMD_ENABLE_SE,
        NFC_CMD_DISABLE_SE,
+       NFC_CMD_LLC_SDREQ,
+       NFC_EVENT_LLC_SDRES,
 /* private: internal use only */
        __NFC_CMD_AFTER_LAST
 };
@@ -140,11 +142,21 @@ enum nfc_attrs {
        NFC_ATTR_LLC_PARAM_RW,
        NFC_ATTR_LLC_PARAM_MIUX,
        NFC_ATTR_SE,
+       NFC_ATTR_LLC_SDP,
 /* private: internal use only */
        __NFC_ATTR_AFTER_LAST
 };
 #define NFC_ATTR_MAX (__NFC_ATTR_AFTER_LAST - 1)
 
+enum nfc_sdp_attr {
+       NFC_SDP_ATTR_UNSPEC,
+       NFC_SDP_ATTR_URI,
+       NFC_SDP_ATTR_SAP,
+/* private: internal use only */
+       __NFC_SDP_ATTR_AFTER_LAST
+};
+#define NFC_SDP_ATTR_MAX (__NFC_SDP_ATTR_AFTER_LAST - 1)
+
 #define NFC_DEVICE_NAME_MAXSIZE 8
 #define NFC_NFCID1_MAXSIZE 10
 #define NFC_SENSB_RES_MAXSIZE 12

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 59f7ffca783e52fb10fcdb8142dc604de8cd45e7..c943edb07b72a142909a618e9b0e264d7312ece9 100644 (file)
--- 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 30b61c1aa9840ba60213accb9dfdc853d71c9c0d..99e911060422ef126a06c92460e0642a35b8e949 100644 (file)
--- 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 465f5953e2d1ebdfcbd2b9d6c84ec6479e34acf1..ca8c6d94ab85c854f2c11fe946ce244df00ba7c9 100644 (file)
--- 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 63975c039b85dfa00a4402102d6f95fd5b2d6236..73fd51098f4dc1250ecc89fe160f56055b635987 100644 (file)
--- 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 87d914d2876aeb029483ab88242d9e4fda6ca006..94bfe19ba678186cb7467ae03213f07e638046dd 100644 (file)
--- 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)