1 files changed, 53 insertions, 29 deletions

diff --git a/drivers/nfc/nxp-nci/core.c b/drivers/nfc/nxp-nci/core.c
index a0ce95a287c5..580cb6ecffee 100644
--- a/drivers/nfc/nxp-nci/core.c
+++ b/drivers/nfc/nxp-nci/core.c
@@ -27,6 +27,9 @@
 			       NFC_PROTO_ISO14443_B_MASK | \
 			       NFC_PROTO_NFC_DEP_MASK)
 
+#define NXP_NCI_RF_PLL_UNLOCKED_NTF nci_opcode_pack(NCI_GID_RF_MGMT, 0x21)
+#define NXP_NCI_RF_TXLDO_ERROR_NTF nci_opcode_pack(NCI_GID_RF_MGMT, 0x23)
+
 static int nxp_nci_open(struct nci_dev *ndev)
 {
 	struct nxp_nci_info *info = nci_get_drvdata(ndev);
@@ -70,29 +73,58 @@ static int nxp_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
 	struct nxp_nci_info *info = nci_get_drvdata(ndev);
 	int r;
 
-	if (!info->phy_ops->write) {
-		r = -ENOTSUPP;
-		goto send_exit;
-	}
+	if (!info->phy_ops->write)
+		return -EOPNOTSUPP;
 
-	if (info->mode != NXP_NCI_MODE_NCI) {
-		r = -EINVAL;
-		goto send_exit;
-	}
+	if (info->mode != NXP_NCI_MODE_NCI)
+		return -EINVAL;
 
 	r = info->phy_ops->write(info->phy_id, skb);
-	if (r < 0)
+	if (r < 0) {
 		kfree_skb(skb);
+		return r;
+	}
 
-send_exit:
-	return r;
+	consume_skb(skb);
+	return 0;
+}
+
+static int nxp_nci_rf_pll_unlocked_ntf(struct nci_dev *ndev,
+				       struct sk_buff *skb)
+{
+	nfc_err(&ndev->nfc_dev->dev,
+		"PLL didn't lock. Missing or unstable clock?\n");
+
+	return 0;
+}
+
+static int nxp_nci_rf_txldo_error_ntf(struct nci_dev *ndev,
+				      struct sk_buff *skb)
+{
+	nfc_err(&ndev->nfc_dev->dev,
+		"RF transmitter couldn't start. Bad power and/or configuration?\n");
+
+	return 0;
 }
 
-static struct nci_ops nxp_nci_ops = {
+static const struct nci_driver_ops nxp_nci_core_ops[] = {
+	{
+		.opcode = NXP_NCI_RF_PLL_UNLOCKED_NTF,
+		.ntf = nxp_nci_rf_pll_unlocked_ntf,
+	},
+	{
+		.opcode = NXP_NCI_RF_TXLDO_ERROR_NTF,
+		.ntf = nxp_nci_rf_txldo_error_ntf,
+	},
+};
+
+static const struct nci_ops nxp_nci_ops = {
 	.open = nxp_nci_open,
 	.close = nxp_nci_close,
 	.send = nxp_nci_send,
 	.fw_download = nxp_nci_fw_download,
+	.core_ops = nxp_nci_core_ops,
+	.n_core_ops = ARRAY_SIZE(nxp_nci_core_ops),
 };
 
 int nxp_nci_probe(void *phy_id, struct device *pdev,
@@ -104,10 +136,8 @@ int nxp_nci_probe(void *phy_id, struct device *pdev,
 	int r;
 
 	info = devm_kzalloc(pdev, sizeof(struct nxp_nci_info), GFP_KERNEL);
-	if (!info) {
-		r = -ENOMEM;
-		goto probe_exit;
-	}
+	if (!info)
+		return -ENOMEM;
 
 	info->phy_id = phy_id;
 	info->pdev = pdev;
@@ -120,31 +150,25 @@ int nxp_nci_probe(void *phy_id, struct device *pdev,
 	if (info->phy_ops->set_mode) {
 		r = info->phy_ops->set_mode(info->phy_id, NXP_NCI_MODE_COLD);
 		if (r < 0)
-			goto probe_exit;
+			return r;
 	}
 
 	info->mode = NXP_NCI_MODE_COLD;
 
 	info->ndev = nci_allocate_device(&nxp_nci_ops, NXP_NCI_NFC_PROTOCOLS,
 					 NXP_NCI_HDR_LEN, 0);
-	if (!info->ndev) {
-		r = -ENOMEM;
-		goto probe_exit;
-	}
+	if (!info->ndev)
+		return -ENOMEM;
 
 	nci_set_parent_dev(info->ndev, pdev);
 	nci_set_drvdata(info->ndev, info);
 	r = nci_register_device(info->ndev);
-	if (r < 0)
-		goto probe_exit_free_nci;
+	if (r < 0) {
+		nci_free_device(info->ndev);
+		return r;
+	}
 
 	*ndev = info->ndev;
-
-	goto probe_exit;
-
-probe_exit_free_nci:
-	nci_free_device(info->ndev);
-probe_exit:
 	return r;
 }
 EXPORT_SYMBOL(nxp_nci_probe);