/*******************************************************************************
 * @file    RFID.cpp
 * @brief
 * @version V0.0.1
 * @date    01/12/2012
 *
 ******************************************************************************/
 
/*
* Copyright © 2013 Texas Instruments Incorporated - http://www.ti.com/ 
 * 
 *  Redistribution and use in source and binary forms, with or without 
 *  modification, are permitted provided that the following conditions 
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright 
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the 
 *    documentation and/or other materials provided with the   
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#include "RFID.h"
#include "hardware.h"
#include "SPI.h"
#include "API.h"
#include "tool.h"
#include "DebugPrint.h"
#include "buffer.h"
#include "msp430.h"
#include "NCI_API.h"
#include "MSP430_hardware.h"
#include "TRF7970A.h"

extern "C" {
#include "TargetAC.h"

}
#define MAXGUARDTIME 5
#define HAL_LF_T3T_MAX          (byte)0x01

bool RFID::isTargetActivated = false;
static byte selectedFrameType = 0xFF;
static byte selectedBaudrate = BAUDRATE_106;

static const indextype BuffSize = 256;
static Buffer<indextype, BuffSize> mailbox;

bool RFID::isRxMultiple = false;
bool RFID::isNFCActive = false;
bool RFID::isNFC_TargetMode = false;
bool RFID::isRFFieldOn = false;

byte RFID::current_frame_type = NO_TAG_MODE;
byte RFID::current_frame_param = 0;
byte RFID::bitFrame = 0;
byte RFID::stmReceiveMode = RCV_IDLE;

bool RFID::isTXfinish = false;

bool RFID::isProtocolError = false;

struct RFID::_AAC {
	byte sens_res[2];byte sel_res;byte prot_sel[2];byte Padding[8];byte sys_code[2];byte pupi[4];byte afi;byte crc_aid[2];byte number_apli;byte proto_info[4];
	byte lfT3tFlags[2];
	byte lfProtocolType;
	byte lfAdvFeat;
	byte lfConBitrF;
} RFID::AACconfig;

Array<10> RFID::nfcid1; //ISO/IEC 18092 11.2.1.26 NFCID1 contents and cascade levels
Array<8> RFID::nfcid2; //ISO/IEC 18092 11.2.2.4 NFCID2 contents

static word _PICCTimeout = 200;


TRF_Mode_t g_selected_mode = BOARD_INIT;
Frequency_t g_selected_frequency = FREQ_STAND_BY;

bool g_p2p_target_flag = false;
bool g_p2p_initiator_flag = false;

uint8_t g_fifo_buffer[255];
uint16_t g_fifo_bytes_received = 0;
volatile uint8_t g_irq_flag = 0x00;
volatile uint8_t g_time_out_flag = 0x00;


bool (*RFID::callbackFunction)() = &RFID::functionDummy;

bool RFID::functionDummy() {
	return false;
}
/**
 *<b>callback</b>
 *<p>
 * call function in NCI.cpp for protocol handling
 *</p>
 *@return returns TRUE or FALSE for aborting
 */
bool RFID::callback() {
	static Timer tCallback;

	if (tCallback.iselapsed()) {
		if (callbackFunction()) {
			return true;
		}
		tCallback.Start(100);
	}
	return false;
}


extern "C" unsigned char RF_Read(unsigned char adr) {
	byte ret;
	TRF797x_readSingle(&ret,adr);
	return ret;

}
extern "C" unsigned char RF_Write(unsigned char adr, unsigned char value) {
	TRF797x_writeSingle(value,adr);
	return 0;

}

extern "C" void RF_WriteCmd(unsigned char cmd) {
	RFID::writeCmd(cmd);
	return;
}

extern "C" u08_t RF_writeFIFO(u08_t *pbuf, u08_t crc_bit, u08_t length, u08_t broken_bits) {
	return TRF797x_writeFIFO(pbuf,(CRC_t)crc_bit,length,broken_bits);

}

extern "C" void RF_resetFIFO(void){
	return TRF797x_resetFIFO();
}

extern "C" void RF_WriteRaw(unsigned char *cmd, unsigned char len) {
	TRF797x_rawWrite(cmd, len);
	return;

}
extern "C" void RF_ReadCont(unsigned char adr, unsigned char *buf, unsigned char len) {
	TRF797x_readCont(buf,adr, len);
	return;

}

extern "C" void _printf(char *fmt, ...) {
	DebugPrint::Write(fmt);
	return;

}
extern "C" void RF_WriteMailBox(unsigned char *data, unsigned char len) {
	mailbox.Write(data, len);
	return;

}

extern "C" byte getSelectedFrameType() {
	return selectedFrameType;

}
extern "C" void setSelectedFrameType(unsigned char type) {

	selectedFrameType = type;

}

void RFID::writeCmd(byte cmd) {

	TRF797x_directCommand(cmd);
}
void RFID::reset() {
	//byte buffer[2];
	//RADIO_DIRECTCOMMAND(Reset, &buffer[0]);
}

void RFID::HFField(char state) {
	if (state == 0) {
		TRF797x_writeSingle(0x01,TRF79X0_CHIP_STATUS_CTRL_REG);
	} else {
		TRF797x_writeSingle(0x21,TRF79X0_CHIP_STATUS_CTRL_REG);
	}
}
void RFID::FieldReset(word msec) {
	HFField(0);
	waitms(msec);
	HFField(1);
}

/**
 *<b>Set some necessary timeouts</b>
 *<p>
 Set either PCD_GUARD_TIME, PICC_TIMEOUT, TRANSMISSIONTIMEOUT or RECEIVE_TIMEOUT. Depends on the parameter
 which is send with.
 *</p>
 *@see Hardware Abstraction Layer Version 1.16, 4.4 RFID_CONTROL
 *@param sendbuf contains the opcode which timeout should be set and the value
 *@param sendlen has to be 2 or 3 to be valid!
 *@return <b>ZERO</b> if no error occured. Else a error code according to API.h will be returned.
 */word RFID::setTimeouts(byte *sendbuf, word sendlen) {
	if (sendlen == 2) {
		Timing(sendbuf[0], sendbuf[1]); //type, value
		return OK;
	} else if (sendlen == 3) {
		Timing(sendbuf[0], (word) (sendbuf[2]) + (word) (sendbuf[1] << 8)); //type, value
		return OK;
	} else {
		return WRONG_LENGTH;
	}
}

//Sets a parameter to the given value
// type			parameter
// value		value for the parameter
void RFID::Timing(byte type, word value) {
	switch (type) {
	case PCD_GUARD_TIME:
		//_PCDGuardTime = value;
		break;
	case PICC_TIMEOUT:
		_PICCTimeout = value;
		break;
	case TRANSMISSIONTIMEOUT:
		//_TransmissionTimeout = value;
		break;
	case RECEIVE_TIMEOUT:
		//_ReceiveTimeout = value;
		break;
	}
}
/**
 *<b>Version</b>
 *<p>
 Returns the actual version of the RFID layer.
 *</p>
 *@param sendbuf NULL
 *@param sendlen has to be 0 to be valid.
 *@param recbuf contains the version string which will be returned to host.
 *@param reclen contains the length of the valid data in recbuf.
 *@return <b>Zero</b> for OK. Else a ErrorCode according to API.h will be returned.
 */word RFID::version(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen != 0) {
		reclen = 0;
		return WRONG_LENGTH;
	}
	if (reclen > RFID_VERSION_LENGTH)
		reclen = RFID_VERSION_LENGTH;
	if (reclen < RFID_VERSION_LENGTH) {
		reclen = 0;
		return DATA_OVERFLOW;
	}
	memcpy(recbuf, (byte *) RFID_VERSION_STR, RFID_VERSION_LENGTH);
	return OK;
}

word RFID::FrameType(byte option, byte param) {
	//FlushFIFO();
	//mailbox.flush();

	//if(current_frame_type == option && current_frame_param == param)    //for timing-optimisation. ToDo: start AAC() always if frame_type remains unchanged (NFC)
	//  return OK;                                                        //ToDo: NO_TAG_MODE has same value as LISTEN_ABF_PASSIVE

	current_frame_type = option;
	current_frame_param = param;

	switch (option) {
	case ISO14443A_BITFRAME:
		SetupFrame(ISO14443A_BYTEFRAME);
		Setup14443A_Bitframe(param);
		break;

	case ISO14443A_BYTEFRAME:
		SetupFrame(ISO14443A_BYTEFRAME);
		Setup14443A_Byteframe();
		break;

	case ISO14443B_BYTEFRAME:
		SetupFrame(ISO14443B_BYTEFRAME);
		break;

	case NFC_TARGET_PASSIVE:
		SetupFrame(param);
		break;

	case FELICA_BYTEFRAME:
		SetupFrame(FELICA_BYTEFRAME);
		break;
	case ISO15693_BYTEFRAME:
		SetupFrame(ISO15693_BYTEFRAME);
		break;
	case NO_TAG_MODE:
		SetupFrame(NO_TAG_MODE);
		break;

//   case GTML_BYTEFRAME:
//       SetupFrame(GTML_BYTEFRAME);
//       break;
//
//   case NFC_INITIATOR_ACTIVE:
//       SetupFrame(NFC_INITIATOR_ACTIVE);
//       break;
//
//   case NFC_TARGET_ACTIVE:
//       SetupFrame(NFC_TARGET_ACTIVE);
//       break;
//
//	case LISTEN_A_PASSIVE:
//		SetupFrame(LISTEN_A_PASSIVE);
//		break;
//	case LISTEN_B_PASSIVE:
//		SetupFrame(LISTEN_B_PASSIVE);
//		break;
//	case LISTEN_F_PASSIVE:
//		SetupFrame(LISTEN_F_PASSIVE);
//		break;

	default:
		//DebugPrint::Write("\nunknown FrameType %04x in RFID\n", option);
		return WRONG_PARAMETER;
	}

	return OK;
}

/**
 *<b>Setup Frame</b>
 *@param isNFC_TargetMode is a flag which is only true when initialized as Target (active and passive)
 *@param isNFCActive is a flag which is only be used to for set registers for higher baudrates
 *@param isNFC_Passive_Target was used at the transceive switch methode. Isn't used anymore, left here for tests
 */
