NFC: pn533: Unconditionaly select the highest p2p bit rate

p2p devices must be able to support 424 kbps, so we should always select
that bitrate in initiator mode.

Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

1 files changed, 20 insertions, 36 deletions

diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index fa12f59..ff3e19d 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -2086,20 +2086,6 @@ error:
 	return rc;
 }
 
-static int pn533_mod_to_baud(struct pn533 *dev)
-{
-	switch (dev->poll_mod_curr) {
-	case PN533_POLL_MOD_106KBPS_A:
-		return 0;
-	case PN533_POLL_MOD_212KBPS_FELICA:
-		return 1;
-	case PN533_POLL_MOD_424KBPS_FELICA:
-		return 2;
-	default:
-		return -EINVAL;
-	}
-}
-
 static int pn533_rf_field(struct nfc_dev *nfc_dev, u8 rf);
 #define PASSIVE_DATA_LEN 5
 static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
@@ -2107,8 +2093,8 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	struct sk_buff *skb;
-	int rc, baud, skb_len;
-	u8 *next, *arg;
+	int rc, skb_len;
+	u8 *next, *arg, nfcid3[NFC_NFCID3_MAXSIZE];
 
 	u8 passive_data[PASSIVE_DATA_LEN] = {0x00, 0xff, 0xff, 0x00, 0x3};
 
@@ -2126,41 +2112,39 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 		return -EBUSY;
 	}
 
-	baud = pn533_mod_to_baud(dev);
-	if (baud < 0) {
-		nfc_dev_err(&dev->interface->dev,
-			    "Invalid curr modulation %d", dev->poll_mod_curr);
-		return baud;
-	}
-
 	skb_len = 3 + gb_len; /* ActPass + BR + Next */
-	if (comm_mode == NFC_COMM_PASSIVE)
-		skb_len += PASSIVE_DATA_LEN;
+	skb_len += PASSIVE_DATA_LEN;
 
-	if (target && target->nfcid2_len)
-		skb_len += NFC_NFCID3_MAXSIZE;
+	/* NFCID3 */
+	skb_len += NFC_NFCID3_MAXSIZE;
+	if (target && !target->nfcid2_len) {
+		nfcid3[0] = 0x1;
+		nfcid3[1] = 0xfe;
+		get_random_bytes(nfcid3 + 2, 6);
+	}
 
 	skb = pn533_alloc_skb(dev, skb_len);
 	if (!skb)
 		return -ENOMEM;
 
 	*skb_put(skb, 1) = !comm_mode;  /* ActPass */
-	*skb_put(skb, 1) = baud;  /* Baud rate */
+	*skb_put(skb, 1) = 0x02;  /* 424 kbps */
 
 	next = skb_put(skb, 1);  /* Next */
 	*next = 0;
 
-	if (comm_mode == NFC_COMM_PASSIVE && baud > 0) {
-		memcpy(skb_put(skb, PASSIVE_DATA_LEN), passive_data,
-		       PASSIVE_DATA_LEN);
-		*next |= 1;
-	}
+	/* Copy passive data */
+	memcpy(skb_put(skb, PASSIVE_DATA_LEN), passive_data, PASSIVE_DATA_LEN);
+	*next |= 1;
 
-	if (target && target->nfcid2_len) {
+	/* Copy NFCID3 (which is NFCID2 from SENSF_RES) */
+	if (target && target->nfcid2_len)
 		memcpy(skb_put(skb, NFC_NFCID3_MAXSIZE), target->nfcid2,
 		       target->nfcid2_len);
-		*next |= 2;
-	}
+	else
+		memcpy(skb_put(skb, NFC_NFCID3_MAXSIZE), nfcid3,
+		       NFC_NFCID3_MAXSIZE);
+	*next |= 2;
 
 	if (gb != NULL && gb_len > 0) {
 		memcpy(skb_put(skb, gb_len), gb, gb_len);