From 3c57861d6b27b9da1d5b7195104d75df6407a255 Mon Sep 17 00:00:00 2001
From: Romuald Conty <romuald@libnfc.org>
Date: Tue, 17 Aug 2010 10:01:11 +0000
Subject: [PATCH] Improve pn53x_decode_target_data() function: move code from
 nfc_initiator_select_passive_target() to the right place (pn53x.c).

---
 libnfc/chips/pn53x.c | 72 +++++++++++++++++++++++++++++++++++++++++++-
 libnfc/nfc.c         | 64 +++++++++++----------------------------
 2 files changed, 89 insertions(+), 47 deletions(-)

diff --git a/libnfc/chips/pn53x.c b/libnfc/chips/pn53x.c
index 8ea1352..ef3800a 100644
--- a/libnfc/chips/pn53x.c
+++ b/libnfc/chips/pn53x.c
@@ -245,8 +245,8 @@ pn53x_decode_target_data(const byte_t* pbtRawData, size_t szDataLen, nfc_chip_t
     case NTT_MIFARE:
     case NTT_GENERIC_PASSIVE_106:
       // We skip the first byte: its the target number (Tg)
-
       pbtRawData++;
+      
       // Somehow they switched the lower and upper ATQA bytes around for the PN531 chipset
       if (nc == NC_PN531) {
         pnti->nai.abtAtqa[1] = *(pbtRawData++);
@@ -268,6 +268,76 @@ pn53x_decode_target_data(const byte_t* pbtRawData, size_t szDataLen, nfc_chip_t
       } else {
         pnti->nai.szAtsLen = 0;
       }
+      
+      // Strip CT (Cascade Tag) to retrieve and store the _real_ UID
+      // (e.g. 0x8801020304050607 is in fact 0x01020304050607)
+      if ((pnti->nai.szUidLen == 8) && (pnti->nai.abtUid[0] == 0x88)) {
+        pnti->nai.szUidLen = 7;
+        memmove (pnti->nai.abtUid, pnti->nai.abtUid + 1, 7);
+      } else if ((pnti->nai.szUidLen == 12) && (pnti->nai.abtUid[0] == 0x88) && (pnti->nai.abtUid[4] == 0x88)) {
+        pnti->nai.szUidLen = 10;
+        memmove (pnti->nai.abtUid, pnti->nai.abtUid + 1, 3);
+        memmove (pnti->nai.abtUid + 3, pnti->nai.abtUid + 5, 7);
+      }
+      break;
+      
+    case NTT_ISO14443B_106:
+      // We skip the first byte: its the target number (Tg)
+      pbtRawData++;
+      
+      // Store the mandatory info
+      memcpy(pnti->nbi.abtAtqb, pbtRawData, 12);
+      pbtRawData += 12;
+      
+      // Store temporarily the ATTRIB_RES length
+      uint8_t ui8AttribResLen = *(pbtRawData++);
+
+      // Store the 4 bytes ID
+      memcpy(pnti->nbi.abtId, pbtRawData,4);
+      pbtRawData += 4;
+
+      pnti->nbi.btParam1 = *(pbtRawData++);
+      pnti->nbi.btParam2 = *(pbtRawData++);
+      pnti->nbi.btParam3 = *(pbtRawData++);
+      pnti->nbi.btParam4 = *(pbtRawData++);
+      
+      // Test if the Higher layer (INF) is available
+      if (ui8AttribResLen > 8) {
+        pnti->nbi.szInfLen = *(pbtRawData++);
+        memcpy(pnti->nbi.abtInf, pbtRawData, pnti->nbi.szInfLen);
+      } else {
+        pnti->nbi.szInfLen = 0;
+      }
+      break;
+      
+    case NTT_FELICA_212:
+    case NTT_FELICA_424:
+      // We skip the first byte: its the target number (Tg)
+      pbtRawData++;
+      
+      // Store the mandatory info
+      pnti->nfi.szLen = *(pbtRawData++);
+      pnti->nfi.btResCode = *(pbtRawData++);
+      // Copy the NFCID2t
+      memcpy(pnti->nfi.abtId, pbtRawData, 8);
+      pbtRawData += 8;
+      // Copy the felica padding
+      memcpy(pnti->nfi.abtPad, pbtRawData, 8);
+      pbtRawData += 8;
+      // Test if the System code (SYST_CODE) is available
+      if (pnti->nfi.szLen > 18)
+      {
+        memcpy(pnti->nfi.abtSysCode, pbtRawData, 2);
+      }
+      break;
+    case NTT_JEWEL_106:
+      // We skip the first byte: its the target number (Tg)
+      pbtRawData++;
+      
+      // Store the mandatory info
+      memcpy(pnti->nji.btSensRes, pbtRawData, 2);
+      pbtRawData += 2;
+      memcpy(pnti->nji.btId, pbtRawData, 4);
       break;
     default:
       return false;
diff --git a/libnfc/nfc.c b/libnfc/nfc.c
index f8558f1..c2ad950 100644
--- a/libnfc/nfc.c
+++ b/libnfc/nfc.c
@@ -484,61 +484,33 @@ nfc_initiator_select_passive_target(const nfc_device_t* pnd,
     switch(nmInitModulation)
     {
       case NM_ISO14443A_106:
-      if(!pn53x_decode_target_data(abtTargetsData+1, szTargetsData-1, pnd->nc, NTT_GENERIC_PASSIVE_106, pnti)) return false;
-      // TODO this should be handled by pn53x_decode_target_data()
-      
-      // Strip CT (Cascade Tag) to retrieve and store the _real_ UID
-      // (e.g. 0x8801020304050607 is in fact 0x01020304050607)
-      if ((pnti->nai.szUidLen == 8) && (pnti->nai.abtUid[0] == 0x88)) {
-        pnti->nai.szUidLen = 7;
-        memmove (pnti->nai.abtUid, pnti->nai.abtUid + 1, 7);
-      } else if ((pnti->nai.szUidLen == 12) && (pnti->nai.abtUid[0] == 0x88) && (pnti->nai.abtUid[4] == 0x88)) {
-        pnti->nai.szUidLen = 10;
-        memmove (pnti->nai.abtUid, pnti->nai.abtUid + 1, 3);
-        memmove (pnti->nai.abtUid + 3, pnti->nai.abtUid + 5, 7);
-      }
+        if(!pn53x_decode_target_data(abtTargetsData+1, szTargetsData-1, pnd->nc, NTT_GENERIC_PASSIVE_106, pnti)) {
+          return false;
+        }
       break;
 
       case NM_FELICA_212:
-      case NM_FELICA_424:
-        // Store the mandatory info
-        pnti->nfi.szLen = abtTargetsData[2];
-        pnti->nfi.btResCode = abtTargetsData[3];
-        // Copy the NFCID2t
-        memcpy(pnti->nfi.abtId,abtTargetsData+4,8);
-        // Copy the felica padding
-        memcpy(pnti->nfi.abtPad,abtTargetsData+12,8);
-        // Test if the System code (SYST_CODE) is available
-        if (szTargetsData > 20)
-        {
-          memcpy(pnti->nfi.abtSysCode,abtTargetsData+20,2);
+        if(!pn53x_decode_target_data(abtTargetsData+1, szTargetsData-1, pnd->nc, NTT_FELICA_212, pnti)) {
+          return false;
         }
-      break;
+        break;
+      case NM_FELICA_424:
+        if(!pn53x_decode_target_data(abtTargetsData+1, szTargetsData-1, pnd->nc, NTT_FELICA_424, pnti)) {
+          return false;
+        }
+        break;
 
       case NM_ISO14443B_106:
-        // Store the mandatory info
-        memcpy(pnti->nbi.abtAtqb,abtTargetsData+2,12);
-        // Ignore the 0x1D byte, and just store the 4 byte id
-        memcpy(pnti->nbi.abtId,abtTargetsData+15,4);
-        pnti->nbi.btParam1 = abtTargetsData[19];
-        pnti->nbi.btParam2 = abtTargetsData[20];
-        pnti->nbi.btParam3 = abtTargetsData[21];
-        pnti->nbi.btParam4 = abtTargetsData[22];
-        // Test if the Higher layer (INF) is available
-        if (szTargetsData > 22)
-        {
-          pnti->nbi.szInfLen = abtTargetsData[23];
-          memcpy(pnti->nbi.abtInf,abtTargetsData+24,pnti->nbi.szInfLen);
-        } else {
-          pnti->nbi.szInfLen = 0;
+        if(!pn53x_decode_target_data(abtTargetsData+1, szTargetsData-1, pnd->nc, NTT_ISO14443B_106, pnti)) {
+          return false;
         }
-      break;
+        break;
 
       case NM_JEWEL_106:
-        // Store the mandatory info
-        memcpy(pnti->nji.btSensRes,abtTargetsData+2,2);
-        memcpy(pnti->nji.btId,abtTargetsData+4,4);
-      break;
+        if(!pn53x_decode_target_data(abtTargetsData+1, szTargetsData-1, pnd->nc, NTT_JEWEL_106, pnti)) {
+          return false;
+        }
+        break;
 
       default:
         // Should not be possible, so whatever...