Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 29e76924 authored by Christophe Ricard's avatar Christophe Ricard Committed by Samuel Ortiz
Browse files

nfc: netlink: Add capability to reply to vendor_cmd with data 



A proprietary vendor command may send back useful data to the user
application.

For example, the field level applied on the NFC router antenna.

Still based on net/wireless/nl80211.c implementation,
add nfc_vendor_cmd_alloc_reply_skb and nfc_vendor_cmd_reply in
order to send back over netlink data generated by a proprietary
command.

Signed-off-by: default avatarChristophe Ricard <christophe-h.ricard@st.com>
Signed-off-by: default avatarSamuel Ortiz <sameo@linux.intel.com>
parent 5a9e0ffc

include/net/nfc/nfc.h

+41 −0
Original line number Diff line number Diff line
@@ -203,6 +203,7 @@ struct nfc_dev { 
	int n_vendor_cmds;



	struct nfc_ops *ops;

	struct genl_info *cur_cmd_info;

};

#define to_nfc_dev(_dev) container_of(_dev, struct nfc_dev, dev)
@@ -318,4 +319,44 @@ static inline int nfc_set_vendor_cmds(struct nfc_dev *dev, 
	return 0;

}



struct sk_buff *__nfc_alloc_vendor_cmd_reply_skb(struct nfc_dev *dev,

						 enum nfc_attrs attr,

						 u32 oui, u32 subcmd,

						 int approxlen);

int nfc_vendor_cmd_reply(struct sk_buff *skb);



/**

 * nfc_vendor_cmd_alloc_reply_skb - allocate vendor command reply

 * @dev: nfc device

 * @oui: vendor oui

 * @approxlen: an upper bound of the length of the data that will

 *      be put into the skb

 *

 * This function allocates and pre-fills an skb for a reply to

 * a vendor command. Since it is intended for a reply, calling

 * it outside of a vendor command's doit() operation is invalid.

 *

 * The returned skb is pre-filled with some identifying data in

 * a way that any data that is put into the skb (with skb_put(),

 * nla_put() or similar) will end up being within the

 * %NFC_ATTR_VENDOR_DATA attribute, so all that needs to be done

 * with the skb is adding data for the corresponding userspace tool

 * which can then read that data out of the vendor data attribute.

 * You must not modify the skb in any other way.

 *

 * When done, call nfc_vendor_cmd_reply() with the skb and return

 * its error code as the result of the doit() operation.

 *

 * Return: An allocated and pre-filled skb. %NULL if any errors happen.

 */

static inline struct sk_buff *

nfc_vendor_cmd_alloc_reply_skb(struct nfc_dev *dev,

				u32 oui, u32 subcmd, int approxlen)

{

	return __nfc_alloc_vendor_cmd_reply_skb(dev,

						NFC_ATTR_VENDOR_DATA,

						oui,

						subcmd, approxlen);

}



#endif /* __NET_NFC_H */

net/nfc/netlink.c

+84 −2
Original line number Diff line number Diff line
@@ -63,6 +63,8 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = { 
	[NFC_ATTR_FIRMWARE_NAME] = { .type = NLA_STRING,

				     .len = NFC_FIRMWARE_NAME_MAXSIZE },

	[NFC_ATTR_SE_APDU] = { .type = NLA_BINARY },

	[NFC_ATTR_VENDOR_DATA] = { .type = NLA_BINARY },



};



static const struct nla_policy nfc_sdp_genl_policy[NFC_SDP_ATTR_MAX + 1] = {
@@ -1503,7 +1505,7 @@ static int nfc_genl_vendor_cmd(struct sk_buff *skb, 
	u32 dev_idx, vid, subcmd;

	u8 *data;

	size_t data_len;

	int i;

	int i, err;



	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||

	    !info->attrs[NFC_ATTR_VENDOR_ID] ||
@@ -1534,12 +1536,92 @@ static int nfc_genl_vendor_cmd(struct sk_buff *skb, 
		if (cmd->vendor_id != vid || cmd->subcmd != subcmd)

			continue;



		return cmd->doit(dev, data, data_len);

		dev->cur_cmd_info = info;

		err = cmd->doit(dev, data, data_len);

		dev->cur_cmd_info = NULL;

		return err;

	}



	return -EOPNOTSUPP;

}



/* message building helper */

static inline void *nfc_hdr_put(struct sk_buff *skb, u32 portid, u32 seq,

				int flags, u8 cmd)

{

	/* since there is no private header just add the generic one */

	return genlmsg_put(skb, portid, seq, &nfc_genl_family, flags, cmd);

}



static struct sk_buff *

__nfc_alloc_vendor_cmd_skb(struct nfc_dev *dev, int approxlen,

			   u32 portid, u32 seq,

			   enum nfc_attrs attr,

			   u32 oui, u32 subcmd, gfp_t gfp)

{

	struct sk_buff *skb;

	void *hdr;



	skb = nlmsg_new(approxlen + 100, gfp);

	if (!skb)

		return NULL;



	hdr = nfc_hdr_put(skb, portid, seq, 0, NFC_CMD_VENDOR);

	if (!hdr) {

		kfree_skb(skb);

		return NULL;

	}



	if (nla_put_u32(skb, NFC_ATTR_DEVICE_INDEX, dev->idx))

		goto nla_put_failure;

	if (nla_put_u32(skb, NFC_ATTR_VENDOR_ID, oui))

		goto nla_put_failure;

	if (nla_put_u32(skb, NFC_ATTR_VENDOR_SUBCMD, subcmd))

		goto nla_put_failure;



	((void **)skb->cb)[0] = dev;

	((void **)skb->cb)[1] = hdr;



	return skb;



nla_put_failure:

	kfree_skb(skb);

	return NULL;

}



struct sk_buff *__nfc_alloc_vendor_cmd_reply_skb(struct nfc_dev *dev,

						 enum nfc_attrs attr,

						 u32 oui, u32 subcmd,

						 int approxlen)

{

	if (WARN_ON(!dev->cur_cmd_info))

		return NULL;



	return __nfc_alloc_vendor_cmd_skb(dev, approxlen,

					  dev->cur_cmd_info->snd_portid,

					  dev->cur_cmd_info->snd_seq, attr,

					  oui, subcmd, GFP_KERNEL);

}

EXPORT_SYMBOL(__nfc_alloc_vendor_cmd_reply_skb);



int nfc_vendor_cmd_reply(struct sk_buff *skb)

{

	struct nfc_dev *dev = ((void **)skb->cb)[0];

	void *hdr = ((void **)skb->cb)[1];



	/* clear CB data for netlink core to own from now on */

	memset(skb->cb, 0, sizeof(skb->cb));



	if (WARN_ON(!dev->cur_cmd_info)) {

		kfree_skb(skb);

		return -EINVAL;

	}



	genlmsg_end(skb, hdr);

	return genlmsg_reply(skb, dev->cur_cmd_info);

}

EXPORT_SYMBOL(nfc_vendor_cmd_reply);



static const struct genl_ops nfc_genl_ops[] = {

	{

		.cmd = NFC_CMD_GET_DEVICE,