[PATCH 11/11] nfc: pn533: increase clock frequency for PN532

[Michael Thalmeier]

Default clock frequency of PN532 is 6.78 MHz. Increase the frequency to 27.12
MHz to increase throughput.

Signed-off-by: Michael Thalmeier <michael.thalmeier@xxxxxxx>
---
 drivers/nfc/pn533/pn533.c | 70 +++++++++++++++++++++++++++++++++++++++++++++++
 drivers/nfc/pn533/pn533.h |  2 ++
 2 files changed, 72 insertions(+)

diff --git a/drivers/nfc/pn533/pn533.c b/drivers/nfc/pn533/pn533.c
index 44bc5e0..19ee4ba 100644
--- a/drivers/nfc/pn533/pn533.c
+++ b/drivers/nfc/pn533/pn533.c
@@ -2469,6 +2469,63 @@ static int pn532_sam_configuration(struct nfc_dev *nfc_dev)
 	return 0;
 }
 
+#define PN533_CFR_27	0x00
+#define PN533_CFR_13	0x01
+#define PN533_CFR_6	0x02
+
+static int pn533_get_cfr(struct nfc_dev *nfc_dev)
+{
+	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+	struct sk_buff *skb;
+	struct sk_buff *resp;
+	int clk;
+
+	dev_dbg(dev->dev, "%s\n", __func__);
+
+	skb = pn533_alloc_skb(dev, 2);
+	if (!skb)
+		return -ENOMEM;
+
+	*skb_put(skb, 1) = 0x02;
+	*skb_put(skb, 1) = 0xFF;
+
+	resp = pn533_send_cmd_sync(dev, PN533_CMD_READ_REGISTER, skb);
+	if (IS_ERR(resp))
+		return PTR_ERR(resp);
+
+	if (resp->len != 1)
+		return -EINVAL;
+
+	clk = resp->data[0];
+
+	dev_kfree_skb(resp);
+	return clk;
+}
+
+static int pn533_set_cfr(struct nfc_dev *nfc_dev, int clk)
+{
+	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+	struct sk_buff *skb;
+	struct sk_buff *resp;
+
+	dev_dbg(dev->dev, "%s\n", __func__);
+
+	skb = pn533_alloc_skb(dev, 3);
+	if (!skb)
+		return -ENOMEM;
+
+	*skb_put(skb, 1) = 0x02;
+	*skb_put(skb, 1) = 0xFF;
+	*skb_put(skb, 1) = clk & 0x03;
+
+	resp = pn533_send_cmd_sync(dev, PN533_CMD_WRITE_REGISTER, skb);
+	if (IS_ERR(resp))
+		return PTR_ERR(resp);
+
+	dev_kfree_skb(resp);
+	return 0;
+}
+
 static int pn533_dev_up(struct nfc_dev *nfc_dev)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
@@ -2478,6 +2535,10 @@ static int pn533_dev_up(struct nfc_dev *nfc_dev)
 
 		if (rc)
 			return rc;
+
+		rc = pn533_set_cfr(nfc_dev, PN533_CFR_27);
+		if (rc)
+			return rc;
 	}
 
 	return pn533_rf_field(nfc_dev, 1);
@@ -2485,6 +2546,15 @@ static int pn533_dev_up(struct nfc_dev *nfc_dev)
 
 static int pn533_dev_down(struct nfc_dev *nfc_dev)
 {
+	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+
+	if (dev->device_type == PN533_DEVICE_PN532) {
+		int rc = pn533_set_cfr(nfc_dev, PN533_CFR_6);
+
+		if (rc)
+			return rc;
+	}
+
 	return pn533_rf_field(nfc_dev, 0);
 }
 
diff --git a/drivers/nfc/pn533/pn533.h b/drivers/nfc/pn533/pn533.h
index 553c7d1..2bd3816 100644
--- a/drivers/nfc/pn533/pn533.h
+++ b/drivers/nfc/pn533/pn533.h
@@ -74,6 +74,8 @@
 #define PN533_FRAME_CMD(f) (f->data[1])
 
 #define PN533_CMD_GET_FIRMWARE_VERSION 0x02
+#define PN533_CMD_READ_REGISTER 0x06
+#define PN533_CMD_WRITE_REGISTER 0x08
 #define PN533_CMD_SAM_CONFIGURATION 0x14
 #define PN533_CMD_RF_CONFIGURATION 0x32
 #define PN533_CMD_IN_DATA_EXCHANGE 0x40
-- 
2.5.5