nfc: netlink: HCI event connectivity implementation

Add support for missing HCI event EVT_CONNECTIVITY and forward
it to userspace.

Signed-off-by: Christophe Ricard <christophe-h.ricard@st.com>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

3 files changed, 51 insertions, 0 deletions

diff --git a/net/nfc/core.c b/net/nfc/core.c
index 1fe3d3b362c0..122bb81da918 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -953,6 +953,19 @@ out:
 }
 EXPORT_SYMBOL(nfc_se_transaction);
 
+int nfc_se_connectivity(struct nfc_dev *dev, u8 se_idx)
+{
+        int rc;
+
+        pr_debug("connectivity: %x\n", se_idx);
+
+        device_lock(&dev->dev);
+        rc = nfc_genl_se_connectivity(dev, se_idx);
+        device_unlock(&dev->dev);
+        return rc;
+}
+EXPORT_SYMBOL(nfc_se_connectivity);
+
 static void nfc_release(struct device *d)
 {
         struct nfc_dev *dev = to_nfc_dev(d);
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index f58c1fba1026..ea023b35f1c2 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -552,6 +552,43 @@ free_msg:
         return -EMSGSIZE;
 }
 
+int nfc_genl_se_connectivity(struct nfc_dev *dev, u8 se_idx)
+{
+        struct nfc_se *se;
+        struct sk_buff *msg;
+        void *hdr;
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+        if (!msg)
+                return -ENOMEM;
+
+        hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+                          NFC_EVENT_SE_CONNECTIVITY);
+        if (!hdr)
+                goto free_msg;
+
+        se = nfc_find_se(dev, se_idx);
+        if (!se)
+                goto free_msg;
+
+        if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
+            nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx) ||
+            nla_put_u8(msg, NFC_ATTR_SE_TYPE, se->type))
+                goto nla_put_failure;
+
+        genlmsg_end(msg, hdr);
+
+        genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+        return 0;
+
+nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+free_msg:
+        nlmsg_free(msg);
+        return -EMSGSIZE;
+}
+
 static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
                                 u32 portid, u32 seq,
                                 struct netlink_callback *cb,
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index c20b784ad720..6c6f76b370b1 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -105,6 +105,7 @@ int nfc_genl_se_added(struct nfc_dev *dev, u32 se_idx, u16 type);
 int nfc_genl_se_removed(struct nfc_dev *dev, u32 se_idx);
 int nfc_genl_se_transaction(struct nfc_dev *dev, u8 se_idx,
                             struct nfc_evt_transaction *evt_transaction);
+int nfc_genl_se_connectivity(struct nfc_dev *dev, u8 se_idx);
 
 struct nfc_dev *nfc_get_device(unsigned int idx);