forked from luck/tmp_suning_uos_patched
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>
This commit is contained in:
Thierry Escande
Samuel Ortiz
parent
e0ae7bac06
commit
d9b8d8e19b
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user