3 files changed, 233 insertions, 2 deletions

diff --git a/net/nfc/digital.h b/net/nfc/digital.h
index 3759add68b1b..71ad7eefddd4 100644
--- a/net/nfc/digital.h
+++ b/net/nfc/digital.h
@@ -71,6 +71,7 @@ static inline int digital_in_send_cmd(struct nfc_digital_dev *ddev,
 void digital_poll_next_tech(struct nfc_digital_dev *ddev);
 
 int digital_in_send_sens_req(struct nfc_digital_dev *ddev, u8 rf_tech);
+int digital_in_send_sensb_req(struct nfc_digital_dev *ddev, u8 rf_tech);
 int digital_in_send_sensf_req(struct nfc_digital_dev *ddev, u8 rf_tech);
 int digital_in_send_iso15693_inv_req(struct nfc_digital_dev *ddev, u8 rf_tech);
 
diff --git a/net/nfc/digital_core.c b/net/nfc/digital_core.c
index e01e15dbf1ab..b105cfb00e76 100644
--- a/net/nfc/digital_core.c
+++ b/net/nfc/digital_core.c
@@ -22,6 +22,8 @@
 #define DIGITAL_PROTO_NFCA_RF_TECH \
         (NFC_PROTO_JEWEL_MASK | NFC_PROTO_MIFARE_MASK | NFC_PROTO_NFC_DEP_MASK)
 
+#define DIGITAL_PROTO_NFCB_RF_TECH      NFC_PROTO_ISO14443_B_MASK
+
 #define DIGITAL_PROTO_NFCF_RF_TECH \
         (NFC_PROTO_FELICA_MASK | NFC_PROTO_NFC_DEP_MASK)
 
@@ -345,6 +347,12 @@ int digital_target_found(struct nfc_digital_dev *ddev,
                 add_crc = digital_skb_add_crc_a;
                 break;
 
+        case NFC_PROTO_ISO14443_B:
+                framing = NFC_DIGITAL_FRAMING_NFCB_T4T;
+                check_crc = digital_skb_check_crc_b;
+                add_crc = digital_skb_add_crc_b;
+                break;
+
         default:
                 pr_err("Invalid protocol %d\n", protocol);
                 return -EINVAL;
@@ -475,6 +483,10 @@ static int digital_start_poll(struct nfc_dev *nfc_dev, __u32 im_protocols,
                 digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106A,
                                       digital_in_send_sens_req);
 
+        if (matching_im_protocols & DIGITAL_PROTO_NFCB_RF_TECH)
+                digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106B,
+                                      digital_in_send_sensb_req);
+
         if (matching_im_protocols & DIGITAL_PROTO_NFCF_RF_TECH) {
                 digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_212F,
                                       digital_in_send_sensf_req);
@@ -635,7 +647,8 @@ static void digital_in_send_complete(struct nfc_digital_dev *ddev, void *arg,
                 goto done;
         }
 
-        if (ddev->curr_protocol == NFC_PROTO_ISO14443) {
+        if ((ddev->curr_protocol == NFC_PROTO_ISO14443) ||
+            (ddev->curr_protocol == NFC_PROTO_ISO14443_B)) {
                 rc = digital_in_iso_dep_pull_sod(ddev, resp);
                 if (rc)
                         goto done;
@@ -676,7 +689,8 @@ static int digital_in_send(struct nfc_dev *nfc_dev, struct nfc_target *target,
                 goto exit;
         }
 
-        if (ddev->curr_protocol == NFC_PROTO_ISO14443) {
+        if ((ddev->curr_protocol == NFC_PROTO_ISO14443) ||
+            (ddev->curr_protocol == NFC_PROTO_ISO14443_B)) {
                 rc = digital_in_iso_dep_push_sod(ddev, skb);
                 if (rc)
                         goto exit;
@@ -747,6 +761,8 @@ struct nfc_digital_dev *nfc_digital_allocate_device(struct nfc_digital_ops *ops,
                 ddev->protocols |= NFC_PROTO_ISO15693_MASK;
         if (supported_protocols & NFC_PROTO_ISO14443_MASK)
                 ddev->protocols |= NFC_PROTO_ISO14443_MASK;
+        if (supported_protocols & NFC_PROTO_ISO14443_B_MASK)
+                ddev->protocols |= NFC_PROTO_ISO14443_B_MASK;
 
         ddev->tx_headroom = tx_headroom + DIGITAL_MAX_HEADER_LEN;
         ddev->tx_tailroom = tx_tailroom + DIGITAL_CRC_LEN;
diff --git a/net/nfc/digital_technology.c b/net/nfc/digital_technology.c
index 278c3fed27e0..88e946adb7bf 100644
--- a/net/nfc/digital_technology.c
+++ b/net/nfc/digital_technology.c
@@ -41,6 +41,24 @@
 #define DIGITAL_MIFARE_READ_RES_LEN 16
 #define DIGITAL_MIFARE_ACK_RES  0x0A
 
+#define DIGITAL_CMD_SENSB_REQ                   0x05
+#define DIGITAL_SENSB_ADVANCED                  BIT(5)
+#define DIGITAL_SENSB_EXTENDED                  BIT(4)
+#define DIGITAL_SENSB_ALLB_REQ                  BIT(3)
+#define DIGITAL_SENSB_N(n)                      ((n) & 0x7)
+
+#define DIGITAL_CMD_SENSB_RES                   0x50
+
+#define DIGITAL_CMD_ATTRIB_REQ                  0x1D
+#define DIGITAL_ATTRIB_P1_TR0_DEFAULT           (0x0 << 6)
+#define DIGITAL_ATTRIB_P1_TR1_DEFAULT           (0x0 << 4)
+#define DIGITAL_ATTRIB_P1_SUPRESS_EOS           BIT(3)
+#define DIGITAL_ATTRIB_P1_SUPRESS_SOS           BIT(2)
+#define DIGITAL_ATTRIB_P2_LISTEN_POLL_1         (0x0 << 6)
+#define DIGITAL_ATTRIB_P2_POLL_LISTEN_1         (0x0 << 4)
+#define DIGITAL_ATTRIB_P2_MAX_FRAME_256         0x8
+#define DIGITAL_ATTRIB_P4_DID(n)                ((n) & 0xf)
+
 #define DIGITAL_CMD_SENSF_REQ   0x00
 #define DIGITAL_CMD_SENSF_RES   0x01
 
@@ -75,6 +93,7 @@ static const u8 digital_ats_fsc[] = {
 };
 
 #define DIGITAL_ATS_FSCI(t0) ((t0) & 0x0F)
+#define DIGITAL_SENSB_FSCI(pi2) (((pi2) & 0xF0) >> 4)
 #define DIGITAL_ATS_MAX_FSC  256
 
 #define DIGITAL_RATS_BYTE1 0xE0
@@ -92,6 +111,32 @@ struct digital_sel_req {
         u8 bcc;
 } __packed;
 
+struct digital_sensb_req {
+        u8 cmd;
+        u8 afi;
+        u8 param;
+} __packed;
+
+struct digital_sensb_res {
+        u8 cmd;
+        u8 nfcid0[4];
+        u8 app_data[4];
+        u8 proto_info[3];
+} __packed;
+
+struct digital_attrib_req {
+        u8 cmd;
+        u8 nfcid0[4];
+        u8 param1;
+        u8 param2;
+        u8 param3;
+        u8 param4;
+} __packed;
+
+struct digital_attrib_res {
+        u8 mbli_did;
+} __packed;
+
 struct digital_sensf_req {
         u8 cmd;
         u8 sc1;
@@ -531,6 +576,175 @@ int digital_in_recv_mifare_res(struct sk_buff *resp)
         return -EIO;
 }
 
+static void digital_in_recv_attrib_res(struct nfc_digital_dev *ddev, void *arg,
+                                       struct sk_buff *resp)
+{
+        struct nfc_target *target = arg;
+        struct digital_attrib_res *attrib_res;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (resp->len < sizeof(*attrib_res)) {
+                PROTOCOL_ERR("12.6.2");
+                rc = -EIO;
+                goto exit;
+        }
+
+        attrib_res = (struct digital_attrib_res *)resp->data;
+
+        if (attrib_res->mbli_did & 0x0f) {
+                PROTOCOL_ERR("12.6.2.1");
+                rc = -EIO;
+                goto exit;
+        }
+
+        rc = digital_target_found(ddev, target, NFC_PROTO_ISO14443_B);
+
+exit:
+        dev_kfree_skb(resp);
+        kfree(target);
+
+        if (rc)
+                digital_poll_next_tech(ddev);
+}
+
+int digital_in_send_attrib_req(struct nfc_digital_dev *ddev,
+                               struct nfc_target *target,
+                               struct digital_sensb_res *sensb_res)
+{
+        struct digital_attrib_req *attrib_req;
+        struct sk_buff *skb;
+        int rc;
+
+        skb = digital_skb_alloc(ddev, sizeof(*attrib_req));
+        if (!skb)
+                return -ENOMEM;
+
+        attrib_req = (struct digital_attrib_req *)skb_put(skb,
+                                                          sizeof(*attrib_req));
+
+        attrib_req->cmd = DIGITAL_CMD_ATTRIB_REQ;
+        memcpy(attrib_req->nfcid0, sensb_res->nfcid0,
+               sizeof(attrib_req->nfcid0));
+        attrib_req->param1 = DIGITAL_ATTRIB_P1_TR0_DEFAULT |
+                             DIGITAL_ATTRIB_P1_TR1_DEFAULT;
+        attrib_req->param2 = DIGITAL_ATTRIB_P2_LISTEN_POLL_1 |
+                             DIGITAL_ATTRIB_P2_POLL_LISTEN_1 |
+                             DIGITAL_ATTRIB_P2_MAX_FRAME_256;
+        attrib_req->param3 = sensb_res->proto_info[1] & 0x07;
+        attrib_req->param4 = DIGITAL_ATTRIB_P4_DID(0);
+
+        rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_attrib_res,
+                                 target);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+static void digital_in_recv_sensb_res(struct nfc_digital_dev *ddev, void *arg,
+                                      struct sk_buff *resp)
+{
+        struct nfc_target *target = NULL;
+        struct digital_sensb_res *sensb_res;
+        u8 fsci;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (resp->len != sizeof(*sensb_res)) {
+                PROTOCOL_ERR("5.6.2.1");
+                rc = -EIO;
+                goto exit;
+        }
+
+        sensb_res = (struct digital_sensb_res *)resp->data;
+
+        if (sensb_res->cmd != DIGITAL_CMD_SENSB_RES) {
+                PROTOCOL_ERR("5.6.2");
+                rc = -EIO;
+                goto exit;
+        }
+
+        if (!(sensb_res->proto_info[1] & BIT(0))) {
+                PROTOCOL_ERR("5.6.2.12");
+                rc = -EIO;
+                goto exit;
+        }
+
+        if (sensb_res->proto_info[1] & BIT(3)) {
+                PROTOCOL_ERR("5.6.2.16");
+                rc = -EIO;
+                goto exit;
+        }
+
+        fsci = DIGITAL_SENSB_FSCI(sensb_res->proto_info[1]);
+        if (fsci >= 8)
+                ddev->target_fsc = DIGITAL_ATS_MAX_FSC;
+        else
+                ddev->target_fsc = digital_ats_fsc[fsci];
+
+        target = kzalloc(sizeof(struct nfc_target), GFP_KERNEL);
+        if (!target) {
+                rc = -ENOMEM;
+                goto exit;
+        }
+
+        rc = digital_in_send_attrib_req(ddev, target, sensb_res);
+
+exit:
+        dev_kfree_skb(resp);
+
+        if (rc) {
+                kfree(target);
+                digital_poll_next_tech(ddev);
+        }
+}
+
+int digital_in_send_sensb_req(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        struct digital_sensb_req *sensb_req;
+        struct sk_buff *skb;
+        int rc;
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+                                     NFC_DIGITAL_RF_TECH_106B);
+        if (rc)
+                return rc;
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFCB);
+        if (rc)
+                return rc;
+
+        skb = digital_skb_alloc(ddev, sizeof(*sensb_req));
+        if (!skb)
+                return -ENOMEM;
+
+        sensb_req = (struct digital_sensb_req *)skb_put(skb,
+                                                        sizeof(*sensb_req));
+
+        sensb_req->cmd = DIGITAL_CMD_SENSB_REQ;
+        sensb_req->afi = 0x00; /* All families and sub-families */
+        sensb_req->param = DIGITAL_SENSB_N(0);
+
+        rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sensb_res,
+                                 NULL);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
 static void digital_in_recv_sensf_res(struct nfc_digital_dev *ddev, void *arg,
                                    struct sk_buff *resp)
 {