NFC: nci: Add NFCEE discover support

[firefly-linux-kernel-4.4.55.git] / net / nfc / nci / rsp.c

diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
index 93b914937263c70e9b3af5ad9de7122677c9052b..ee094dfab2edd25d9696ec981b3128eabc941884 100644 (file)
--- a/net/nfc/nci/rsp.c
+++ b/net/nfc/nci/rsp.c
@@ -196,6 +196,23 @@ static void nci_rf_deactivate_rsp_packet(struct nci_dev *ndev,
        }
 }
 
+static void nci_nfcee_discover_rsp_packet(struct nci_dev *ndev,
+                                         struct sk_buff *skb)
+{
+       struct nci_nfcee_discover_rsp *discover_rsp;
+
+       if (skb->len != 2) {
+               nci_req_complete(ndev, NCI_STATUS_NFCEE_PROTOCOL_ERROR);
+               return;
+       }
+
+       discover_rsp = (struct nci_nfcee_discover_rsp *)skb->data;
+
+       if (discover_rsp->status != NCI_STATUS_OK ||
+           discover_rsp->num_nfcee == 0)
+               nci_req_complete(ndev, discover_rsp->status);
+}
+
 void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
 {
        __u16 rsp_opcode = nci_opcode(skb->data);
@@ -241,6 +258,10 @@ void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
                nci_rf_deactivate_rsp_packet(ndev, skb);
                break;
 
+       case NCI_OP_NFCEE_DISCOVER_RSP:
+               nci_nfcee_discover_rsp_packet(ndev, skb);
+               break;
+
        default:
                pr_err("unknown rsp opcode 0x%x\n", rsp_opcode);
                break;