void RFID::SetupFrame(byte mode) {
	isRxMultiple = false;
	isNFCActive = false; //Only set by NFC_INITIATOR_ACTIVE to set the baudrate register
	bitFrame = 0x00;
	stmReceiveMode = RCV_IDLE;

	//quick fix that only listen for TypeF in P2P mode (type A and Type F activated)
	if(mode==0x35){
		mode=0x34;
	}

	switch (mode) {
	case ISO14443A_BYTEFRAME:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		selectedFrameType = ISO14443A_BYTEFRAME;
		selectedBaudrate = BAUDRATE_106;
		TRF797x_Init(TYPE_A_RW, FREQ_106_KBPS);
		break;
	case ISO14443B_BYTEFRAME:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		selectedFrameType = ISO14443B_BYTEFRAME;
		selectedBaudrate = BAUDRATE_106;
		TRF797x_Init(TYPE_B_RW, FREQ_106_KBPS);
		break;
	case ISO15693_BYTEFRAME:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		selectedFrameType = ISO15693_BYTEFRAME;
		selectedBaudrate = BAUDRATE_26_48;
		TRF797x_Init(TYPE_V_RW, FREQ_106_KBPS);
		break;
	case FELICA_BYTEFRAME:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		selectedFrameType = FELICA_BYTEFRAME;
		selectedBaudrate = BAUDRATE_212;
		TRF797x_Init(TYPE_F_RW, FREQ_212_KBPS);
		break;
	case NFC_INITIATOR_ACTIVE:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		break;
	case NFC_TARGET_PASSIVE:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		break;
	case NFC_TARGET_ACTIVE:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		break;
	case LISTEN_A_PASSIVE:
		isNFC_TargetMode = true;
		isTargetActivated = false;
		isRFFieldOn = false;
		selectedFrameType = ISO14443A_BYTEFRAME;
		selectedBaudrate = BAUDRATE_106;
		TRF797x_Init(CARD_EMULATION_TYPE_A, FREQ_106_KBPS);
		mailbox.flush();

		break;
	case LISTEN_B_PASSIVE:
		isNFC_TargetMode = true;
		isTargetActivated = false;
		isRFFieldOn = false;
		selectedFrameType = ISO14443B_BYTEFRAME;
		selectedBaudrate = BAUDRATE_106;
		TRF797x_Init(CARD_EMULATION_TYPE_B, FREQ_106_KBPS);
		mailbox.flush();
		break;
	case LISTEN_F_PASSIVE:
		isNFC_TargetMode = true;
		isTargetActivated = false;
		selectedFrameType = FELICA_BYTEFRAME;
		selectedBaudrate = BAUDRATE_212;
		TRF797x_Init(P2P_PASSIVE_TARGET_MODE, FREQ_212_KBPS);
		mailbox.flush();
		break;
	case NO_TAG_MODE:
		isTargetActivated = false;
		isNFC_TargetMode = false;
		TRF797x_Init(BOARD_INIT, FREQ_STAND_BY);
		break;
	default:
		//DebugPrint::Write("\nUnknown Frametype");
		break;
	}

	return;
}
/**
 *<b>Setup 14443A-Bitframe</b>
 *<p>
 * Bitframe is used for REQA and Anticollision.
 *</p>
 *@param bitcount contains the total number of bits to send in a bitframe
 */
void RFID::Setup14443A_Bitframe(byte bitcount) {

	bitFrame = bitcount;
	TRF797x_writeSingle(0x88,TRF79X0_ISO_CONTROL_REG);
}

void RFID::Setup14443A_Byteframe() {

	bitFrame = 0x00;
}

void RFID::callbackInit(bool (*callback)()) {
	RFID::callbackFunction = callback;
}
/**
 *<b>RFID Control</b>
 *<p>
 * Todo: add description
 *</p>
 *@param sendbuf
 *@param sendlen
 *@param recbuf
 *@param reclen
 *@return
 */
unsigned short RFID::transceive(unsigned char cmd, unsigned char* sendbuf,
		unsigned short sendlen, unsigned char* recbuf,
		unsigned short & reclen) {

	static Timer txTimer;
	static Timer tCallback;
	static Timer timeout;
	IRQ_flag_t irq_status;
	unsigned short ret_value = 0;
	unsigned char broken_frame_bits = 0;
	// Flag used to switch the RX setting to receive without CRC, once the
	//command to write has been sent, and a response has been received - the
	// flag will be used to switch the setting back to receive with CRC
	bool mifare_ultra_light_flag = false;
	bool iso15693_flag = false;


	isTXfinish = false;

	if (isProtocolError == true) {
		isProtocolError = false;
		return RF_FRAMING_ERROR;
	}

	// Check if the packet is a broken Frame
	if(bitFrame < 8)
	{
		broken_frame_bits = bitFrame;
	}

	if(sendlen == 0x06 && sendbuf[0] == 0xA2)
	{
		mifare_ultra_light_flag = true;
		// Register 01h. Chip Status Control
		TRF797x_writeSingle(0x88, TRF79X0_ISO_CONTROL_REG);
	}

	if(g_selected_mode == TYPE_V_RW && sendbuf[0] & BIT6)
	{
		iso15693_flag = true;
	}

	byte ret = TRF797x_writeFIFO(sendbuf, (CRC_t) (
			bitFrame ? TRF79X0_TRANSMIT_NO_CRC_CMD | 0x80 : TRF79X0_TRANSMIT_CRC_CMD | 0x80), sendlen,broken_frame_bits);
	if (reclen == 0) {
		if (ret == STATUS_SUCCESS_T)
			ret_value = OK;
		else
			ret_value = RF_FRAMING_ERROR;
	} else {
		if(iso15693_flag == true)
		{
			waitms(15);
			TRF797x_directCommand(TRF79X0_TRANSMIT_NEXT_SLOT_CMD);
		}

		irq_status = TRF797x_irqHandler(_PICCTimeout);

		if (irq_status == IRQ_STATUS_RX_COMPLETE) {
			// Read the mailbox
			reclen = mailbox.used();
			mailbox.Read(recbuf, reclen);
			ret_value = OK;
		} else if (irq_status == IRQ_STATUS_ERROR) {
			reclen = 0;
			ret_value = RF_FRAMING_ERROR;
		} else {
			reclen = 0;
			ret_value = NO_ANSWER_FROM_TAG;
		}

		if(mifare_ultra_light_flag == true)
		{
			// Register 01h. Chip Status Control
			TRF797x_writeSingle(0x08, TRF79X0_ISO_CONTROL_REG);
		}
	}

	return ret_value;
}

unsigned char RFID::BitCollision() {

	return 0xFF;

}

