diff --git a/libnfc/iso14443-subr.c b/libnfc/iso14443-subr.c
index b97bd71..97afbd1 100644
--- a/libnfc/iso14443-subr.c
+++ b/libnfc/iso14443-subr.c
@@ -27,6 +27,7 @@
 #endif // HAVE_CONFIG_H
 
 #include <stdio.h>
+#include <string.h>
 
 #include <nfc/nfc.h>
 
@@ -54,7 +55,7 @@ iso14443a_crc_append (byte_t * pbtData, size_t szLen)
 }
 
 byte_t *
-iso14443a_locate_historical_bytes(byte_t * pbtAts, size_t szAts, size_t * pszTk)
+iso14443a_locate_historical_bytes (byte_t * pbtAts, size_t szAts, size_t * pszTk)
 {
   if (szAts) {
     size_t offset = 1;
@@ -75,3 +76,33 @@ iso14443a_locate_historical_bytes(byte_t * pbtAts, size_t szAts, size_t * pszTk)
   *pszTk = 0;
   return NULL;
 }
+
+/**
+ * @brief Add cascade tags (0x88) in UID
+ * @see ISO/IEC 14443-3 (6.4.4 UID contents and cascade levels)
+ */
+void 
+iso14443_cascade_uid (const byte_t abtUID[], const size_t szUID, byte_t * pbtCascadedUID, size_t * pszCascadedUID)
+{
+  switch (szUID) { 
+    case 7: 
+      pbtCascadedUID[0] = 0x88; 
+      memcpy (pbtCascadedUID + 1, abtUID, 7); 
+      *pszCascadedUID = 8; 
+      break; 
+ 
+    case 10: 
+      pbtCascadedUID[0] = 0x88; 
+      memcpy (pbtCascadedUID + 1, abtUID, 3); 
+      pbtCascadedUID[4] = 0x88; 
+      memcpy (pbtCascadedUID + 5, abtUID + 3, 7); 
+      *pszCascadedUID = 12; 
+      break; 
+ 
+    case 4: 
+    default: 
+      memcpy (pbtCascadedUID, abtUID, szUID); 
+      *pszCascadedUID = szUID; 
+      break; 
+  } 
+}
diff --git a/libnfc/nfc-internal.h b/libnfc/nfc-internal.h
index d5fbb6f..96c8f27 100644
--- a/libnfc/nfc-internal.h
+++ b/libnfc/nfc-internal.h
@@ -213,5 +213,6 @@ struct nfc_driver_t {
 nfc_device_t  *nfc_device_new (void);
 void           nfc_device_free (nfc_device_t *nfc_device);
 
+void 	iso14443_cascade_uid (const byte_t abtUID[], const size_t szUID, byte_t * pbtCascadedUID, size_t * pszCascadedUID);
 
 #endif // __NFC_INTERNAL_H__
diff --git a/libnfc/nfc.c b/libnfc/nfc.c
index 6df6c7b..3ec5e24 100644
--- a/libnfc/nfc.c
+++ b/libnfc/nfc.c
@@ -299,30 +299,9 @@ nfc_initiator_select_passive_target (nfc_device_t * pnd,
   byte_t  abtInit[MAX(12, szInitData)];
   size_t  szInit;
 
-  // TODO Put this in a function: this part is defined by ISO14443-3 (UID and Cascade levels)
   switch (nm.nmt) {
   case NMT_ISO14443A:
-    switch (szInitData) {
-    case 7:
-      abtInit[0] = 0x88;
-      memcpy (abtInit + 1, pbtInitData, 7);
-      szInit = 8;
-      break;
-
-    case 10:
-      abtInit[0] = 0x88;
-      memcpy (abtInit + 1, pbtInitData, 3);
-      abtInit[4] = 0x88;
-      memcpy (abtInit + 5, pbtInitData + 3, 7);
-      szInit = 12;
-      break;
-
-    case 4:
-    default:
-      memcpy (abtInit, pbtInitData, szInitData);
-      szInit = szInitData;
-      break;
-    }
+    iso14443_cascade_uid (pbtInitData, szInitData, abtInit, &szInit);
     break;
 
   default: