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 bool EmulateTag::init(){
00052   pn532.begin();
00053   return pn532.SAMConfig();
00054 }
00055 
00056 void EmulateTag::setNdefFile(const uint8_t* ndef, const int16_t ndefLength){
00057   if(ndefLength >  (NDEF_MAX_LENGTH -2)){
00058     DMSG("ndef file too large (> NDEF_MAX_LENGHT -2) - aborting");
00059     return;
00060   }
00061 
00062   ndef_file[0] = ndefLength >> 8;
00063   ndef_file[1] = ndefLength & 0xFF;
00064   memcpy(ndef_file+2, ndef, ndefLength);
00065 }
00066 
00067 void EmulateTag::setUid(uint8_t* uid){
00068   uidPtr = uid;
00069 }
00070 
00071 bool EmulateTag::emulate(const uint16_t tgInitAsTargetTimeout){
00072 
00073   uint8_t command[] = {
00074         PN532_COMMAND_TGINITASTARGET,
00075         5,                  // MODE: PICC only, Passive only
00076 
00077         0x04, 0x00,         // SENS_RES
00078         0x00, 0x00, 0x00,   // NFCID1
00079         0x20,               // SEL_RES
00080 
00081         0,0,0,0,0,0,0,0,
00082         0,0,0,0,0,0,0,0,   // FeliCaParams
00083         0,0,
00084 
00085         0,0,0,0,0,0,0,0,0,0, // NFCID3t
00086 
00087         0, // length of general bytes
00088         0  // length of historical bytes
00089   };
00090 
00091   if(uidPtr != 0){  // if uid is set copy 3 bytes to nfcid1
00092     memcpy(command + 4, uidPtr, 3);
00093   }
00094 
00095   if(1 != pn532.tgInitAsTarget(command,sizeof(command), tgInitAsTargetTimeout)){
00096     DMSG("tgInitAsTarget failed or timed out!");
00097     return false;
00098   }
00099 
00100   uint8_t compatibility_container[] = {
00101     0, 0x0F,
00102     0x20,
00103     0, 0x54,
00104     0, 0xFF,
00105     0x04,       // T
00106     0x06,       // L
00107     0xE1, 0x04, // File identifier
00108     ((NDEF_MAX_LENGTH & 0xFF00) >> 8), (NDEF_MAX_LENGTH & 0xFF), // maximum NDEF file size
00109     0x00,       // read access 0x0 = granted
00110     0x00        // write access 0x0 = granted | 0xFF = deny
00111   };
00112 
00113   if(tagWriteable == false){
00114     compatibility_container[14] = 0xFF;
00115   }
00116 
00117   tagWrittenByInitiator = false;
00118 
00119   uint8_t rwbuf[128];
00120   uint8_t sendlen;
00121   int16_t status;
00122   tag_file currentFile = NONE;
00123   uint16_t cc_size = sizeof(compatibility_container);
00124   bool runLoop = true;
00125 
00126   while(runLoop){
00127     status = pn532.tgGetData(rwbuf, sizeof(rwbuf));
00128     if(status < 0){
00129       DMSG("tgGetData failed!\n");
00130       pn532.inRelease();
00131       return true;
00132     }
00133 
00134     uint8_t p1 = rwbuf[C_APDU_P1];
00135     uint8_t p2 = rwbuf[C_APDU_P2];
00136     uint8_t lc = rwbuf[C_APDU_LC];
00137     uint16_t p1p2_length = ((int16_t) p1 << 8) + p2;
00138 
00139     switch(rwbuf[C_APDU_INS]){
00140     case ISO7816_SELECT_FILE:
00141       switch(p1){
00142       case C_APDU_P1_SELECT_BY_ID:
00143     if(p2 != 0x0c){
00144       DMSG("C_APDU_P2 != 0x0c\n");
00145       setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00146     } else if(lc == 2 && rwbuf[C_APDU_DATA] == 0xE1 && (rwbuf[C_APDU_DATA+1] == 0x03 || rwbuf[C_APDU_DATA+1] == 0x04)){
00147       setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00148       if(rwbuf[C_APDU_DATA+1] == 0x03){
00149         currentFile = CC;
00150       } else if(rwbuf[C_APDU_DATA+1] == 0x04){
00151         currentFile = NDEF;
00152       }
00153     } else {
00154       setResponse(TAG_NOT_FOUND, rwbuf, &sendlen);
00155     }
00156     break;
00157       case C_APDU_P1_SELECT_BY_NAME: 
00158         const uint8_t ndef_tag_application_name_v2[] = {0, 0x7, 0xD2, 0x76, 0x00, 0x00, 0x85, 0x01, 0x01 };
00159     if(0 == memcmp(ndef_tag_application_name_v2, rwbuf + C_APDU_P2, sizeof(ndef_tag_application_name_v2))){
00160       setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00161     } else{
00162       DMSG("function not supported\n");
00163       setResponse(FUNCTION_NOT_SUPPORTED, rwbuf, &sendlen);
00164     } 
00165     break;
00166       }
00167       break;
00168     case ISO7816_READ_BINARY:
00169       switch(currentFile){
00170       case NONE:
00171     setResponse(TAG_NOT_FOUND, rwbuf, &sendlen);
00172     break;
00173       case CC:
00174     if( p1p2_length > NDEF_MAX_LENGTH){
00175       setResponse(END_OF_FILE_BEFORE_REACHED_LE_BYTES, rwbuf, &sendlen);
00176     }else {
00177       memcpy(rwbuf,compatibility_container + p1p2_length, lc);
00178       setResponse(COMMAND_COMPLETE, rwbuf + lc, &sendlen, lc);
00179     }
00180     break;
00181       case NDEF:
00182     if( p1p2_length > NDEF_MAX_LENGTH){
00183       setResponse(END_OF_FILE_BEFORE_REACHED_LE_BYTES, rwbuf, &sendlen);
00184     }else {
00185       memcpy(rwbuf, ndef_file + p1p2_length, lc);
00186       setResponse(COMMAND_COMPLETE, rwbuf + lc, &sendlen, lc);
00187     }
00188     break;
00189       }
00190       break;    
00191     case ISO7816_UPDATE_BINARY:
00192       if(!tagWriteable){
00193       setResponse(FUNCTION_NOT_SUPPORTED, rwbuf, &sendlen);
00194       } else{      
00195     if( p1p2_length > NDEF_MAX_LENGTH){
00196       setResponse(MEMORY_FAILURE, rwbuf, &sendlen);
00197     }
00198     else{
00199       memcpy(ndef_file + p1p2_length, rwbuf + C_APDU_DATA, lc);
00200       setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
00201       tagWrittenByInitiator = true;
00202       
00203       uint16_t ndef_length = (ndef_file[0] << 8) + ndef_file[1];
00204       if ((ndef_length > 0) && (updateNdefCallback != 0)) {
00205         updateNdefCallback(ndef_file + 2, ndef_length);
00206       }
00207     }
00208       }
00209       break;
00210     default:
00211       DMSG("Command not supported!");
00212       DMSG_HEX(rwbuf[C_APDU_INS]);
00213       DMSG("\n");
00214       setResponse(FUNCTION_NOT_SUPPORTED, rwbuf, &sendlen);
00215     }
00216     status = pn532.tgSetData(rwbuf, sendlen);
00217     if(status < 0){
00218       DMSG("tgSetData failed\n!");
00219       pn532.inRelease();
00220       return true;
00221     }
00222   }
00223   pn532.inRelease();
00224   return true;
00225 }
00226 
00227 void EmulateTag::setResponse(responseCommand cmd, uint8_t* buf, uint8_t* sendlen, uint8_t sendlenOffset){
00228   switch(cmd){
00229   case COMMAND_COMPLETE:
00230     buf[0] = R_APDU_SW1_COMMAND_COMPLETE;
00231     buf[1] = R_APDU_SW2_COMMAND_COMPLETE;
00232     *sendlen = 2 + sendlenOffset;
00233     break;
00234   case TAG_NOT_FOUND:
00235     buf[0] = R_APDU_SW1_NDEF_TAG_NOT_FOUND;
00236     buf[1] = R_APDU_SW2_NDEF_TAG_NOT_FOUND;
00237     *sendlen = 2;    
00238     break;
00239   case FUNCTION_NOT_SUPPORTED:
00240     buf[0] = R_APDU_SW1_FUNCTION_NOT_SUPPORTED;
00241     buf[1] = R_APDU_SW2_FUNCTION_NOT_SUPPORTED;
00242     *sendlen = 2; 
00243     break;
00244   case MEMORY_FAILURE:
00245     buf[0] = R_APDU_SW1_MEMORY_FAILURE;
00246     buf[1] = R_APDU_SW2_MEMORY_FAILURE;
00247     *sendlen = 2;
00248     break;
00249   case END_OF_FILE_BEFORE_REACHED_LE_BYTES:
00250     buf[0] = R_APDU_SW1_END_OF_FILE_BEFORE_REACHED_LE_BYTES;
00251     buf[1] = R_APDU_SW2_END_OF_FILE_BEFORE_REACHED_LE_BYTES;
00252     *sendlen= 2;
00253     break;
00254   }
00255 }