/**
 *<b>Call Interface</b>
 *<p>
 * Todo: add description
 *</p>
 *@param opcode contains the Command code
 *@param sendbuf contains the reveived data from host.
 *@param sendlen contains the length of revieved data from host
 *@param recbuf contains the sending data to the host.
 *@param reclen contains the length of sending data to the host
 *@return
 */word RFID::Call(word opcode, byte *sendbuf, word sendlen, byte *recbuf,
		word &reclen) {

	switch (opcode) {
	case RFID_INIT:
		Init(sendbuf, sendlen, recbuf, reclen);
		return OK;
	case RFID_VERSION:
		return version(sendbuf, sendlen, recbuf, reclen);
	case RFID_FRAMETYPE: {
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		word ret = 0;
		ret = FrameType(sendbuf[0], sendbuf[1]);
		if (ret != OK) {
			reclen = 0;
			return ret;
		}
		/*if(reclen >= 1) {
		 reclen = 1;
		 recbuf[0]=param;
		 }*/
		return OK;
	}
	case RFID_TRANSCEIVE: {

		if ((sendlen == 0) && (reclen == 0)) {
			return GENERAL_ERROR;
		}

		if ((sendlen == 0) && (reclen != 0)) {

			return RFID::readMailbox(recbuf, reclen);

		} else {

//			if (sendlen != 6) {
//			DebugPrint::Write("\nsendbuf[%02x]: ", sendlen);
//			DebugPrint::Hex(sendbuf, sendlen);
//			}
			word ret = RFID::transceive(0, sendbuf, sendlen, recbuf, reclen);
			//DebugPrint::Write("\nBitCollsionPos: %02x \nrecbuf: ", BitCollision());
			//DebugPrint::Write("recbuf[%02x]: ", reclen);
			//DebugPrint::Hex(recbuf, reclen);
			return ret;

		}

	}
	case RFID_AUTHENTICATE:
		return UNKNOWN_COMMAND;
	case RFID_CONTROL:
		if (sendlen < 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		return RFID::control(sendbuf, sendlen, recbuf, reclen);
	case RFID_DISCOVERY:
		return modeDetection(recbuf, reclen);
	case RFID_RESET:
		return UNKNOWN_COMMAND;
	default:
		//printf("RFID: unknown command %x\n",opcode);
		reclen = 0;
		return UNKNOWN_COMMAND;
	}
}

/**
 *<b>Call</b>
 *<p>
 * Wrapper for RFID::Call
 *</p>
 *@param opcode contains the opcode to execute
 *@param s1 contains the only parameter sent via sendbuf. sendlen will be 1.
 *@return any errorcode from RFID::Call. If the call was OK and returned exactly one byte via recbuf, this byte is returned as lower byte.
 */word RFID::Call(word opcode, byte s1) {
	word rlen = 1;
	byte r[1] = { 0 };
	word ret = Call(opcode, &s1, 1, r, rlen);

//if no error occured and one byte is received, return this byte as lower byte
	if (ret == OK && rlen == 1) {
		return r[0] & 0x00FF;
	} else {
		return ret;
	}
}

/**
 *<b>Call</b>
 *<p>
 * Wrapper for RFID::Call
 *</p>
 *@param opcode contains the opcode to execute
 *@param s1 contains sendbuf[0].
 *@param s2 contains sendbuf[1]. If s2 is a word (higher byte is not 0x00) then s2 is sent as sendbuf[1] and sendbuf[2]
 *@return any errorcode from RFID::Call. If the call was OK and returned exactly one byte via recbuf, this byte is returned as lower byte.
 */word RFID::Call(word opcode, byte s1, word s2) {
	byte r[1] = { 0 };
	word rlen = 1;
	word ret;

	if (s2 & 0xFF00) {
		byte b[] = { s1, (s2 >> 8), (s2 & 0x00FF) };
		ret = Call(opcode, b, 3, r, rlen);
	} else {
		byte b[] = { s1, (byte) s2 };
		ret = Call(opcode, b, 2, r, rlen);
	}

//if no error occured and one byte is received, return this byte as lower byte
	if (ret == OK && rlen == 1) {
		return r[0] & 0x00FF;
	} else {
		return ret;
	}
}

unsigned short RFID::Init(unsigned char* sendbuf, unsigned short sendlen,
		unsigned char* recbuf, unsigned short & reclen) {

	TRF797x_setup();

	//default initialization for the AAC()
	/* NFCForum TS Digital Protocol 1.0 */

	AACconfig.sens_res[0] = 0x04; //4.6.3 SENS_RES Response, Byte1
	AACconfig.sens_res[1] = 0x04; //Byte2
	AACconfig.sel_res = 0x40; //4.8.2 SEL_RES Response
	AACconfig.Padding[0] = 0xFF; //PAD0 = see NFC FORUM TS Digital Protocol 1.0; Padding is set with dummy values for testing Typ3 communication
	AACconfig.Padding[1] = 0xFF; //PAD0
	AACconfig.Padding[2] = 0xFF; //PAD1
	AACconfig.Padding[3] = 0xFF; //PAD1
	AACconfig.Padding[4] = 0xFF; //PAD1
	AACconfig.Padding[5] = 0xFF; //MRTIcheck
	AACconfig.Padding[6] = 0xFF; //MRTIupadate
	AACconfig.Padding[7] = 0xFF; //PAD2
	AACconfig.sys_code[0] = 0xFF; //HCI default 0x12
	AACconfig.sys_code[1] = 0xFF; //HCI default 0xFC
	AACconfig.afi = 0x00;

	setSensRes(AACconfig.sens_res, 2);
	setSelRes(AACconfig.sel_res);
	setPadding(AACconfig.Padding, 8);
	setSysCode(AACconfig.sys_code, 2);
	setAFI(AACconfig.afi);

	nfcid1.data[0] = 0x08;
	nfcid1.data[1] = 0x12;
	nfcid1.data[2] = 0x34;
	nfcid1.data[3] = 0x56;
	nfcid1.len = 4;
	setNfcid1(nfcid1.data, nfcid1.len);

	for (int i = 0; i < 6; i++) {
		nfcid1.data[i + 4] = 0x00;
	}

	nfcid2.data[0] = 0x02; // protocol Byte1
	nfcid2.data[1] = 0xFE; // protocol Byte2
	nfcid2.data[2] = 0x12;
	nfcid2.data[3] = 0x34;
	nfcid2.data[4] = 0x56;
	nfcid2.data[5] = 0x78;
	nfcid2.data[6] = 0x90;
	nfcid2.data[7] = 0xAB;
	nfcid2.len = 8;

	setNfcid2(nfcid2.data, nfcid2.len);

	AACconfig.pupi[0] = 0x01;
	AACconfig.pupi[1] = 0x02;
	AACconfig.pupi[2] = 0x03;
	AACconfig.pupi[3] = 0x04;

	setPupi_(AACconfig.pupi, 4);

	AACconfig.crc_aid[0] = 0x00;
	AACconfig.crc_aid[1] = 0x00;
	AACconfig.number_apli = 0x00;

	setCRCAid(AACconfig.crc_aid, 2);
	setNumberApli(AACconfig.number_apli);

	AACconfig.proto_info[0] = 0x00;
	AACconfig.proto_info[1] = 0x00;
	AACconfig.proto_info[2] = 0x45; //protocol info byte NCI default 0x45 (FWI 0x04) (ADC FO 0x05)
	AACconfig.proto_info[3] = 0x00;

	setProtoInfo(AACconfig.proto_info, 4);

	return OK;
}

unsigned short RFID::baudrate_air(unsigned char baudrate) {

	byte ret;

	TRF797x_readSingle(&ret,TRF79X0_ISO_CONTROL_REG);

	if(g_selected_mode == P2P_INITATIOR_MODE ||
	   g_selected_mode == P2P_PASSIVE_TARGET_MODE ||
	   g_selected_mode == P2P_ACTIVE_TARGET_MODE)
	{
		// Check that the current baud rate, and the current register value are not the same
		if (baudrate == BAUDRATE_212 && (ret & 0x1F) != 0x1A)
		{
			ret |= 0x02;
			TRF797x_writeSingle(ret & 0xFD,TRF79X0_ISO_CONTROL_REG);
		}
		else if (baudrate == BAUDRATE_424 && (ret & 0x0F) != 0x1B)
		{
			ret |= 0x03;
			TRF797x_writeSingle(ret & 0xFF,TRF79X0_ISO_CONTROL_REG);
		}
	}
	else
	{
		// Check that the current baud rate, and the current register value are not the same
		if (baudrate == BAUDRATE_106 && (ret & 0x03) != 0x00)
		{
			TRF797x_writeSingle(ret & 0xFC,TRF79X0_ISO_CONTROL_REG);
		}
		else if (baudrate == BAUDRATE_212 && (ret & 0x03) != 0x01)
		{
			ret |= 0x01;
			TRF797x_writeSingle(ret & 0xFE,TRF79X0_ISO_CONTROL_REG);
		}
		else if (baudrate == BAUDRATE_424 && (ret & 0x03) != 0x02)
		{
			ret |= 0x02;
			TRF797x_writeSingle(ret & 0xFD,TRF79X0_ISO_CONTROL_REG);
		}
		else if (baudrate == BAUDRATE_848 && (ret & 0x03) != 0x03)
		{
			ret |= 0x03;
			TRF797x_writeSingle(ret & 0xFF,TRF79X0_ISO_CONTROL_REG);
		}
	}


	return OK;

}

word RFID::control(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	switch (sendbuf[0]) {
	case PCD_GUARD_TIME:
	case PICC_TIMEOUT:
	case TRANSMISSIONTIMEOUT:
	case RECEIVE_TIMEOUT:
		reclen = 0;
		return setTimeouts(sendbuf, sendlen);

	case REG_READ:
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (reclen != 1) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		TRF797x_readSingle(recbuf,sendbuf[1]);
		reclen = 1;
		return OK;

	case REG_WRITE:
		if (sendlen != 3) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		TRF797x_writeSingle(sendbuf[2], sendbuf[1]); //value, adr
		reclen = 0;
		return OK;

	case FIELD_RESET:
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		FieldReset(sendbuf[1]); //msec
		reclen = 0;
		return OK;

	case RCRESET:
		if (sendlen != 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		//CHECK(HW::Call(HW_RC_RESET));
		reclen = 0;
		return OK;

	case HF_FIELD:
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (sendbuf[1] & 0xFE) {
			reclen = 0;
			return WRONG_PARAMETER;
		} //must be 0x00 or 0x01
		HFField(sendbuf[1]); //state
		reclen = 0;
		return OK;

	case MODINDEX:
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		//ModIndex_value = sendbuf[1];
		//RFID::Write(ModGsPReg, ModIndex_value);
		reclen = 0;
		return OK;

	case MF_AUTHBIT_CLEAN: //clean the bit which turns the crypto-unit on
		if (sendlen != 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		//ClearBits(Status2Reg, 0x08); //Write(Status2Reg,0x00);
		reclen = 0;
		return OK;

	case POSSIBLE_BAUDRATE_AIR:
		if (sendlen != 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (reclen < 1) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		recbuf[0] = BAUDRATE_848;
		reclen = 1;
		return OK;

	case BAUDRATE_AIR: //change the airinterface speed
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		reclen = 0;
		return baudrate_air(sendbuf[1]);
		//return 0;

	case CHECK_RF: //check the external RFFieldl
		if (sendlen != 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (reclen < 1) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		recbuf[0] = isRFFieldOn;
		reclen = 1;
		return OK;

	case SET_RECEIVING: //put the PN512 into receive mode ->nfc mode
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (sendbuf[1] == 0x00) {
			//RFID::Write(CommandReg, PCD_IDLE);
		} //put the pn512 in the idle mode
		if (sendbuf[1] == 0x01) {
			//RFID::Write(CommandReg, PCD_RECEIVE);
		} //put the pn512 in the receive mode
		reclen = 0;
		return OK;

	case TARGET_AC_TEST:
		if (sendlen != 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		NFCTarget_init_light();
		reclen = 0;
		return OK;

	case POWER_SAVE:
		if (sendlen != 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		//RFID::HFField(0);
		//RFID::Write(CommandReg, 0x30);
		reclen = 0;
		return OK;

	case RX_GAIN:
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		//RFID::Write(RFCfgReg, (sendbuf[1] & 0x7f));
		//rcf_sensitivity = (sendbuf[1] & 0x7f);
		reclen = 0;
		return OK;

	case CRC_ENABLE: {
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (sendbuf[1] & 0xFE) {
			reclen = 0;
			return WRONG_PARAMETER;
		}
		//byte ret = RFID::Read(RxModeReg);
		if (sendbuf[1]) {
			bitFrame = false;
		} else {
			bitFrame = true;
		}
		reclen = 0;
		return OK;
	}

	case THRESHOLD:
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		//Treshold = sendbuf[1];
		//RFID::Write(RxTresholdReg, sendbuf[1]);
		reclen = 0;
		return OK;

	case BITCOLLISION:
		if (sendlen != 1) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (reclen < 1) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		recbuf[0] = BitCollision();
		reclen = 1;
		return OK;

	case GET_RANDOM_ID: {
		if (reclen < 10) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		uint8_t len = 0;
		//FlushFIFO();
		//RFID::Write(CommandReg, PCD_GenRandomID); //genarate a random ID
		//waitus(500);
		//SetBits(ControlReg, 0x20); //copy random ID into FIFO
		//while (Read(FIFOLevelReg) && (reclen > len)) {
		//	recbuf[len++] = Read(FIFODataReg);
		//}
		while (len < reclen){
			recbuf[len] = len;
			len++;
		}


		reclen = len;
		return OK;
	}

	case AAC_CONFIGURATION:
		return aacConfiguration(sendbuf, sendlen, recbuf, reclen);

	case CHECK_PON:
		//return checkPON(recbuf, reclen);
		return 0;

	case TARGET_ACTIVATED:
		return checkTargetActivated(recbuf, reclen);

	case RX_MULTIPLE:
		if (sendlen != 2) {
			reclen = 0;
			return WRONG_LENGTH;
		}
		if (sendbuf[1] == 0x01) {
			//SetBits(RxModeReg, 0x04);
			isRxMultiple = true;
		} else if (sendbuf[1] == 0x00) {
			//ClearBits(RxModeReg, 0x04);
			isRxMultiple = false;
		} else {
			reclen = 0;
			return WRONG_PARAMETER;
		}
		if (reclen >= 1) {
			//recbuf[0] = (isRxMultiple ? 0x01 : 0x00);
			reclen = 1;
		}
		return OK;

	default:
		reclen = 0;
		return UNKNOWN_COMMAND;
	}
}

/**
 * <b>AAC configuration</b>
 * <p>
 * This method dispatches the incomming data to several functions to configure the AAC().
 * </p>
 * @param sendbuf contains the from host received data. sendbuf[0]=sub-opcode 'AAC_CONFIGURATION' + sendbuf[1]=sub-sub-opcode 'SENS_RES' (for example) + sendbuf[i] = data
 * @param sendlen contains the length of received data
 * @param recbuf contains the data returned to host. At SENS_RES for example: 0x44, 0x00
 * @param reclen contains the length of data returned to host. At SENS_RES it will be 2.
 * @return <b>ZERO</b> if no error occured. Else a error code according to API.h will be returned.
 */word RFID::aacConfiguration(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	/* sendbuf+2/sendlen-2 to cut off the opcodes */
	switch (sendbuf[1]) {
	case SENS_RES: //r/w
		return sgSensRes(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case NFCID1: //r/w
		return sgNfcid1(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case SEL_RES: //r/w
		return sgSelRes(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case NFCID2: //r/w
		return sgNfcid2(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case SYS_CODE: //r/w
		return sgSysCode(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case AAC_PADDING: //r/w    // Felica PMM
		return sgPadding(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case PUPI_REG: //-/w
		return setPupi(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case AAC_ATQB: //r/w
		return sgAtqb(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case AAC_AFI: //r/w
		return sgAfi(sendbuf + 2, sendlen - 2, recbuf, reclen);

	case LF_T3T_MAX: //-/w
	      return getLfT3tMax(sendlen-2, recbuf, reclen);

	case LF_T3T_FLAGS: //r/w
		return sgLfT3tFlags(sendbuf+2, sendlen-2, recbuf, reclen);

	case LF_T3T_IDENTIFIERS_1:
	case LF_T3T_IDENTIFIERS_2:
	case LF_T3T_IDENTIFIERS_3:
	case LF_T3T_IDENTIFIERS_4:
	case LF_T3T_IDENTIFIERS_5:
	case LF_T3T_IDENTIFIERS_6:
	case LF_T3T_IDENTIFIERS_7:
	case LF_T3T_IDENTIFIERS_8:
	case LF_T3T_IDENTIFIERS_9:

	case LF_T3T_IDENTIFIERS_10:
	case LF_T3T_IDENTIFIERS_11:
	case LF_T3T_IDENTIFIERS_12:
	case LF_T3T_IDENTIFIERS_13:
	case LF_T3T_IDENTIFIERS_14:
	case LF_T3T_IDENTIFIERS_15:
	case LF_T3T_IDENTIFIERS_16:
		return sgLfT3tIdentifier(sendbuf+1, sendlen-1, recbuf, reclen);

	case LF_PROTOCOL_TYPE:
		return sgLfProtocolType(sendbuf+2, sendlen-2, recbuf, reclen);

	case LF_ADV_FEAT:
		return sgLfAdvFeat(sendbuf+2, sendlen-2, recbuf, reclen);

	case LF_CON_BITR_F:
		return sgLfConBitrF(sendbuf+2, sendlen-2, recbuf, reclen);

	case LN_NFC_DEP:
		return sgLnNfcDep(sendbuf+2, sendlen-2, recbuf, reclen);

	default:
		reclen = 0;
		return UNKNOWN_COMMAND;
	}
}

 /**
 * <b> Set Get LN_NFC_DEP </b>
 * <p>
 * Todo: add description
 * </p>
 * @see Hardware Abstraction Layer 1.34 4.4.18.4 LN_NFC_DEP
 * @param sendbuf
 * @param sendlen
 * @param recbuf
 * @param reclen
 * @return
 */
 word RFID::sgLnNfcDep(byte *sendbuf, word sendlen, byte *recbuf, word &reclen)
 {
   /* not implemented yet, is supposed to be like an T3T Identifier for the DH */
	  if(sendlen == 10)
	  {
	    memcpy(nfcid2.data, sendbuf + 2, 8);
	    nfcid2.len = 8;

	    setNfcid2(sendbuf + 2, 8);

	    memcpy(AACconfig.sys_code, sendbuf, 2);
	    reclen = 0;
	    return OK;
	  }
	  else if(sendlen == 0 && reclen >= 10)
	  {
	    memcpy(recbuf, AACconfig.sys_code, 2);
	    memcpy(recbuf + 2, nfcid2.data, 8);
	  }
	  else
	    return DATA_OVERFLOW;

	  return OK;

//   reclen = 0;
//   return OK;
 }

 /**
 * <b>Set Get LF_CON_BITR_F </b>
 * <p>
 * Todo: add description
 * </p>
 * @see Hardware Abstraction Layer Specification 1.34 4.4.18.13 LF_CON_BITR_F
 * @see NFCForum-TS-NCI-1.0 Table 35
 * @param sendbuf
 * @param sendlen
 * @param recbuf
 * @param reclen
 * @return
 */
 word RFID::sgLfConBitrF(byte *sendbuf, word sendlen, byte *recbuf, word &reclen)
 {
   if(sendlen == 1) {
     AACconfig.lfConBitrF = sendbuf[0];
     reclen = 0;
   }
   else if(sendlen == 0) {
     if(reclen == 0) return DATA_OVERFLOW;
     recbuf[0] = AACconfig.lfConBitrF;
     reclen = 1;
   }
   else { return WRONG_LENGTH; }
   return OK;
 }

 /**
 * <b> Set Get LF_ADV_FEAT </b>
 * <p>
 * Todo: add description
 * </p>
 * @see NFCForum-TS-NCI-1.0 Table 35
 * @param sendbuf
 * @param sendlen
 * @param recbuf
 * @param reclen
 * @return
 */
 word RFID::sgLfAdvFeat(byte *sendbuf, word sendlen, byte *recbuf, word &reclen)
 {
   if(sendlen == 1) {
     AACconfig.lfAdvFeat = sendbuf[0];
     reclen = 0;
   }
   else if(sendlen == 0) {
     if(reclen == 0) return DATA_OVERFLOW;
     recbuf[0] = AACconfig.lfAdvFeat;
     reclen = 1;
   }
   else { return WRONG_LENGTH; }
   return OK;
 }

 /**
 * <b> Set Get LF_PROTOCOL_TYPE </b>
 * <p>
 * Todo: add description
 * </p>
 * @param sendbuf
 * @param sendlen
 * @param recbuf
 * @param reclen
 * @return
 */
 word RFID::sgLfProtocolType(byte *sendbuf, word sendlen, byte *recbuf, word &reclen)
 {
   if(sendlen == 1) {
     AACconfig.lfProtocolType = sendbuf[0];
     if(AACconfig.lfProtocolType == 0x02)
     {
    	 TRF797x_setP2PTargetFlag(true);
     }
     else
    	 TRF797x_setP2PTargetFlag(false);
     reclen = 0;
     return OK;
   }
   else if(sendlen == 0) {
     if(reclen < 1) return DATA_OVERFLOW;
     recbuf[0] = AACconfig.lfProtocolType;
     reclen = 1;
     return OK;
   }
   return WRONG_LENGTH;
 }

 /**
 * <b> Set Get LF_T3T_IDENTIFIERS 1-16 </b>
 * <p>
 * Todo: add description
 * </p>
 * @param sendbuf
 * @param sendlen
 * @param recbuf
 * @param reclen
 * @return
 */
 word RFID::sgLfT3tIdentifier(byte *sendbuf, word sendlen, byte *recbuf, word &reclen)
 {
   //sendbuf[0] is the Identifiers ID (1-16)
   if(sendlen == 11) {
     memcpy(AACconfig.sys_code, sendbuf+1, 2);
     memcpy(nfcid2.data, sendbuf+3, 8);
     setNfcid2(sendbuf + 3, 8);
     return OK;
   }
   else if(sendlen == 1) {
     if(reclen < 10) return DATA_OVERFLOW;
     memcpy(recbuf, AACconfig.sys_code, 2);
     memcpy(recbuf+2, nfcid2.data, 8);
     return OK;
   }
   else {
     return WRONG_LENGTH;
   }
//   return GENERAL_ERROR;
 }


 /**
 * <b> Set Get LF_T3T_FLAGS </b>
 * <p>
 * Todo: add description
 * </p>
 * @param sendbuf
 * @param sendlen
 * @param recbuf
 * @param sendlen
 * @return
 */
 word RFID::sgLfT3tFlags(byte *sendbuf, word sendlen, byte *recbuf, word &reclen)
 {
   if(sendlen == 2) {
     AACconfig.lfT3tFlags[0] = sendbuf[0];
     AACconfig.lfT3tFlags[1] = sendbuf[1];
     reclen = 0;
   }
   else if((sendlen == 0) && (reclen >= 2)) {
     recbuf[0] = AACconfig.lfT3tFlags[0];
     recbuf[1] = AACconfig.lfT3tFlags[1];
     reclen = 2;
   }
   else {
     reclen = 0;
     return WRONG_LENGTH;
   }
   return OK;
 }

 /**
 * <b> Get LF_T3T_MAX </b>
 * <p>
 * Returns the value of LF_T3T_MAX. Specified at NFCForum-TS-NCI-1.0 6.1.8 Listen F Parameters
 * Table 35.
 * This register is read only and one byte data will be returned. Valid return values are 00h-0Fh.
 * </p>
 * @param sendlen contains the sendlen. SHALL be zero.
 * @param recbuf contains the returned data.
 * @param reclen is the maximum available recbuf size and will be changed to the returned length of data.
 * @return <b>OK</b> if no error occured. Else a error code according to 1.10 Error codes will be returned.
 */
 word RFID::getLfT3tMax(word sendlen, byte *recbuf, word &reclen)
 {
   if(sendlen != 0)
     return WRONG_LENGTH;

   if(reclen == 0)
     return DATA_OVERFLOW;

   recbuf[0] = HAL_LF_T3T_MAX;
   reclen = 1;
   return OK;
 }

/**
 *<b>Set/Get AFI</b>
 *<p>
 Todo: add description
 *</p>
 *@see ISO/IEC 14443-3 7.9 ATQB Response
 *@param sendbuf
 *@param sendlen
 *@param recbuf
 *@param reclen
 *@return
 */word RFID::sgAfi(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 1) {
		AACconfig.afi = sendbuf[0];
		setAFI(sendbuf[0]);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) {
		if (reclen < 1) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		reclen = 1;
		recbuf[0] = AACconfig.afi;
		return OK;
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

/**
 * <b>Set/Get PUPI</b>
 * <p>
 * Write and read PUPI register. With sendlen of 4 it is possible to write the pupi register. With
 * sendlen of 0 you can read the pupi register. At a read access the reclen has to be at minimum 4.
 * </p>
 * @see ISO/IEC 14443-3 7.9 ATQB Response
 * @param sendbuf contains the data which will be written in pupi register
 * @param sendlen contains the length of sendbuffer. Has to be 0 or 4.
 * @param recbuf contains the data of pupi req.
 * @param reclen contains the length of recbuffer
 * @return <b>ZERO</b> if no error occured. Else a error code according to API.h will be returned.
 */word RFID::setPupi(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 4) {
		memcpy(AACconfig.pupi, sendbuf, 4);
		setPupi_(sendbuf, 4);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) {
		if (reclen < 4) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		memcpy(recbuf, AACconfig.pupi, 4);
		reclen = 4;
		return OK;
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

/**
 * <b>Set/Get ATQB</b>
 * <p>
 * Write and read the ATQB register. A write command has to be 6 or 7 byte long. Depends if extended
 * ATQB or normal is used. The parameters are written in the order:
 * - CRC-AID: 2 byte. Default: '00 00'
 * - NUMBER_APLI: 1 byte. Default: '00'
 * - PROTO_INFO: 3-4 byte. Default: '00 00 E4'
 * With a sendlen of 0 and a reclen greater or equal 7 you can read the current settings of ATQB.
 * </p>
 * @see Kronegger HAL Specification 1.23 4.4.18.10 AAC_ATQB
 * @see ISO/IEC 14443-3 7.9 ATQB Response
 * @param sendbuf
 * @param sendlen
 * @param recbuf
 * @param reclen
 * @return
 */word RFID::sgAtqb(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 6 || sendlen == 7) {
		memcpy(AACconfig.crc_aid, sendbuf, 2);
		AACconfig.number_apli = sendbuf[2];
		memcpy(AACconfig.proto_info, sendbuf + 3, sendlen - 3);
		setCRCAid(sendbuf, 2);
		setNumberApli(sendbuf[2]);
		setProtoInfo(sendbuf + 3, sendlen - 3);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) {
		if (reclen < 7) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		reclen = 7;
		memcpy(recbuf, AACconfig.crc_aid, 2);
		recbuf[2] = AACconfig.number_apli;
		memcpy(recbuf + 3, AACconfig.proto_info, 4);
		return OK;
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

/**
 *<b>Set/Get SYS_CODE</b>
 *<p>
 Todo: add description
 *</p>
 *@param sendbuf
 *@param sendlen
 *@param recbuf
 *@param reclen
 *@return
 */word RFID::sgSysCode(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 2) { //write
		memcpy(AACconfig.sys_code, sendbuf, 2);
		setSysCode(sendbuf, 2);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) { //read
		if (reclen >= 2) {
			memcpy(recbuf, AACconfig.sys_code, 2);
			reclen = 2;
			return OK;
		} else {
			reclen = 0;
			return DATA_OVERFLOW;
		}
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

/**
 * <b>Set/Get NFCID2</b>
 * <p>
 * Write and read the NFCID2. The NFCID2 is used for passive communication with 212 and 424kbaud.
 * The NFCID2 can be written in two different ways. With a sendlen of 06h the first two bytes of
 * the NFCID2 are fixed to 02h FEh and the six bytes are attached.
 * With a sendlen of 08h also the first two bytes are written.
 * To read the NFCID2 the sendlen has to be 00h and the reclen at minimum 08h.
 * </p>
 * @see ISO/IEC 18092 11.2.2.4 NFCID2 contents
 * @param sendbuf contains the NFCID2 with or without prefix.
 * @param sendlen contains the length of sendbuf. Allowed length: 0, 6 and 8.
 * @param recbuf contains the current setting of the NFCID2
 * @param reclen contains the length of recbuf. At minimum 8.
 * @return <b>ZERO</b> if no error occured. Else a error code according to API.h will be returned.
 */word RFID::sgNfcid2(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 6) { //write
		nfcid2.data[0] = 0x02;
		nfcid2.data[1] = 0xFE;
		memcpy(nfcid2.data + 2, sendbuf, sendlen);
		nfcid2.len = 8;
		setNfcid2(nfcid2.data, 8);
		reclen = 0;
		return OK;
	} else if (sendlen == 8) {
		memcpy(nfcid2.data, sendbuf, sendlen);
		nfcid2.len = 8;
		setNfcid2(nfcid2.data, 8);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) { //read
		if (reclen >= 8) {
			memcpy(recbuf, nfcid2.data, nfcid2.len);
			reclen = nfcid2.len;
			return OK;
		} else {
			reclen = 0;
			return DATA_OVERFLOW;
		}
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

/**
 *<b>Set/Get SEL_RES/SAK</b>
 *<p>
 Todo: add description
 *</p>
 *@param sendbuf
 *@param sendlen
 *@param recbuf
 *@param reclen
 *@return
 */word RFID::sgSelRes(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 1) { //write
		AACconfig.sel_res = sendbuf[0];
		setSelRes(sendbuf[0]);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) { //read
		if (reclen >= 1) {
			recbuf[0] = AACconfig.sel_res;
			reclen = 1;
			return OK;
		} else {
			reclen = 0;
			return DATA_OVERFLOW;
		}
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

/**
 * <b>Set/Get NFCID1</b>
 * <p>
 * With this opcode the NFCID1 for Listen Modes can be configured. It is possible to write 3 or 4 bytes
 * NFCID. If you write only 3 bytes the first one of the four byte NFCID is fixed to 0x08 which indicates
 * a random generated UID. If you write 4 bytes, the first byte is overwritten aswell.
 * If the sendlen is zero you can read the current NFCID. For this reclen as to be at minimum 4.
 * </p>
 * @see NFCForum-TS-DigitalProtocol Requirements 17: NFCID1 Length
 * @param sendbuf contains the NFCID1 which will be written. Three or four bytes.
 * @param sendlen contains the length of sendbuffer. Has to be 0, 3 or 4.
 * @param recbuf contains the current NFCID1 settings.
 * @param reclen contains the length of recbuffer and has to be at minimum 4.
 * @return <b>ZERO</b> if no error occured. Else a error code according to API.h will be returned.
 */word RFID::sgNfcid1(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 3) {
		nfcid1.data[0] = 0x08;
		memcpy(nfcid1.data + 1, sendbuf, sendlen);
		nfcid1.len = 4;
		setNfcid1(nfcid1.data, 4);
//		DebugPrint::Write("\nsetID:");
//		DebugPrint::Hex(nfcid1.data, 4);
		reclen = 0;
		return OK;
	} else if (sendlen == 4) {
		memcpy(nfcid1.data, sendbuf, sendlen);
		nfcid1.len = 4;
		setNfcid1(nfcid1.data, 4);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) {
		if (reclen >= nfcid1.len) {
			memcpy(recbuf, nfcid1.data, nfcid1.len);
			reclen = nfcid1.len;
			return OK;
		} else {
			reclen = 0;
			return DATA_OVERFLOW;
		} //reclen to short
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

/**
 *<b>Set/Get SENS_RES</b>
 *<p>
 Todo: add description
 *</p>
 *@param sendbuf
 *@param sendlen
 *@param recbuf
 *@param reclen
 *@return
 */word RFID::sgSensRes(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) {
	if (sendlen == 2) { //write
		memcpy(AACconfig.sens_res, sendbuf, 2);
		setSensRes(sendbuf, 2);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) { //read
		if (reclen >= 2) {
			recbuf[0] = AACconfig.sens_res[0];
			recbuf[1] = AACconfig.sens_res[1];
			reclen = 2;
			return OK;
		} else {
			reclen = 0;
			return DATA_OVERFLOW;
		} //reclen to short
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}
/**
 *<b>Set/Get Padding</b>
 *<p>
 Todo: add description
 *</p>
 *@see ISO/IEC 14443-3 7.9 ATQB Response
 *@param sendbuf
 *@param sendlen
 *@param recbuf
 *@param reclen
 *@return
 */word RFID::sgPadding(byte *sendbuf, word sendlen, byte *recbuf, word &reclen) //PMM
		{
	if (sendlen == 8) {
		memcpy(AACconfig.Padding, sendbuf, 8);
		setPadding(sendbuf, 8);
		reclen = 0;
		return OK;
	} else if (sendlen == 0) {
		if (reclen < 8) {
			reclen = 0;
			return DATA_OVERFLOW;
		}
		reclen = 8;
		memcpy(recbuf, AACconfig.Padding, 8);
		return OK;
	} else {
		reclen = 0;
		return WRONG_LENGTH;
	}
}

int RFID::run() {
	if ((isNFC_TargetMode == true)) {
		TRF797x_irqHandler(0);
	}
	return OK;
}

extern "C" unsigned char RF_AAC() {

	TRF797x_Init(g_selected_mode, g_selected_frequency);
	mailbox.flush();
	return 0;
}

/**
 *<b>Read Mailbox</b>
 *<p>
 * Before checking the mailbox run once rfid::run when using NFC-TargetMode resp. transceive when using Initiator-mode. If data is
 * available it will be written into recbuf.
 *</p>
 *@see API.h
 *@param recbuf will contains the received data from target
 *@param reclen will contain the length of the valid data in recbuf
 *@param recbuf contains the answer from target if received.
 *@param reclen contains the length of valid data in recbuf
 *@return OK on success
 *        MORE_DATA\RF_FRAMING_ERR_MORE_DATA if isRxMultiple is true and more frames are available
 *        RF_FRAMING_ERROR on CRC, Parity error, etc.
 */word RFID::readMailbox(byte *recbuf, word &reclen) {
	word len;

	if (isNFC_TargetMode) {

		if (isProtocolError) {
			reclen = 0;
			isRFFieldOn = true;
			isProtocolError = false;
			return RF_FRAMING_ERROR;
		}
		if(isRFFieldOn==false){
			reclen = 0;
			isRFFieldOn = true;
			isProtocolError = false;
			return RF_FRAMING_ERROR;
		}

		len = mailbox.used(); //get how many bytes are in the mailbox buffer
		if (len == 0) {
			reclen = 0;
			return NO_ANSWER_FROM_TAG;
		}
		if (len > reclen) { //more data in buffer than expected. Reclen too short!
			mailbox.flush();
			reclen = 0;
			return DATA_OVERFLOW;
		}

		//len-=2;
		reclen = 0;
		reclen = mailbox.Read(recbuf, len);
		mailbox.flush();
		return OK;
	}
	else
		return GENERAL_ERROR;

}

/**
 *<b>Check Target Activated</b>
 *<p>
 Will return '1' if the Select command or if the Polling command was answered.
 *</p>
 *@param recbuf contains the indicating byte. 0x01 -> got selected, 0x00-> not
 *@param reclen has to be at minimum 1 and will return 1 byte.
 *@return <b>ZERO</b> if no error occured. Else a error code according to API.h will be returned.
 */
unsigned short RFID::checkTargetActivated(unsigned char* recbuf, unsigned short & reclen) {

	if (reclen < 1) {
		reclen = 0;
		return DATA_OVERFLOW;
	}

	RFID::isTargetActivated = isTagSelected();
	if (isTargetActivated) {
		recbuf[0] = 0x01;
		//DebugPrint::Write("\nTargetActivated");
	} else {
		recbuf[0] = 0x00;
		//DebugPrint::Write("\nnoTargetActivated");
	}

	reclen = 1;
	return OK;

}
unsigned short RFID::modeDetection(byte* recbuf, word & reclen) {

	recbuf[0] = selectedFrameType;
	recbuf[1] = selectedBaudrate;
	reclen = 2;
	return OK;

}


void TRF797x_setup(void)
{
	RFSPI::init();

	waitms(5);

	// Initialize Radio GPIOs
	TRF_DISABLE();
	TRF_ENABLESET();
	TRF_ENABLE();

	//
	// Enable an interrupt Low to High transition for the IRQ pin on the TRF7970A
	//
	IRQ_PIN_SET;
	IRQ_INT_ON;
	IRQ_EDGE_SET;
	IRQ_CLR;

	TRF797x_Init(BOARD_INIT, FREQ_STAND_BY);
}

IRQ_flag_t TRF797x_irqHandler(word timeout_ms) {
	IRQ_flag_t irq_status = IRQ_STATUS_IDLE;
	uint8_t irq_buffer[2] = {0,0};
	uint8_t target_protocol[2] = {0,0};
	uint8_t fifo_status_length = 0;
	uint8_t fifo_index = 0;
	static Timer txTimer;
	bool RFID_mode_enabled = false;
	bool NFC_mode_enabled = false;

	word elapsed_time_ms = 0;
	word timer_delay_value = 0;

	volatile uint8_t x;

	if (IRQ_IS_SET()) //is important
		g_irq_flag = 0x01;
	else {
		g_irq_flag = 0x00;
	}

	//
	// Check if the IRQ flag has been set - else initialize a 10 mS time out and go into LMP0
	//
	while (g_irq_flag == 0x00 && elapsed_time_ms != timeout_ms) {


		if(txTimer.iselapsed())
		{
			if (RFID::callbackFunction())
			{
				__no_operation();
				break;
			}
			elapsed_time_ms += timer_delay_value;
			if((timeout_ms - elapsed_time_ms) > 100)
			{
				timer_delay_value = 100;
			}
			else if(timeout_ms != elapsed_time_ms)
				timer_delay_value = timeout_ms - elapsed_time_ms;
			txTimer.Start(timer_delay_value);
		}
	}


	// Check what mode is enabled
	if (g_selected_mode == P2P_PASSIVE_TARGET_MODE
			|| g_selected_mode == P2P_ACTIVE_TARGET_MODE
			|| g_selected_mode == CARD_EMULATION_TYPE_A
			|| g_selected_mode == CARD_EMULATION_TYPE_B)
	{
		NFC_mode_enabled = true;

	}else if( g_selected_mode == TYPE_A_RW
			|| g_selected_mode == TYPE_B_RW
			|| g_selected_mode == TYPE_F_RW
			|| g_selected_mode == TYPE_V_RW)
	{
		RFID_mode_enabled = true;
	}

//
// Disable Timer
//
	txTimer.Stop();
	
	if (g_irq_flag == 0 && timeout_ms > 0) {
		irq_status = IRQ_STATUS_TIME_OUT;
	} else if (g_irq_flag == 0 && timeout_ms == 0)
		irq_status = IRQ_STATUS_IDLE;
	else {
		// Note: readCont() works properly with Type-B, however Type-A Card Emulation,
		// the NFC Target Protocol does not get updated with the correct protocol when receiving the second Select (0x93 0x70)
		if(NFC_mode_enabled)
			RFSPI::read(TRF79X0_NFC_TARGET_PROTOCOL_REG, target_protocol);

		//
		// Read IRQ Register
		//
		RFSPI::readCont(TRF79X0_IRQ_STATUS_REG, irq_buffer, 2);
		if (irq_buffer[0] & IRQ_STATUS_FIFO_HIGH_OR_LOW) {
			if (irq_buffer[0] & IRQ_STATUS_RX_COMPLETE) {
				g_fifo_bytes_received = 0;
				//
				// Read the FIFO status and FIFO into g_nfc_buffer
				//
				RFSPI::read(TRF79X0_FIFO_STATUS_REG, &fifo_status_length);

				fifo_index = 0;

				while (fifo_status_length > 0 && g_fifo_bytes_received < 255 &&
						irq_status == IRQ_STATUS_IDLE) {

					// Update the received bytes
					g_fifo_bytes_received += fifo_status_length;

					if(g_fifo_bytes_received > 255)
					{
						// Reset
						TRF797x_Init(g_selected_mode, g_selected_frequency);
						irq_status = IRQ_STATUS_ERROR;
					}
					else
					{
						//
						// Read the FIFO Data
						//
						RFSPI::readCont(TRF79X0_FIFO_REG,
								&g_fifo_buffer[fifo_index], fifo_status_length);

						// Once the data has been read check if the IRQ buffer was completed
						if(irq_buffer[0] == IRQ_STATUS_RX_COMPLETE)
						{
							// Type F - P2P Workaround
							if((g_selected_mode == P2P_PASSIVE_TARGET_MODE || g_selected_mode == TYPE_F_RW) && g_fifo_buffer[0] != g_fifo_bytes_received)
							{
								// Do nothing if for P2P we have not finished receiving complete payload.
							}
							else
							{
								irq_status = IRQ_STATUS_RX_COMPLETE;
								break;
							}
						} else if(! (irq_buffer[0] & IRQ_STATUS_RX_COMPLETE))
						{
							// In any other case where RX_COMPLETE was not set
							irq_status = IRQ_STATUS_ERROR;
							break;
						}

						// Update fifo_index
						fifo_index = fifo_index + fifo_status_length;

						if (!IRQ_IS_SET())
							g_irq_flag = 0;

						// Type F - P2P Workaround
						if(g_selected_mode == P2P_PASSIVE_TARGET_MODE || g_selected_mode == TYPE_F_RW)
						{

							// Check if we have received all the bytes defined in the first packet.
							if(g_fifo_buffer[0] == g_fifo_bytes_received)
							{
								irq_status = IRQ_STATUS_RX_COMPLETE;
								break;
							}
							// If we have not read all the bytes, then every 1 mS go read out the FIFO status register to ensure we do not get an overflow flag.
							else
							{
								// Start a timer with 1 mS
								txTimer.Start(1);

								while(g_irq_flag == 0 && !txTimer.iselapsed())
								{
									// Do nothing
								}

								txTimer.Stop();
							}

						}
						else
						{
							while (g_irq_flag == 0) {
								//
								// Enable Low Power Mode 0
								//
								//__bis_SR_register(LPM0_bits);
								if (RFID::callbackFunction()) {
									__no_operation();
									irq_status = IRQ_STATUS_TIME_OUT;
									break;
								}
							}
						}

						RFSPI::readCont(TRF79X0_IRQ_STATUS_REG, irq_buffer, 2);

						//
						// Read the FIFO status and FIFO into g_nfc_buffer
						//
						RFSPI::read(TRF79X0_FIFO_STATUS_REG,
								&fifo_status_length);

						// Mask off the lower 7 bits.
						fifo_status_length &= 0x7F;
					}
				}

				if(irq_status == IRQ_STATUS_RX_COMPLETE)
				{
					if (isTagSelected() == true || g_selected_mode == TYPE_A_RW || g_selected_mode == TYPE_B_RW || g_selected_mode == TYPE_F_RW || g_selected_mode == TYPE_V_RW) {
						RF_WriteMailBox(g_fifo_buffer,g_fifo_bytes_received);
					}
				}

			} else if (irq_buffer[0] & IRQ_STATUS_TX_COMPLETE) {
				irq_status = IRQ_STATUS_FIFO_HIGH_OR_LOW;
			}
		} else if (irq_buffer[0] == IRQ_STATUS_RX_COMPLETE) {

			RFID::isRFFieldOn = true;

			//
			// Read the FIFO status and FIFO into g_nfc_buffer
			//
			RFSPI::read(TRF79X0_FIFO_STATUS_REG, &fifo_status_length);

			if ((target_protocol[0] == 0xC9
					&& g_selected_mode == CARD_EMULATION_TYPE_A)
					|| (target_protocol[0] == 0xC5
							&& g_selected_mode == CARD_EMULATION_TYPE_B)
					|| (target_protocol[0] == 0xD2
							&& g_selected_mode == P2P_PASSIVE_TARGET_MODE
							&& g_selected_frequency == FREQ_212_KBPS)
					|| (target_protocol[0] == 0xD3
							&& g_selected_mode == P2P_PASSIVE_TARGET_MODE
							&& g_selected_frequency == FREQ_424_KBPS)
					|| (target_protocol[0] == 0xD2
							&& g_selected_mode == P2P_ACTIVE_TARGET_MODE
							&& g_selected_frequency == FREQ_212_KBPS)
					|| (target_protocol[0] == 0xD3
							&& g_selected_mode == P2P_ACTIVE_TARGET_MODE
							&& g_selected_frequency == FREQ_424_KBPS)
					|| (target_protocol[0] == 0xD2
							&& g_selected_mode == P2P_INITATIOR_MODE
							&& g_selected_frequency == FREQ_212_KBPS)
					|| (target_protocol[0] == 0xD3
							&& g_selected_mode == P2P_INITATIOR_MODE
							&& g_selected_frequency == FREQ_424_KBPS)
					|| g_selected_mode == TYPE_A_RW
					|| g_selected_mode == TYPE_B_RW
					|| g_selected_mode == TYPE_F_RW
					|| g_selected_mode == TYPE_V_RW)  {

				irq_status = IRQ_STATUS_RX_COMPLETE;

				if (fifo_status_length > 0) {
					//
					// Read the FIFO Data
					//
					RFSPI::readCont(TRF79X0_FIFO_REG, g_fifo_buffer,
							fifo_status_length);

					g_fifo_bytes_received = fifo_status_length;
				}

				if(g_fifo_bytes_received == 0x01 && g_fifo_buffer[0] == 0x52)
				{
					__no_operation();
				}

				if (isTagSelected() == true
					|| g_selected_mode == TYPE_A_RW
					|| g_selected_mode == TYPE_B_RW
					|| g_selected_mode == TYPE_F_RW
					|| g_selected_mode == TYPE_V_RW) {
					RF_WriteMailBox(g_fifo_buffer, g_fifo_bytes_received);
				}
			} else {
				TRF797x_Init(g_selected_mode, g_selected_frequency);
			}

		} else if (irq_buffer[0] & IRQ_STATUS_RX_COMPLETE) {
			// DEBUG
			// Read the FIFO status and FIFO into g_nfc_buffer
			RFSPI::read(TRF79X0_FIFO_STATUS_REG, &fifo_status_length);

			if((target_protocol[0] & 0xC0) != 0xC0 || irq_buffer[0] & IRQ_STATUS_NFC_PROTOCOL_ERROR)
				TRF797x_Init(g_selected_mode, g_selected_frequency);
		} else if (irq_buffer[0] == IRQ_STATUS_TX_COMPLETE) {
			irq_status = IRQ_STATUS_TX_COMPLETE;
			TRF797x_resetFIFO();
			RFID::isTXfinish = true;
		}
		// Handle NFC remaining cases
		else if(NFC_mode_enabled)
		{
			if (irq_buffer[0] & IRQ_STATUS_NFC_COLLISION_ERROR ||
				irq_buffer[0] & IRQ_STATUS_NFC_PROTOCOL_ERROR) {
				if(isTagSelected())
					RFID::isProtocolError = true;
				TRF797x_Init(g_selected_mode, g_selected_frequency);
				irq_status = IRQ_STATUS_ERROR;
			}
			else if (irq_buffer[0] & IRQ_STATUS_NFC_RF_FIELD_CHANGE) {
				irq_status = IRQ_STATUS_NO_RESPONSE;
				if ((target_protocol[0] & 0xC0) != 0xC0 && target_protocol[0] != 0x00) {
					// In the case we get this in the middle of a transfer
					TRF797x_Init(g_selected_mode, g_selected_frequency);
				}

			}
		}
		// Handle RFID remaining cases
		else if(RFID_mode_enabled)
		{
			if (irq_buffer[0] & IRQ_STATUS_RFID_COLLISION_ERROR ||
				irq_buffer[0] & IRQ_STATUS_RFID_FRAMING_OR_EOF_ERROR ||
				irq_buffer[0] & IRQ_STATUS_RFID_PARITY_ERROR ||
				irq_buffer[0] & IRQ_STATUS_RFID_CRC_ERROR) {
				TRF797x_Init(g_selected_mode, g_selected_frequency);
				irq_status = IRQ_STATUS_ERROR;
			} else if (irq_buffer[0] == IRQ_STATUS_RFID_NO_RESPONSE_INT)
			{
				irq_status = IRQ_STATUS_NO_RESPONSE;
			}
		}

	}

	// RF Field for Target Modes
	if (NFC_mode_enabled && irq_status != IRQ_STATUS_IDLE && irq_status != IRQ_STATUS_TIME_OUT) {
		if ((target_protocol[0] & 0xC0) == 0xC0) {
			RFID::isRFFieldOn = true;
			GPIO::setExtField(true);
		} else if(RFID::isRFFieldOn){
			RFID::isRFFieldOn = false;
			RFID::isTargetActivated = false;
			GPIO::setExtField(false);
			NFCTarget_init_light();
		}
	}
	else if(NFC_mode_enabled == false)
	{
			RFID::isRFFieldOn = false;
			RFID::isTargetActivated = false;
			GPIO::setExtField(false);
			NFCTarget_init_light();
	}

// Reset Global Flags
	g_irq_flag = 0x00;
	g_time_out_flag = 0x00;

	return irq_status;
}

//===============================================================
// NAME: void Trf796xDirectCommand (uint8_t *pbuf)
//
// BRIEF: Is used to transmit a Direct Command to TRF796x.
//
// INPUTS:
//    Parameters:
//        uint8_t        *pbuf        Direct Command
//
// OUTPUTS:
//
// PROCESS:    [1] transmit Direct Command
//
// CHANGE:
// DATE          WHO    DETAIL
// 24Nov2010    RP    Original Code
//===============================================================

void TRF797x_directCommand(uint8_t command) {
	uint8_t tx_buffer[2];

	if (SPIMODE) // SPI mode given by jumper setting
	{
		if (command == TRF79X0_RESET_FIFO_CMD) {
			//
			// Reset FIFO CMD + Dummy byte
			//
			tx_buffer[0] = TRF79X0_RESET_FIFO_CMD | BIT7;
			tx_buffer[1] = 0x00;
			RFSPI::rawWrite(tx_buffer, 2);
		} else {
			tx_buffer[0] = command | BIT7;
			RFSPI::rawWrite(tx_buffer, 1);
		}
	}

}

//===============================================================
// NAME: void TRF797xRawWrite (uint8_t *pbuf, uint8_t length)
//
// BRIEF: Is used to write direct to the TRF797x.
//
// INPUTS:
//	Parameters:
//		uint8_t		*pbuf		raw data
//		uint8_t		length		number of data bytes
//
// OUTPUTS:
//
// PROCESS:	[1] send raw data to TRF797x
//
// CHANGE:
// DATE  		WHO	DETAIL
// 24Nov2010	RP	Original Code
//===============================================================

void TRF797x_rawWrite(uint8_t *pbuf, uint8_t length) {
	if (SPIMODE) // SPI mode given by jumper setting
	{
		RFSPI::rawWrite(pbuf, length);
	} else // parallel mode
	{
//		ParallelRawWrite(pbuf, length);
	}
}

//===============================================================
// NAME: void TRF797xReadSingle (uint8_t *pbuf, uint8_t number)
//
// BRIEF: Is used to read specified TRF797x registers.
//
// INPUTS:
//	Parameters:
//		uint8_t		*pbuf		addresses of the registers
//		uint8_t		number		number of the registers
//
// OUTPUTS:
//
// PROCESS:	[1] read registers
//			[2] write contents to *pbuf
//
// CHANGE:
// DATE  		WHO	DETAIL
// 24Nov2010	RP	Original Code
//===============================================================

void TRF797x_readSingle(uint8_t *pbuf, uint8_t reg) {
	if (SPIMODE) // SPI mode given by jumper setting
	{
		RFSPI::read(reg, pbuf);
	} else // parallel mode
	{
//		ParallelReadSingle(pbuf, number);
	}
}

//===============================================================
// NAME: void TRF797xReadCont (uint8_t *pbuf, uint8_t length)
//
// BRIEF: Is used to read a specified number of TRF797x registers
// from a specified address upwards.
//
// INPUTS:
//	Parameters:
//		uint8_t		*pbuf		address of first register
//		uint8_t		length		number of registers
//
// OUTPUTS:
//
// PROCESS:	[1] read registers
//			[2] write contents to *pbuf
//
// CHANGE:
// DATE  		WHO	DETAIL
// 24Nov2010	RP	Original Code
//===============================================================

void TRF797x_readCont(uint8_t *pbuf, uint8_t reg, uint8_t length) {
	if (SPIMODE) // SPI mode given by jumper setting
	{
		RFSPI::readCont(reg, pbuf, length);
	} else // parallel mode
	{
//		ParallelReadCont(pbuf, length);
	}
}

//===============================================================
// NAME: void TRF797xWriteSingle (uint8_t *pbuf, uint8_t length)
//
// BRIEF: Is used to write to specified reader chip registers.
//
// INPUTS:
//	uint8_t	*pbuf	addresses of the registers followed by the
//					contents to write
//	uint8_t	length	number of registers * 2
//
// OUTPUTS:
//
// PROCESS:	[1] write to the registers
//
// CHANGE:
// DATE  		WHO	DETAIL
// 24Nov2010	RP	Original Code
//===============================================================

void TRF797x_writeSingle(uint8_t data, uint8_t reg) {
	if (SPIMODE) // SPI mode given by jumper setting
	{
		RFSPI::write(reg, &data);
	} else // parallel mode
	{
//		ParallelWriteSingle(pbuf, length);
	}
}

//===============================================================
// NAME: void SpiWriteCont (uint8_t *pbuf, uint8_t length)
//
// BRIEF: Is used to write to a specific number of reader chip
// registers from a specific address upwards.
//
// INPUTS:
//	uint8_t	*pbuf	address of first register followed by the
//					contents to write
//	uint8_t	length	number of registers + 1
//
// OUTPUTS:
//
// PROCESS:	[1] write to the registers
//
// CHANGE:
// DATE  		WHO	DETAIL
//===============================================================

void TRF797x_writeCont(uint8_t *pbuf, uint8_t reg, uint8_t length) {
	if (SPIMODE) // SPI mode given by jumper setting
	{
		RFSPI::writeCont(reg, pbuf, length);
	} else // parallel mode
	{
//		ParallelWriteCont(pbuf, length);
	}
}

void TRF797x_resetFIFO(void) {
	uint8_t tx_buffer[2];

	//
	// Reset FIFO CMD + Dummy byte
	//
	tx_buffer[0] = TRF79X0_RESET_FIFO_CMD | BIT7;
	tx_buffer[1] = 0x00;
	if (SPIMODE)
		RFSPI::rawWrite(tx_buffer, 2);
	else
	{

	}
}

word TRF797x_Init(TRF_Mode_t mode, Frequency_t frequency) {
	uint8_t rx_buffer[2];
	word ret_val = OK;

	IRQ_INT_OFF;

	g_selected_mode = mode;
	g_selected_frequency = frequency;

	if (mode == BOARD_INIT) {
		do {
			//
			// Soft Init Command
			//
			TRF797x_directCommand(TRF79X0_SOFT_INIT_CMD);

			//
			// Idle Command
			//
			TRF797x_directCommand(TRF79X0_IDLE_CMD);

			//MCU_delayMillisecond(1);
			waitms(1);

			//
			// Register 09h. Modulator Control
			//
			TRF797x_readSingle(rx_buffer, TRF79X0_MODULATOR_CONTROL_REG);

		} while (rx_buffer[0] != 0x91);

		//
		// Register 09h. Modulator Control
		//
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req.
		TRF797x_writeSingle(0x00, TRF79X0_MODULATOR_CONTROL_REG);

		//
		// Register 0Bh. Regulator Control
		//
		TRF797x_writeSingle(0x01, TRF79X0_REGULATOR_CONTROL_REG);

		//
		// Reset FIFO CMD + Dummy byte
		//
		TRF797x_resetFIFO();

		//
		// Register 00h. Chip Status Control
		//
		// +5 V operation
		TRF797x_writeSingle(0x01, TRF79X0_CHIP_STATUS_CTRL_REG);

		//		//
		//		// Register 14h. FIFO IRQ Level
		//		//
		//		// RX High = 96 bytes , TX Low = 32 bytes
		//		TRF797x_writeSingle(0x0F, TRF79X0_FIFO_IRQ_LEVEL);
	} else if (mode == P2P_INITATIOR_MODE) {
		// Check if there is another reader on the field
		if(TRF797x_checkExternalField() == HF_FIELD_ERROR)
		{
			ret_val = HF_FIELD_ERROR;
		}
		else
		{
			//
			// Register 00h. Chip Status Control
			//
			// Turn off transmitter, +5 V operation
			TRF797x_writeSingle(0x21, TRF79X0_CHIP_STATUS_CTRL_REG);

			if (frequency == FREQ_106_KBPS) {
				TRF797x_writeSingle(0x31, TRF79X0_ISO_CONTROL_REG);
			} else if (frequency == FREQ_212_KBPS) {
				TRF797x_writeSingle(0x32, TRF79X0_ISO_CONTROL_REG);
			} else if (frequency == FREQ_424_KBPS) {
				TRF797x_writeSingle(0x33, TRF79X0_ISO_CONTROL_REG);
			}
			//
			// Register 09h. Modulator Control
			//
			// SYS_CLK (in this case 13.56 MHz) out optional, based on system req.
			TRF797x_writeSingle(0x00, TRF79X0_MODULATOR_CONTROL_REG);

			//
			// Register 0Ah. RX Special Settings
			//
			// Turn off transmitter, +5 V operation
			TRF797x_writeSingle(0x80, TRF79X0_RX_SPECIAL_SETTINGS_REG);

			//
			// Register 0Bh. Regulator Control
			//
			TRF797x_writeSingle(0x01, TRF79X0_REGULATOR_CONTROL_REG);

			//
			// Register 14h. FIFO IRQ Level
			//
			// RX High = 96 bytes , TX Low = 32 bytes
			TRF797x_writeSingle(0x0F, TRF79X0_FIFO_IRQ_LEVEL);

			//
			// Register 16h. NFC Low Detection Level
			//
			TRF797x_writeSingle(0x03, TRF79X0_NFC_LOW_DETECTION_LEVEL);

			//
			// Register 18h. NFC Target level
			//
			TRF797x_writeSingle(0x07, TRF79X0_NFC_TARGET_LEVEL_REG);

			//
			// Guard Time Delay (GT_F)
			//
			waitms(20);
		}
	} else if (mode == P2P_PASSIVE_TARGET_MODE) {
		//
		// Soft Init Command
		//
		TRF797x_directCommand(TRF79X0_SOFT_INIT_CMD);

		//
		// Idle Command
		//
		TRF797x_directCommand(TRF79X0_IDLE_CMD);

		//
		// Disable Decoder Command
		//
		TRF797x_directCommand(TRF79X0_STOP_DECODERS_CMD);

		//
		// Register 00h. Chip Status Control
		//
		// RF output active, +5 V operation
		TRF797x_writeSingle(0x21, TRF79X0_CHIP_STATUS_CTRL_REG);

		//
		// Register 01h. ISO Control Register
		//
		if (frequency == FREQ_106_KBPS) {
			TRF797x_writeSingle(0x21, TRF79X0_ISO_CONTROL_REG);
		} else if (frequency == FREQ_212_KBPS) {
			TRF797x_writeSingle(0x22, TRF79X0_ISO_CONTROL_REG);
		} else if (frequency == FREQ_424_KBPS) {
			TRF797x_writeSingle(0x23, TRF79X0_ISO_CONTROL_REG);
		}

		//
		// Register 09h. Modulator Control
		//
		// SYS_CLK Disabled, based on system req.
		TRF797x_writeSingle(0x01, TRF79X0_MODULATOR_CONTROL_REG);

		//
		// Register 0Ah. RX Special Settings
		//
		TRF797x_writeSingle(0x1C, TRF79X0_RX_SPECIAL_SETTINGS_REG);

		//
		// Register 14h. FIFO IRQ Level
		//
		// RX High = 96 bytes , TX Low = 32 bytes
		TRF797x_writeSingle(0x0F, TRF79X0_FIFO_IRQ_LEVEL);

		//
		// Register 16h. NFC Low Detection Level
		//
		TRF797x_writeSingle(0x83, TRF79X0_NFC_LOW_DETECTION_LEVEL);

		//
		// Register 18h. NFC Target level
		//
		TRF797x_writeSingle(0x07, TRF79X0_NFC_TARGET_LEVEL_REG);

		//
		// Enable Decoder Command
		//
		TRF797x_directCommand(TRF79X0_RUN_DECODERS_CMD);

	} else if (mode == CARD_EMULATION_TYPE_A) {
		//
		// Soft Init Command
		//
		TRF797x_directCommand(TRF79X0_SOFT_INIT_CMD);

		//
		// Idle Command
		//
		TRF797x_directCommand(TRF79X0_IDLE_CMD);

		//
		// Disable Decoder Command
		//
		TRF797x_directCommand(TRF79X0_STOP_DECODERS_CMD);

		//
		// Register 00h. Chip Status Control
		//
		// Turn off transmitter, +5 V operation
		TRF797x_writeSingle(0x21, TRF79X0_CHIP_STATUS_CTRL_REG);

		//
		// Register 01h. Chip Status Control
		//
		TRF797x_writeSingle(0x24, TRF79X0_ISO_CONTROL_REG);

		//
		// Register 09h. Modulator Control - Needed at boot up, otherwise TRF
		// only receives Type F commands
		//
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req.
		TRF797x_writeSingle(0x01, TRF79X0_MODULATOR_CONTROL_REG);

		//
		// Register 0Ah. RX Special Settings
		//
		// Turn off transmitter, +5 V operation
		TRF797x_writeSingle(0x30, TRF79X0_RX_SPECIAL_SETTINGS_REG);

		//
		// Register 14h. FIFO IRQ Level
		//
		TRF797x_writeSingle(0x0F, TRF79X0_FIFO_IRQ_LEVEL);

		//
		// Register 16h. NFC Low Detection Level
		//
		TRF797x_writeSingle(0x83, TRF79X0_NFC_LOW_DETECTION_LEVEL);

		//
		// Register 18h. NFC Target level
		//
		TRF797x_writeSingle(0x07, TRF79X0_NFC_TARGET_LEVEL_REG);

		//		//
		//		// Register 1Ah. NFC Target level
		// Note: Enable the MOD pin output.
		//		//
		//		TRF797x_writeSingle(0x40, TRF79X0_TEST_SETTING1_REG);

		//
		// Enable Decoder Command
		//
		TRF797x_directCommand(TRF79X0_RUN_DECODERS_CMD);

	} else if (mode == CARD_EMULATION_TYPE_B) {
		//
		// Soft Init Command
		//
		TRF797x_directCommand(TRF79X0_SOFT_INIT_CMD);

		//
		// Idle Command
		//
		TRF797x_directCommand(TRF79X0_IDLE_CMD);

		//
		// Disable Decoder Command
		//
		TRF797x_directCommand(TRF79X0_STOP_DECODERS_CMD);

		//
		// Register 00h. Chip Status Control
		//
		// Turn off transmitter, +5 V operation
		TRF797x_writeSingle(0x21, TRF79X0_CHIP_STATUS_CTRL_REG);

		//
		// Register 01h. Chip Status Control
		//
		TRF797x_writeSingle(0x25, TRF79X0_ISO_CONTROL_REG);

		//
		// Register 02h.
		//
		TRF797x_writeSingle(0x00, TRF79X0_ISO14443B_OPTIONS_REG);

		//
		// Register 09h. Modulator Control
		//
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req.
		TRF797x_writeSingle(0x00, TRF79X0_MODULATOR_CONTROL_REG);

		//
		// Register 0Ah. RX Special Settings
		// Note: Disabling Bandpass for Felica and ISO15693 + enabling the RX gain reduction
		// improved the range.
		//
		// Turn off transmitter, +5 V operation
		TRF797x_writeSingle(0xF0, TRF79X0_RX_SPECIAL_SETTINGS_REG);

		//
		// Register 0Bh. Regulator Control
		//
		TRF797x_writeSingle(0x87, TRF79X0_REGULATOR_CONTROL_REG);

		//
		// Register 0Dh. Interrupt Mask Register
		//
		//		TRF797x_writeSingle(0x3F,TRF79X0_IRQ_MASK_REG);		// NO Response IRQ Enable
		TRF797x_writeSingle(0x3E, TRF79X0_IRQ_MASK_REG);

		//
		// Register 14h. FIFO IRQ Level
		//
		// RX High = 96 bytes , TX Low = 32 bytes
		TRF797x_writeSingle(0x0F, TRF79X0_FIFO_IRQ_LEVEL);

		//
		// Register 16h. NFC Low Detection Level
		// Note: No difference if set to 1-7 for card emulation.
		//
		TRF797x_writeSingle(0x03, TRF79X0_NFC_LOW_DETECTION_LEVEL);

		//
		// Register 18h. NFC Target level
		// Note: The Nexus 4 only works when this level is set to 7 and the reg
		// 0x0A is set to 0xF0 or 0x3C
		//
		TRF797x_writeSingle(0x07, TRF79X0_NFC_TARGET_LEVEL_REG);

		//
		// Enable Decoder Command
		//
		TRF797x_directCommand(TRF79X0_RUN_DECODERS_CMD);

	} else if (mode == TYPE_A_RW) {
		// Register 09h. Modulator Control
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req. 100% OOK
		TRF797x_writeSingle(0x01, TRF79X0_MODULATOR_CONTROL_REG);

		// Register 01h. Chip Status Control
		TRF797x_writeSingle(0x08, TRF79X0_ISO_CONTROL_REG);
	} else if (mode == TYPE_B_RW) {
		// Register 01h. Chip Status Control
		TRF797x_writeSingle(0x0C, TRF79X0_ISO_CONTROL_REG);

		// Register 09h. Modulator Control
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req. - 10%
		TRF797x_writeSingle(0x00, TRF79X0_MODULATOR_CONTROL_REG);
	} else if (mode == TYPE_F_RW) {
		// Register 01h. Chip Status Control
		TRF797x_writeSingle(0x1A, TRF79X0_ISO_CONTROL_REG);

		// Register 09h. Modulator Control
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req. - 10%
		TRF797x_writeSingle(0x00, TRF79X0_MODULATOR_CONTROL_REG);
	} else if (mode == TYPE_V_RW) {
		// Register 01h. Chip Status Control
		TRF797x_writeSingle(0x02, TRF79X0_ISO_CONTROL_REG);

		// Register 09h. Modulator Control
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req. - 10%
		TRF797x_writeSingle(0x00, TRF79X0_MODULATOR_CONTROL_REG);
	}
	IRQ_INT_ON;

	// Handle any IRQ in case the IRQ line is enabled
	// Ensure the IRQ line is low before sending out a packet
	TRF797x_irqHandler(0);

	TRF797x_displayMode();

	return ret_val;
}
status_ TRF797x_writeFIFO(byte *pbuf, CRC_t crc_bit, byte length, byte broken_bits) {
	status_ status;
	IRQ_flag_t irq_flag = IRQ_STATUS_IDLE;
	byte remaining_bytes = 0;
	byte fifo_status_length = 0;
	byte payload_length = 0;

	// Ensure the IRQ line is low before sending out a packet
	TRF797x_irqHandler(0);

	if (length > 127) {
		payload_length = 127;
	} else {
		payload_length = length;
	}

	remaining_bytes = length - payload_length;

	if (SPIMODE)
		RFSPI::writePacket(pbuf, crc_bit, length, payload_length, TRUE,broken_bits);

	while (irq_flag != IRQ_STATUS_TX_COMPLETE) {
		if(RFID::callback()){
			return STATUS_FAIL_T;
		}
		// Workaround for Type A commands - check the IRQ within 10 mS to refill FIFO
		if (g_selected_mode == CARD_EMULATION_TYPE_A && length > 127)
		{
			irq_flag = TRF797x_irqHandler(7);
		}
		else {
			// No workaround needed, implement a longer timeout, allowing for FIFO IRQ to handle the FIFO levels
			irq_flag = TRF797x_irqHandler(0);
		}

		if (irq_flag == IRQ_STATUS_ERROR || irq_flag == IRQ_STATUS_NO_RESPONSE) {
			status = STATUS_FAIL_T;
			break;
		} else if (irq_flag == IRQ_STATUS_TX_COMPLETE) {
			status = STATUS_SUCCESS_T;
		} else if ((irq_flag == IRQ_STATUS_FIFO_HIGH_OR_LOW
				|| irq_flag == IRQ_STATUS_TIME_OUT) && remaining_bytes > 0) {
			// Modify the pointer to point to the next address of data for payload larger than 127 bytes
			pbuf = pbuf + payload_length;

			TRF797x_readSingle(&fifo_status_length, TRF79X0_FIFO_STATUS_REG);

			// Check if there are more remaining bytes than available spots on the TRF7970
			if (remaining_bytes > (127 - fifo_status_length)) {
				// If there are more bytes than available then payload length is the (127 - fifo_status_length)
				payload_length = (127 - fifo_status_length);
			} else {
				payload_length = remaining_bytes;
			}

			remaining_bytes = remaining_bytes - payload_length;

			if (SPIMODE)
				RFSPI::writePacket(pbuf, crc_bit, length, payload_length, FALSE,0x00);
		}
	}

	return status;
}

void TRF797x_setP2PTargetFlag(bool target_flag)
{
	g_p2p_target_flag = target_flag;

}

void TRF797x_setP2PInitiatorFlag(bool initiator_flag)
{
	g_p2p_initiator_flag = initiator_flag;
}

void TRF797x_displayMode(void)
{
	if( g_selected_mode == P2P_INITATIOR_MODE ||
		g_selected_mode == P2P_PASSIVE_TARGET_MODE ||
		g_selected_mode == P2P_ACTIVE_TARGET_MODE)
	{
		GPIO::setRWMode(false);
		GPIO::setP2PTargetMode(true);
		GPIO::setCEMode(false);
	}
	else if(g_selected_mode == CARD_EMULATION_TYPE_A ||
			g_selected_mode == CARD_EMULATION_TYPE_B)
	{
		GPIO::setRWMode(false);
		GPIO::setP2PTargetMode(false);
		GPIO::setCEMode(true);
	}
	else if(g_selected_mode == TYPE_A_RW ||
			g_selected_mode == TYPE_B_RW ||
			g_selected_mode == TYPE_F_RW ||
			g_selected_mode == TYPE_V_RW)
	{
		GPIO::setRWMode(true);
		GPIO::setP2PTargetMode(false);
		GPIO::setCEMode(false);
	}
	else
	{
		GPIO::setRWMode(false);
		GPIO::setP2PTargetMode(false);
		GPIO::setCEMode(false);
	}
}

word TRF797x_checkExternalField(void)
{
	uint8_t rx_buffer;
	word ret_val = OK;

	//
	// Soft Init Command
	//
	TRF797x_directCommand(TRF79X0_SOFT_INIT_CMD);

	//
	// Idle Command
	//
	TRF797x_directCommand(TRF79X0_IDLE_CMD);

	// Register 00h. Chip Status Control
	// RF output active, +5 V operation
	TRF797x_writeSingle(0x03, TRF79X0_CHIP_STATUS_CTRL_REG);

	// Check if there an external RF Field
	TRF797x_directCommand(TRF79X0_TEST_EXTERNAL_RF_CMD);

	// Delay 50 uS to latch the RSSI value to the RSSI register
	waitus(50);

	TRF797x_readSingle(&rx_buffer, TRF79X0_RSSI_LEVEL_REG);

	// If the External RF Field is 0x00, we continue else we return fail
	if ((rx_buffer & 0x3F) != 0x00) {
		// Register 00h. Chip Status Control
		// RF output de-activated, +5 V operation
		TRF797x_writeSingle(0x01, TRF79X0_CHIP_STATUS_CTRL_REG);
		ret_val = HF_FIELD_ERROR;
	}
	else
	{

		//
		// Register 09h. Modulator Control
		//
		// SYS_CLK (in this case 13.56 MHz) out optional, based on system req.
		TRF797x_writeSingle(0x00, TRF79X0_MODULATOR_CONTROL_REG);

		//
		// Register 0Bh. Regulator Control
		//
		TRF797x_writeSingle(0x01, TRF79X0_REGULATOR_CONTROL_REG);

		//
		// Register 14h. FIFO IRQ Level
		//
		// RX High = 96 bytes , TX Low = 32 bytes
		TRF797x_writeSingle(0x0F, TRF79X0_FIFO_IRQ_LEVEL);
	}


	return ret_val;
}




#pragma vector = PORT2_VECTOR
__interrupt void TRF797x_IRQ_ISR(void) // interrupt handler
{
//	P2IFG &= ~0x01;  // FAIL
	IRQ_CLR;
	if (RFID::isNFC_TargetMode) {
		if (isTagSelected()) {
			g_irq_flag = 0x01;
			return;
		}
		if (TRF797x_irqHandler(0) == IRQ_STATUS_RX_COMPLETE) {
			NFC_ProcessReceivedData(&g_fifo_buffer[0]);
		}
		if (isTagSelected()) {
			RFID::isRFFieldOn = true;
		}
	} else {
		g_irq_flag = 0x01;
	}
}



