Commit d9b8d8e1 authored by Thierry Escande's avatar Thierry Escande Committed by Samuel Ortiz

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: default avatarThierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: default avatarSamuel Ortiz <sameo@linux.intel.com>
parent e0ae7bac
......@@ -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
......
......@@ -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;
......
......@@ -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 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
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 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
}
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;
......
......@@ -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,
......
......@@ -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 @@ int nfc_genl_device_removed(struct nfc_dev *dev)
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 @@ static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info)
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,
},
};
......
......@@ -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)
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment