emulatetag.cpp

00001 /**************************************************************************/
00002 /*!
00003     @file     emulatetag.cpp
00004     @author   Armin Wieser
00005     @license  BSD
00006 */
00007 /**************************************************************************/
00008 
00009 #include "emulatetag.h "
00010 #include "PN532_debug.h"
00011 
00012 #include <string.h>
00013 
00014 #define MAX_TGREAD
00015 
00016 
00017 // Command APDU
00018 #define C_APDU_CLA   0
00019 #define C_APDU_INS   1 // instruction
00020 #define C_APDU_P1    2 // parameter 1
00021 #define C_APDU_P2    3 // parameter 2
00022 #define C_APDU_LC    4 // length command
00023 #define C_APDU_DATA  5 // data
00024 
00025 #define C_APDU_P1_SELECT_BY_ID   0x00
00026 #define C_APDU_P1_SELECT_BY_NAME 0x04
00027 
00028 // Response APDU
00029 #define R_APDU_SW1_COMMAND_COMPLETE 0x90 
00030 #define R_APDU_SW2_COMMAND_COMPLETE 0x00 
00031 
00032 #define R_APDU_SW1_NDEF_TAG_NOT_FOUND 0x6a
00033 #define R_APDU_SW2_NDEF_TAG_NOT_FOUND 0x82
00034 
00035 #define R_APDU_SW1_FUNCTION_NOT_SUPPORTED 0x6A
00036 #define R_APDU_SW2_FUNCTION_NOT_SUPPORTED 0x81
00037 
00038 #define R_APDU_SW1_MEMORY_FAILURE 0x65
00039 #define R_APDU_SW2_MEMORY_FAILURE 0x81
00040 
00041 #define R_APDU_SW1_END_OF_FILE_BEFORE_REACHED_LE_BYTES 0x62
00042 #define R_APDU_SW2_END_OF_FILE_BEFORE_REACHED_LE_BYTES 0x82
00043 
00044 // ISO7816-4 commands
00045 #define ISO7816_SELECT_FILE 0xA4
00046 #define ISO7816_READ_BINARY 0xB0
00047 #define ISO7816_UPDATE_BINARY 0xD6
00048 
00049 typedef enum { NONE, CC, NDEF } tag_file;   // CC ... Compatibility Container
00050 
00051 const uint8_t compatibility_container[] = {
00052   0, 0x0F,
00053   0x20,
00054   0, 0x54,
00055   0, 0xFF,
00056   0x04,
00057   0x06,
00058   0xE1, 0x04,
00059   0xFF, 0xFE,
00060   0x00,
00061   0x00
00062 };
00063 
00064 bool EmulateTag::init(){
00065   pn532.begin();
00066   return pn532.SAMConfig();
00067 }
00068 
00069 void EmulateTag::setNdefFile(const uint8_t* ndef, const int16_t ndefLength){
00070   if(ndefLength >  (NDEF_MAX_LENGTH -2)){
00071     DMSG("ndef file too large (> NDEF_MAX_LENGHT -2) - aborting");
00072     return;
00073   }
00074 
00075   ndef_file[0] = ndefLength >> 8;
00076   ndef_file[1] = ndefLength & 0xFF;
00077   memcpy(ndef_file+2, ndef, ndefLength);
00078 }
00079 
00080 void EmulateTag::setUid(uint8_t* uid){
00081   uidPtr = uid;
00082 }
00083 
00084 bool EmulateTag::emulate(const uint16_t tgInitAsTargetTimeout){
00085 
00086   uint8_t command[] = {
00087         PN532_COMMAND_TGINITASTARGET,
00088         5,                  // MODE: PICC only, Passive only
00089 
00090         0x04, 0x00,         // SENS_RES
00091         0x00, 0x00, 0x00,   // NFCID1
00092         0x20,               // SEL_RES
00093 
00094         0,0,0,0,0,0,0,0,
00095         0,0,0,0,0,0,0,0,   // FeliCaParams
00096         0,0,
00097 
00098         0,0,0,0,0,0,0,0,0,0, // NFCID3t
00099 
00100         0, // length of general bytes
00101         0  // length of historical bytes
00102   };
00103 
00104   if(uidPtr != 0){  // if uid is set copy 3 bytes to nfcid1
00105     memcpy(command + 4, uidPtr, 3);
00106   }
00107 
00108   if(1 != pn532.tgInitAsTarget(command,sizeof(command), tgInitAsTargetTimeout)){
00109     DMSG("tgInitAsTarget failed or timed out!");
00110     return false;
00111   }
00112 
00113   tagWrittenByInitiator = false;
00114 
00115   uint8_t rwbuf[128];
00116   uint8_t sendlen;
00117   int16_t status;
00118   tag_file currentFile = NONE;
00119   //uint16_t cc_size = sizeof(compatibility_container);
00120   bool runLoop = true;
00121 
00122   while(runLoop){
00123     status = pn532.tgGetData(rwbuf, sizeof(rwbuf));
00124     if(status < 0){
00125       DMSG("tgGetData failed!\n");
00126       pn532.inRelease();
00127       return true;
00128     }
00129 
00130     uint8_t p1 = rwbuf[C_APDU_P1];
00131     uint8_t p2 = rwbuf[C_APDU_P2];
00132     uint8_t lc = rwbuf[C_APDU_LC];
00133     uint16_t p1p2_length = ((int16_t) p1 << 8) + p2;
00134 
00135     switch(rwbuf[C_APDU_INS]){
00136     case ISO7816_SELECT_FILE:
00137       switch(p1){
00138       case C_APDU_P1_SELECT_BY_ID:
00139     if(p2 != 0x0c){
00140       DMSG("C_APDU_P2 != 0x0c\n");
00141       setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00142     } else if(lc == 2 && rwbuf[C_APDU_DATA] == 0xE1 && (rwbuf[C_APDU_DATA+1] == 0x03 || rwbuf[C_APDU_DATA+1] == 0x04)){
00143       setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00144       if(rwbuf[C_APDU_DATA+1] == 0x03){
00145         currentFile = CC;
00146       } else if(rwbuf[C_APDU_DATA+1] == 0x04){
00147         currentFile = NDEF;
00148       }
00149     } else {
00150       setResponse(TAG_NOT_FOUND, rwbuf, &sendlen);
00151     }
00152     break;
00153       case C_APDU_P1_SELECT_BY_NAME: 
00154         const uint8_t ndef_tag_application_name_v2[] = {0, 0x7, 0xD2, 0x76, 0x00, 0x00, 0x85, 0x01, 0x01 };
00155     if(0 == memcmp(ndef_tag_application_name_v2, rwbuf + C_APDU_P2, sizeof(ndef_tag_application_name_v2))){
00156       setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00157     } else{
00158       DMSG("function not supported\n");
00159       setResponse(FUNCTION_NOT_SUPPORTED, rwbuf, &sendlen);
00160     } 
00161     break;
00162       }
00163       break;
00164     case ISO7816_READ_BINARY:
00165       switch(currentFile){
00166       case NONE:
00167     setResponse(TAG_NOT_FOUND, rwbuf, &sendlen);
00168     break;
00169       case CC:
00170     if( p1p2_length > NDEF_MAX_LENGTH){
00171       setResponse(END_OF_FILE_BEFORE_REACHED_LE_BYTES, rwbuf, &sendlen);
00172     }else {
00173       memcpy(rwbuf,compatibility_container + p1p2_length, lc);
00174       setResponse(COMMAND_COMPLETE, rwbuf + lc, &sendlen, lc);
00175     }
00176     break;
00177       case NDEF:
00178     if( p1p2_length > NDEF_MAX_LENGTH){
00179       setResponse(END_OF_FILE_BEFORE_REACHED_LE_BYTES, rwbuf, &sendlen);
00180     }else {
00181       memcpy(rwbuf, ndef_file + p1p2_length, lc);
00182       setResponse(COMMAND_COMPLETE, rwbuf + lc, &sendlen, lc);
00183     }
00184     break;
00185       }
00186       break;    
00187     case ISO7816_UPDATE_BINARY:
00188       if( p1p2_length > NDEF_MAX_LENGTH){
00189     setResponse(MEMORY_FAILURE, rwbuf, &sendlen);
00190       }
00191       else{
00192     memcpy(ndef_file + p1p2_length, rwbuf + C_APDU_DATA, lc);
00193     setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00194     tagWrittenByInitiator = true;
00195       }
00196       break;
00197     default:
00198       DMSG("Command not supported!");
00199       DMSG_HEX(rwbuf[C_APDU_INS]);
00200       DMSG("\n");
00201       setResponse(FUNCTION_NOT_SUPPORTED, rwbuf, &sendlen);
00202     }
00203     status = pn532.tgSetData(rwbuf, sendlen);
00204     if(status < 0){
00205       DMSG("tgSetData failed\n!");
00206       pn532.inRelease();
00207       return true;
00208     }
00209   }
00210   pn532.inRelease();
00211   return true;
00212 }
00213 
00214 void EmulateTag::setResponse(responseCommand cmd, uint8_t* buf, uint8_t* sendlen, uint8_t sendlenOffset){
00215   switch(cmd){
00216   case COMMAND_COMPLETE:
00217     buf[0] = R_APDU_SW1_COMMAND_COMPLETE;
00218     buf[1] = R_APDU_SW2_COMMAND_COMPLETE;
00219     *sendlen = 2 + sendlenOffset;
00220     break;
00221   case TAG_NOT_FOUND:
00222     buf[0] = R_APDU_SW1_NDEF_TAG_NOT_FOUND;
00223     buf[1] = R_APDU_SW2_NDEF_TAG_NOT_FOUND;
00224     *sendlen = 2;    
00225     break;
00226   case FUNCTION_NOT_SUPPORTED:
00227     buf[0] = R_APDU_SW1_FUNCTION_NOT_SUPPORTED;
00228     buf[1] = R_APDU_SW2_FUNCTION_NOT_SUPPORTED;
00229     *sendlen = 2; 
00230     break;
00231   case MEMORY_FAILURE:
00232     buf[0] = R_APDU_SW1_MEMORY_FAILURE;
00233     buf[1] = R_APDU_SW2_MEMORY_FAILURE;
00234     *sendlen = 2;
00235     break;
00236   case END_OF_FILE_BEFORE_REACHED_LE_BYTES:
00237     buf[0] = R_APDU_SW1_END_OF_FILE_BEFORE_REACHED_LE_BYTES;
00238     buf[1] = R_APDU_SW2_END_OF_FILE_BEFORE_REACHED_LE_BYTES;
00239     *sendlen= 2;
00240     break;
00241   }
00242 }