NFC: netlink: Implement vendor command support

Vendor commands are passed from userspace through the
NFC_CMD_VENDOR netlink command, allowing driver and hardware
specific operations implementations like for example RF tuning
or production line calibration.

Drivers will associate a set of vendor commands to a vendor
id, which could typically be an OUI. The netlink kernel
implementation will try to match the received vendor id
and sub command attributes with the registered ones. When
such match is found, the driver defined sub command routine
is called.

Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

1 files changed, 55 insertions, 0 deletions

diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 3763036710ae..f85f37ed19b2 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -5,6 +5,12 @@
  *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
  *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
  *
+ * Vendor commands implementation based on net/wireless/nl80211.c
+ * which is:
+ *
+ * Copyright 2006-2010  Johannes Berg <johannes@sipsolutions.net>
+ * Copyright 2013-2014  Intel Mobile Communications GmbH
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; either version 2 of the License, or
@@ -1489,6 +1495,50 @@ static int nfc_genl_se_io(struct sk_buff *skb, struct genl_info *info)
         return nfc_se_io(dev, se_idx, apdu, apdu_len, se_io_cb, ctx);
 }
 
+static int nfc_genl_vendor_cmd(struct sk_buff *skb,
+                               struct genl_info *info)
+{
+        struct nfc_dev *dev;
+        struct nfc_vendor_cmd *cmd;
+        u32 dev_idx, vid, subcmd;
+        u8 *data;
+        size_t data_len;
+        int i;
+
+        if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+            !info->attrs[NFC_ATTR_VENDOR_ID] ||
+            !info->attrs[NFC_ATTR_VENDOR_SUBCMD])
+                return -EINVAL;
+
+        dev_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+        vid = nla_get_u32(info->attrs[NFC_ATTR_VENDOR_ID]);
+        subcmd = nla_get_u32(info->attrs[NFC_ATTR_VENDOR_SUBCMD]);
+
+        dev = nfc_get_device(dev_idx);
+        if (!dev || !dev->vendor_cmds || !dev->n_vendor_cmds)
+                return -ENODEV;
+
+        data = nla_data(info->attrs[NFC_ATTR_VENDOR_DATA]);
+        if (data) {
+                data_len = nla_len(info->attrs[NFC_ATTR_VENDOR_DATA]);
+                if (data_len == 0)
+                        return -EINVAL;
+        } else {
+                data_len = 0;
+        }
+
+        for (i = 0; i < dev->n_vendor_cmds; i++) {
+                cmd = &dev->vendor_cmds[i];
+
+                if (cmd->vendor_id != vid || cmd->subcmd != subcmd)
+                        continue;
+
+                return cmd->doit(dev, data, data_len);
+        }
+
+        return -EOPNOTSUPP;
+}
+
 static const struct genl_ops nfc_genl_ops[] = {
         {
                 .cmd = NFC_CMD_GET_DEVICE,
@@ -1579,6 +1629,11 @@ static const struct genl_ops nfc_genl_ops[] = {
                 .doit = nfc_genl_activate_target,
                 .policy = nfc_genl_policy,
         },
+        {
+                .cmd = NFC_CMD_VENDOR,
+                .doit = nfc_genl_vendor_cmd,
+                .policy = nfc_genl_policy,
+        },
 };