diff options
| -rw-r--r-- | net/nfc/digital.h | 5 | ||||
| -rw-r--r-- | net/nfc/digital_core.c | 25 | ||||
| -rw-r--r-- | net/nfc/digital_technology.c | 57 |
3 files changed, 83 insertions, 4 deletions
diff --git a/net/nfc/digital.h b/net/nfc/digital.h index 3c757dc7d44f..3759add68b1b 100644 --- a/net/nfc/digital.h +++ b/net/nfc/digital.h @@ -74,6 +74,11 @@ int digital_in_send_sens_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); +int digital_in_iso_dep_pull_sod(struct nfc_digital_dev *ddev, + struct sk_buff *skb); +int digital_in_iso_dep_push_sod(struct nfc_digital_dev *ddev, + struct sk_buff *skb); + int digital_target_found(struct nfc_digital_dev *ddev, struct nfc_target *target, u8 protocol); diff --git a/net/nfc/digital_core.c b/net/nfc/digital_core.c index e1f240266adf..5b440e7d9598 100644 --- a/net/nfc/digital_core.c +++ b/net/nfc/digital_core.c @@ -624,20 +624,30 @@ static void digital_in_send_complete(struct nfc_digital_dev *ddev, void *arg, if (IS_ERR(resp)) { rc = PTR_ERR(resp); + resp = NULL; goto done; } - if (ddev->curr_protocol == NFC_PROTO_MIFARE) + if (ddev->curr_protocol == NFC_PROTO_MIFARE) { rc = digital_in_recv_mifare_res(resp); - else - rc = ddev->skb_check_crc(resp); + /* crc check is done in digital_in_recv_mifare_res() */ + goto done; + } + + if (ddev->curr_protocol == NFC_PROTO_ISO14443) { + rc = digital_in_iso_dep_pull_sod(ddev, resp); + if (rc) + goto done; + } + + rc = ddev->skb_check_crc(resp); +done: if (rc) { kfree_skb(resp); resp = NULL; } -done: data_exch->cb(data_exch->cb_context, resp, rc); kfree(data_exch); @@ -649,6 +659,7 @@ static int digital_in_send(struct nfc_dev *nfc_dev, struct nfc_target *target, { struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev); struct digital_data_exch *data_exch; + int rc; data_exch = kzalloc(sizeof(struct digital_data_exch), GFP_KERNEL); if (!data_exch) { @@ -662,6 +673,12 @@ static int digital_in_send(struct nfc_dev *nfc_dev, struct nfc_target *target, if (ddev->curr_protocol == NFC_PROTO_NFC_DEP) return digital_in_send_dep_req(ddev, target, skb, data_exch); + if (ddev->curr_protocol == NFC_PROTO_ISO14443) { + rc = digital_in_iso_dep_push_sod(ddev, skb); + if (rc) + return rc; + } + ddev->skb_add_crc(skb); return digital_in_send_cmd(ddev, skb, 500, digital_in_send_complete, diff --git a/net/nfc/digital_technology.c b/net/nfc/digital_technology.c index 6649d9461dff..278c3fed27e0 100644 --- a/net/nfc/digital_technology.c +++ b/net/nfc/digital_technology.c @@ -61,6 +61,15 @@ #define DIGITAL_ISO15693_RES_IS_VALID(flags) \ (!((flags) & DIGITAL_ISO15693_RES_FLAG_ERROR)) +#define DIGITAL_ISO_DEP_I_PCB 0x02 +#define DIGITAL_ISO_DEP_PNI(pni) ((pni) & 0x01) + +#define DIGITAL_ISO_DEP_PCB_TYPE(pcb) ((pcb) & 0xC0) + +#define DIGITAL_ISO_DEP_I_BLOCK 0x00 + +#define DIGITAL_ISO_DEP_BLOCK_HAS_DID(pcb) ((pcb) & 0x08) + static const u8 digital_ats_fsc[] = { 16, 24, 32, 40, 48, 64, 96, 128, }; @@ -118,6 +127,54 @@ struct digital_iso15693_inv_res { static int digital_in_send_sdd_req(struct nfc_digital_dev *ddev, struct nfc_target *target); +int digital_in_iso_dep_pull_sod(struct nfc_digital_dev *ddev, + struct sk_buff *skb) +{ + u8 pcb; + u8 block_type; + + if (skb->len < 1) + return -EIO; + + pcb = *skb->data; + block_type = DIGITAL_ISO_DEP_PCB_TYPE(pcb); + + /* No support fo R-block nor S-block */ + if (block_type != DIGITAL_ISO_DEP_I_BLOCK) { + pr_err("ISO_DEP R-block and S-block not supported\n"); + return -EIO; + } + + if (DIGITAL_ISO_DEP_BLOCK_HAS_DID(pcb)) { + pr_err("DID field in ISO_DEP PCB not supported\n"); + return -EIO; + } + + skb_pull(skb, 1); + + return 0; +} + +int digital_in_iso_dep_push_sod(struct nfc_digital_dev *ddev, + struct sk_buff *skb) +{ + /* + * Chaining not supported so skb->len + 1 PCB byte + 2 CRC bytes must + * not be greater than remote FSC + */ + if (skb->len + 3 > ddev->target_fsc) + return -EIO; + + skb_push(skb, 1); + + *skb->data = DIGITAL_ISO_DEP_I_PCB | ddev->curr_nfc_dep_pni; + + ddev->curr_nfc_dep_pni = + DIGITAL_ISO_DEP_PNI(ddev->curr_nfc_dep_pni + 1); + + return 0; +} + static void digital_in_recv_ats(struct nfc_digital_dev *ddev, void *arg, struct sk_buff *resp) { |