File content as of revision 3:4189a10038e6:

/**************************************************************************/
/*!
    @file     emulatetag.cpp
    @author   Armin Wieser
    @license  BSD
*/
/**************************************************************************/

#include "emulatetag.h"
#include "PN532_debug.h"

#include <string.h>

#define MAX_TGREAD


// Command APDU
#define C_APDU_CLA   0
#define C_APDU_INS   1 // instruction
#define C_APDU_P1    2 // parameter 1
#define C_APDU_P2    3 // parameter 2
#define C_APDU_LC    4 // length command
#define C_APDU_DATA  5 // data

#define C_APDU_P1_SELECT_BY_ID   0x00
#define C_APDU_P1_SELECT_BY_NAME 0x04

// Response APDU
#define R_APDU_SW1_COMMAND_COMPLETE 0x90 
#define R_APDU_SW2_COMMAND_COMPLETE 0x00 

#define R_APDU_SW1_NDEF_TAG_NOT_FOUND 0x6a
#define R_APDU_SW2_NDEF_TAG_NOT_FOUND 0x82

#define R_APDU_SW1_FUNCTION_NOT_SUPPORTED 0x6A
#define R_APDU_SW2_FUNCTION_NOT_SUPPORTED 0x81

#define R_APDU_SW1_MEMORY_FAILURE 0x65
#define R_APDU_SW2_MEMORY_FAILURE 0x81

#define R_APDU_SW1_END_OF_FILE_BEFORE_REACHED_LE_BYTES 0x62
#define R_APDU_SW2_END_OF_FILE_BEFORE_REACHED_LE_BYTES 0x82

// ISO7816-4 commands
#define ISO7816_SELECT_FILE 0xA4
#define ISO7816_READ_BINARY 0xB0
#define ISO7816_UPDATE_BINARY 0xD6

typedef enum { NONE, CC, NDEF } tag_file;   // CC ... Compatibility Container

const uint8_t compatibility_container[] = {
  0, 0x0F,
  0x20,
  0, 0x54,
  0, 0xFF,
  0x04,
  0x06,
  0xE1, 0x04,
  0xFF, 0xFE,
  0x00,
  0x00
};

bool EmulateTag::init(){
  pn532.begin();
  return pn532.SAMConfig();
}

void EmulateTag::setNdefFile(const uint8_t* ndef, const int16_t ndefLength){
  if(ndefLength >  (NDEF_MAX_LENGTH -2)){
	DMSG("ndef file too large (> NDEF_MAX_LENGHT -2) - aborting");
	return;
  }

  ndef_file[0] = ndefLength >> 8;
  ndef_file[1] = ndefLength & 0xFF;
  memcpy(ndef_file+2, ndef, ndefLength);
}

void EmulateTag::setUid(uint8_t* uid){
  uidPtr = uid;
}

