NFC: llcp: Service Name Lookup netlink interface (d9b8d8e1) · Commits · e / devices / android_kernel_oneplus_sm7250

include/uapi/linux/nfc.h

+12 −0

Original line number	Original line	Diff line number	Diff line
	@@ -90,6 +90,8 @@ enum nfc_commands {
	NFC_CMD_LLC_SET_PARAMS,		NFC_CMD_LLC_SET_PARAMS,
	NFC_CMD_ENABLE_SE,		NFC_CMD_ENABLE_SE,
	NFC_CMD_DISABLE_SE,		NFC_CMD_DISABLE_SE,
			NFC_CMD_LLC_SDREQ,
			NFC_EVENT_LLC_SDRES,
	/* private: internal use only */		/* private: internal use only */
	__NFC_CMD_AFTER_LAST		__NFC_CMD_AFTER_LAST
	};		};
	@@ -140,11 +142,21 @@ enum nfc_attrs {
	NFC_ATTR_LLC_PARAM_RW,		NFC_ATTR_LLC_PARAM_RW,
	NFC_ATTR_LLC_PARAM_MIUX,		NFC_ATTR_LLC_PARAM_MIUX,
	NFC_ATTR_SE,		NFC_ATTR_SE,
			NFC_ATTR_LLC_SDP,
	/* private: internal use only */		/* private: internal use only */
	__NFC_ATTR_AFTER_LAST		__NFC_ATTR_AFTER_LAST
	};		};
	#define NFC_ATTR_MAX (__NFC_ATTR_AFTER_LAST - 1)		#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_DEVICE_NAME_MAXSIZE 8
	#define NFC_NFCID1_MAXSIZE 10		#define NFC_NFCID1_MAXSIZE 10
	#define NFC_SENSB_RES_MAXSIZE 12		#define NFC_SENSB_RES_MAXSIZE 12

net/nfc/llcp/commands.c

+78 −0

Original line number	Original line	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;		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)		void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
	{		{
	kfree(sdp->tlv);		kfree(sdp->tlv);
	kfree(sdp);		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,		int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
	u8 *tlv_array, u16 tlv_array_len)		u8 *tlv_array, u16 tlv_array_len)
	{		{
	@@ -511,6 +558,37 @@ int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
	return 0;		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)		int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
	{		{
	struct sk_buff *skb;		struct sk_buff *skb;

net/nfc/llcp/llcp.c

+32 −0

Original line number	Original line	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->rx_work);
	cancel_work_sync(&local->timeout_work);		cancel_work_sync(&local->timeout_work);
	kfree_skb(local->rx_pending);		kfree_skb(local->rx_pending);
			nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
	kfree(local);		kfree(local);
	}		}

	@@ -1147,6 +1148,7 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
	struct nfc_llcp_sdp_tlv *sdp;		struct nfc_llcp_sdp_tlv *sdp;
	HLIST_HEAD(llc_sdres_list);		HLIST_HEAD(llc_sdres_list);
	size_t sdres_tlvs_len;		size_t sdres_tlvs_len;
			HLIST_HEAD(nl_sdres_list);

	dsap = nfc_llcp_dsap(skb);		dsap = nfc_llcp_dsap(skb);
	ssap = nfc_llcp_ssap(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);		hlist_add_head(&sdp->node, &llc_sdres_list);
	break;		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:		default:
	pr_err("Invalid SNL tlv value 0x%x\n", type);		pr_err("Invalid SNL tlv value 0x%x\n", type);
	break;		break;
	@@ -1239,6 +1265,9 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
	}		}

	exit:		exit:
			if (!hlist_empty(&nl_sdres_list))
			nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);

	if (!hlist_empty(&llc_sdres_list))		if (!hlist_empty(&llc_sdres_list))
	nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);		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_miu = LLCP_DEFAULT_MIU;
	local->remote_lto = LLCP_DEFAULT_LTO;		local->remote_lto = LLCP_DEFAULT_LTO;

			mutex_init(&local->sdreq_lock);
			INIT_HLIST_HEAD(&local->pending_sdreqs);

	list_add(&local->list, &llcp_devices);		list_add(&local->list, &llcp_devices);

	return 0;		return 0;

net/nfc/llcp/llcp.h

+9 −0

Original line number	Original line	Diff line number	Diff line
	@@ -97,6 +97,10 @@ struct nfc_llcp_local {
	u8 remote_opt;		u8 remote_opt;
	u16 remote_wks;		u16 remote_wks;

			struct mutex sdreq_lock;
			struct hlist_head pending_sdreqs;
			u8 sdreq_next_tid;

	/* sockets array */		/* sockets array */
	struct llcp_sock_list sockets;		struct llcp_sock_list sockets;
	struct llcp_sock_list connecting_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);		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);		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_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(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);		void nfc_llcp_recv(void data, struct sk_buff skb, int err);
	int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);		int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
	int nfc_llcp_send_symm(struct nfc_dev *dev);		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_cc(struct nfc_llcp_sock *sock);
	int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,		int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
	struct hlist_head *tlv_list, size_t tlvs_len);		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_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_disconnect(struct nfc_llcp_sock *sock);
	int nfc_llcp_send_i_frame(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	Original line	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_LTO] = { .type = NLA_U8 },
	[NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 },		[NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 },
	[NFC_ATTR_LLC_PARAM_MIUX] = { .type = NLA_U16 },		[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,		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;		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,		static int nfc_genl_send_device(struct sk_buff msg, struct nfc_dev dev,
	u32 portid, u32 seq,		u32 portid, u32 seq,
	struct netlink_callback *cb,		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;		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[] = {		static struct genl_ops nfc_genl_ops[] = {
	{		{
	.cmd = NFC_CMD_GET_DEVICE,		.cmd = NFC_CMD_GET_DEVICE,
	@@ -916,6 +1080,11 @@ static struct genl_ops nfc_genl_ops[] = {
	.doit = nfc_genl_llc_set_params,		.doit = nfc_genl_llc_set_params,
	.policy = nfc_genl_policy,		.policy = nfc_genl_policy,
	},		},
			{
			.cmd = NFC_CMD_LLC_SDREQ,
			.doit = nfc_genl_llc_sdreq,
			.policy = nfc_genl_policy,
			},
	};		};