Merge branch 'for-4.2/core' of git://git.kernel.dk/linux-block

[linux-2.6-microblaze.git] / net / nfc / netlink.c

diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 3763036..f85f37e 100644 (file)
--- 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,
+       },
 };