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

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

include/uapi/linux/nfc.h

+12 −0
Original line number Diff line number Diff line
@@ -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

net/nfc/llcp/commands.c

+78 −0
Original line number Diff line number Diff line
@@ -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;

net/nfc/llcp/llcp.c

+32 −0
Original line number Diff line number Diff line
@@ -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;

net/nfc/llcp/llcp.h

+9 −0
Original line number Diff line number Diff line
@@ -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,

net/nfc/netlink.c

+169 −0
Original line number Diff line number Diff line
@@ -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,

	},

};

CRA Git | Maintained and supported by SUSTech CRA and CCSE