bool EmulateTag::emulate(const uint16_t tgInitAsTargetTimeout){

  uint8_t command[] = {
        PN532_COMMAND_TGINITASTARGET,
        5,                  // MODE: PICC only, Passive only

        0x04, 0x00,         // SENS_RES
        0x00, 0x00, 0x00,   // NFCID1
        0x20,               // SEL_RES

        0,0,0,0,0,0,0,0,
        0,0,0,0,0,0,0,0,   // FeliCaParams
        0,0,

        0,0,0,0,0,0,0,0,0,0, // NFCID3t

        0, // length of general bytes
        0  // length of historical bytes
  };

  if(uidPtr != 0){  // if uid is set copy 3 bytes to nfcid1
    memcpy(command + 4, uidPtr, 3);
  }

  if(1 != pn532.tgInitAsTarget(command,sizeof(command), tgInitAsTargetTimeout)){
    DMSG("tgInitAsTarget failed or timed out!");
    return false;
  }

  tagWrittenByInitiator = false;

  uint8_t rwbuf[128];
  uint8_t sendlen;
  int16_t status;
  tag_file currentFile = NONE;
  uint16_t cc_size = sizeof(compatibility_container);
  bool runLoop = true;

  while(runLoop){
    status = pn532.tgGetData(rwbuf, sizeof(rwbuf));
    if(status < 0){
      DMSG("tgGetData failed!\n");
      pn532.inRelease();
      return true;
    }

    uint8_t p1 = rwbuf[C_APDU_P1];
    uint8_t p2 = rwbuf[C_APDU_P2];
    uint8_t lc = rwbuf[C_APDU_LC];
    uint16_t p1p2_length = ((int16_t) p1 << 8) + p2;

    switch(rwbuf[C_APDU_INS]){
    case ISO7816_SELECT_FILE:
      switch(p1){
      case C_APDU_P1_SELECT_BY_ID:
	if(p2 != 0x0c){
	  DMSG("C_APDU_P2 != 0x0c\n");
	  setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
	} else if(lc == 2 && rwbuf[C_APDU_DATA] == 0xE1 && (rwbuf[C_APDU_DATA+1] == 0x03 || rwbuf[C_APDU_DATA+1] == 0x04)){
	  setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
	  if(rwbuf[C_APDU_DATA+1] == 0x03){
	    currentFile = CC;
	  } else if(rwbuf[C_APDU_DATA+1] == 0x04){
	    currentFile = NDEF;
	  }
	} else {
	  setResponse(TAG_NOT_FOUND, rwbuf, &sendlen);
	}
	break;
      case C_APDU_P1_SELECT_BY_NAME: 
        const uint8_t ndef_tag_application_name_v2[] = {0, 0x7, 0xD2, 0x76, 0x00, 0x00, 0x85, 0x01, 0x01 };
	if(0 == memcmp(ndef_tag_application_name_v2, rwbuf + C_APDU_P2, sizeof(ndef_tag_application_name_v2))){
	  setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
	} else{
	  DMSG("function not supported\n");
	  setResponse(FUNCTION_NOT_SUPPORTED, rwbuf, &sendlen);
	} 
	break;
      }
      break;
    case ISO7816_READ_BINARY:
      switch(currentFile){
      case NONE:
	setResponse(TAG_NOT_FOUND, rwbuf, &sendlen);
	break;
      case CC:
	if( p1p2_length > NDEF_MAX_LENGTH){
	  setResponse(END_OF_FILE_BEFORE_REACHED_LE_BYTES, rwbuf, &sendlen);
	}else {
	  memcpy(rwbuf,compatibility_container + p1p2_length, lc);
	  setResponse(COMMAND_COMPLETE, rwbuf + lc, &sendlen, lc);
	}
	break;
      case NDEF:
	if( p1p2_length > NDEF_MAX_LENGTH){
	  setResponse(END_OF_FILE_BEFORE_REACHED_LE_BYTES, rwbuf, &sendlen);
	}else {
	  memcpy(rwbuf, ndef_file + p1p2_length, lc);
	  setResponse(COMMAND_COMPLETE, rwbuf + lc, &sendlen, lc);
	}
	break;
      }
      break;    
    case ISO7816_UPDATE_BINARY:
      if( p1p2_length > NDEF_MAX_LENGTH){
	setResponse(MEMORY_FAILURE, rwbuf, &sendlen);
      }
      else{
	memcpy(ndef_file + p1p2_length, rwbuf + C_APDU_DATA, lc);
	setResponse(COMMAND_COMPLETE, rwbuf, &sendlen);
	tagWrittenByInitiator = true;
      }
      break;
    default:
      DMSG("Command not supported!");
      DMSG_HEX(rwbuf[C_APDU_INS]);
      DMSG("\n");
      setResponse(FUNCTION_NOT_SUPPORTED, rwbuf, &sendlen);
    }
    status = pn532.tgSetData(rwbuf, sendlen);
    if(status < 0){
      DMSG("tgSetData failed\n!");
      pn532.inRelease();
      return true;
    }
  }
  pn532.inRelease();
  return true;
}

void EmulateTag::setResponse(responseCommand cmd, uint8_t* buf, uint8_t* sendlen, uint8_t sendlenOffset){
  switch(cmd){
  case COMMAND_COMPLETE:
    buf[0] = R_APDU_SW1_COMMAND_COMPLETE;
    buf[1] = R_APDU_SW2_COMMAND_COMPLETE;
    *sendlen = 2 + sendlenOffset;
    break;
  case TAG_NOT_FOUND:
    buf[0] = R_APDU_SW1_NDEF_TAG_NOT_FOUND;
    buf[1] = R_APDU_SW2_NDEF_TAG_NOT_FOUND;
    *sendlen = 2;    
    break;
  case FUNCTION_NOT_SUPPORTED:
    buf[0] = R_APDU_SW1_FUNCTION_NOT_SUPPORTED;
    buf[1] = R_APDU_SW2_FUNCTION_NOT_SUPPORTED;
    *sendlen = 2; 
    break;
  case MEMORY_FAILURE:
    buf[0] = R_APDU_SW1_MEMORY_FAILURE;
    buf[1] = R_APDU_SW2_MEMORY_FAILURE;
    *sendlen = 2;
    break;
  case END_OF_FILE_BEFORE_REACHED_LE_BYTES:
    buf[0] = R_APDU_SW1_END_OF_FILE_BEFORE_REACHED_LE_BYTES;
    buf[1] = R_APDU_SW2_END_OF_FILE_BEFORE_REACHED_LE_BYTES;
    *sendlen= 2;
    break;
  }
}