C++ Dbprintf函数代码示例

int mifare_desfire_des_auth1(uint32_t uid, uint8_t *blockData){

	int len;
	// load key, keynumber
	uint8_t data[2]={0x0a, 0x00};
	uint8_t receivedAnswer[MAX_FRAME_SIZE];
	uint8_t receivedAnswerPar[MAX_PARITY_SIZE];
	
	len = mifare_sendcmd_special(NULL, 1, 0x02, data, receivedAnswer,receivedAnswerPar,NULL);
	if (len == 1) {
		if (MF_DBGLEVEL >= MF_DBG_ERROR)
			Dbprintf("Cmd Error: %02x", receivedAnswer[0]);
		return 1;
	}
	
	if (len == 12) {
		if (MF_DBGLEVEL >= MF_DBG_EXTENDED)	{
			Dbprintf("Auth1 Resp: %02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
				receivedAnswer[0],receivedAnswer[1],receivedAnswer[2],receivedAnswer[3],receivedAnswer[4],
				receivedAnswer[5],receivedAnswer[6],receivedAnswer[7],receivedAnswer[8],receivedAnswer[9],
				receivedAnswer[10],receivedAnswer[11]);
			}
			memcpy(blockData, receivedAnswer, 12);
	        return 0;
	}
	return 1;
}

int mifare_ultra_writeblock(uint8_t blockNo, uint8_t *blockData) 
{
	uint16_t len;
	uint8_t par[3] = {0};  // enough for 18 parity bits
	uint8_t d_block[18] = {0x00};
	uint8_t receivedAnswer[MAX_FRAME_SIZE];
	uint8_t receivedAnswerPar[MAX_PARITY_SIZE];

	// command MIFARE_CLASSIC_WRITEBLOCK
	len = mifare_sendcmd_short(NULL, true, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL);

	if ((len != 1) || (receivedAnswer[0] != 0x0A)) {   //  0x0a - ACK
		if (MF_DBGLEVEL >= MF_DBG_ERROR)
			Dbprintf("Cmd Addr Error: %02x", receivedAnswer[0]);
		return 1;
	}

	memcpy(d_block, blockData, 16);
	AppendCrc14443a(d_block, 16);

	ReaderTransmitPar(d_block, sizeof(d_block), par, NULL);

	len = ReaderReceive(receivedAnswer, receivedAnswerPar);

	if ((len != 1) || (receivedAnswer[0] != 0x0A)) {   //  0x0a - ACK
		if (MF_DBGLEVEL >= MF_DBG_ERROR)
			Dbprintf("Cmd Data Error: %02x %d", receivedAnswer[0],len);
		return 2;
	}
	return 0;
}

int mifare_ultra_readblock(uint32_t uid, uint8_t blockNo, uint8_t *blockData)
{
	// variables
	uint16_t len;
	uint8_t	bt[2];
	
	uint8_t* receivedAnswer = get_bigbufptr_recvrespbuf();
	uint8_t* receivedAnswerPar = receivedAnswer + MAX_FRAME_SIZE;
	
	// command MIFARE_CLASSIC_READBLOCK
	len = mifare_sendcmd_short(NULL, 1, 0x30, blockNo, receivedAnswer, receivedAnswerPar, NULL);
	if (len == 1) {
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd Error: %02x", receivedAnswer[0]);
		return 1;
	}
	if (len != 18) {
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd Error: card timeout. len: %x", len);
		return 2;
	}
    
	memcpy(bt, receivedAnswer + 16, 2);
	AppendCrc14443a(receivedAnswer, 16);
	if (bt[0] != receivedAnswer[16] || bt[1] != receivedAnswer[17]) {
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd CRC response error.");
		return 3;
	}
	
	memcpy(blockData, receivedAnswer, 14);
	return 0;
}

int mifare_ultra_readblock(uint8_t blockNo, uint8_t *blockData)
{
	uint16_t len;
	uint8_t	bt[2];
	uint8_t receivedAnswer[MAX_FRAME_SIZE];
	uint8_t receivedAnswerPar[MAX_PARITY_SIZE];
	

	len = mifare_sendcmd_short(NULL, 1, 0x30, blockNo, receivedAnswer, receivedAnswerPar, NULL);
	if (len == 1) {
		if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Cmd Error: %02x", receivedAnswer[0]);
		return 1;
	}
	if (len != 18) {
		if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Cmd Error: card timeout. len: %x", len);
		return 2;
	}
    
	memcpy(bt, receivedAnswer + 16, 2);
	AppendCrc14443a(receivedAnswer, 16);
	if (bt[0] != receivedAnswer[16] || bt[1] != receivedAnswer[17]) {
		if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Cmd CRC response error.");
		return 3;
	}
	
	memcpy(blockData, receivedAnswer, 14);
	return 0;
}

int mifare_classic_readblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) 
{
	// variables
	int len;	
	uint8_t	bt[2];
	
	uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
	uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE];
	
	// command MIFARE_CLASSIC_READBLOCK
	len = mifare_sendcmd_short(pcs, 1, 0x30, blockNo, receivedAnswer, receivedAnswerPar, NULL);
	if (len == 1) {
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd Error: %02x", receivedAnswer[0]);  
		return 1;
	}
	if (len != 18) {
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd Error: card timeout. len: %x", len);  
		return 2;
	}

	memcpy(bt, receivedAnswer + 16, 2);
	AppendCrc14443a(receivedAnswer, 16);
	if (bt[0] != receivedAnswer[16] || bt[1] != receivedAnswer[17]) {
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd CRC response error.");  
		return 3;
	}
	
	memcpy(blockData, receivedAnswer, 16);
	return 0;
}

int mifare_ultra_writeblock(uint32_t uid, uint8_t blockNo, uint8_t *blockData) 
{
    // variables
    uint16_t len;     
    uint8_t par[3] = {0};  // enough for 18 parity bits
        
    uint8_t d_block[18];
    uint8_t* receivedAnswer = get_bigbufptr_recvrespbuf();
	uint8_t* receivedAnswerPar = receivedAnswer + MAX_FRAME_SIZE;
        
    // command MIFARE_CLASSIC_WRITEBLOCK
    len = mifare_sendcmd_short(NULL, true, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL);

    if ((len != 1) || (receivedAnswer[0] != 0x0A)) {   //  0x0a - ACK
        if (MF_DBGLEVEL >= 1)   Dbprintf("Cmd Addr Error: %02x", receivedAnswer[0]);  
        return 1;
    }

	memset(d_block,'\0',18);
	memcpy(d_block, blockData, 16);
    AppendCrc14443a(d_block, 16);

	ReaderTransmitPar(d_block, sizeof(d_block), par, NULL);

    // Receive the response
    len = ReaderReceive(receivedAnswer, receivedAnswerPar);    

	if ((len != 1) || (receivedAnswer[0] != 0x0A)) {   //  0x0a - ACK
        if (MF_DBGLEVEL >= 1)   Dbprintf("Cmd Data Error: %02x %d", receivedAnswer[0],len);
        return 2;
    }        

    return 0;
} 

int mifare_desfire_des_auth2(uint32_t uid, uint8_t *key, uint8_t *blockData){

	int len;
	uint8_t data[17] = {0x00};
	data[0] = 0xAF;
	memcpy(data+1,key,16);
	
	uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
	uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE];
	
	len = mifare_sendcmd_special2(NULL, 1, 0x03, data, receivedAnswer, receivedAnswerPar ,NULL);
	
	if ((receivedAnswer[0] == 0x03) && (receivedAnswer[1] == 0xae)) {
		if (MF_DBGLEVEL >= MF_DBG_ERROR)
			Dbprintf("Auth Error: %02x %02x", receivedAnswer[0], receivedAnswer[1]);
		return 1;
	}
	
	if (len == 12){
		if (MF_DBGLEVEL >= MF_DBG_EXTENDED) {
			Dbprintf("Auth2 Resp: %02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
				receivedAnswer[0],receivedAnswer[1],receivedAnswer[2],receivedAnswer[3],receivedAnswer[4],
				receivedAnswer[5],receivedAnswer[6],receivedAnswer[7],receivedAnswer[8],receivedAnswer[9],
				receivedAnswer[10],receivedAnswer[11]);
			}
		memcpy(blockData, receivedAnswer, 12);
		return 0;
	}
	return 1;
}

void Fpga_print_status(void)
{
	Dbprintf("Fgpa");
	if(downloaded_bitstream == FPGA_BITSTREAM_HF) Dbprintf("  mode.............HF");
	else if(downloaded_bitstream == FPGA_BITSTREAM_LF) Dbprintf("  mode.............LF");
	else Dbprintf("  mode.............%d", downloaded_bitstream);
}

// 3 olika ISO sätt att skicka data till DESFIRE (direkt, inkapslat, inkapslat ISO)
// cmd  =  cmd bytes to send
// cmd_len = length of cmd
// dataout = pointer to response data array
int DesfireAPDU(uint8_t *cmd, size_t cmd_len, uint8_t *dataout){

	size_t len = 0;
	size_t wrappedLen = 0;
	uint8_t wCmd[USB_CMD_DATA_SIZE] = {0x00};
	
	uint8_t resp[MAX_FRAME_SIZE];
    uint8_t par[MAX_PARITY_SIZE];
	
	wrappedLen = CreateAPDU( cmd, cmd_len, wCmd);
	
	if (MF_DBGLEVEL >= 4) {
		print_result("WCMD <--: ", wCmd, wrappedLen);	
	}
	ReaderTransmit( wCmd, wrappedLen, NULL);

	len = ReaderReceive(resp, par);
	
	if ( !len ) {
		if (MF_DBGLEVEL >= 4) Dbprintf("fukked");
		return FALSE; //DATA LINK ERROR
	}
	// if we received an I- or R(ACK)-Block with a block number equal to the
	// current block number, toggle the current block number
	else if (len >= 4 // PCB+CID+CRC = 4 bytes
	         && ((resp[0] & 0xC0) == 0 // I-Block
	             || (resp[0] & 0xD0) == 0x80) // R-Block with ACK bit set to 0
	         && (resp[0] & 0x01) == pcb_blocknum) // equal block numbers
	{
		pcb_blocknum ^= 1;  //toggle next block 
	}

	memcpy(dataout, resp, len);
	return len;
}	

int mifare_sendcmd_short_special(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t* data, uint8_t* answer, uint8_t *answer_parity, uint32_t *timing)
{
    uint8_t dcmd[8];//, ecmd[4];
    //uint32_t par=0;

    dcmd[0] = cmd;
    dcmd[1] = data[0];
	dcmd[2] = data[1];
	dcmd[3] = data[2];
	dcmd[4] = data[3];
	dcmd[5] = data[4];
	AppendCrc14443a(dcmd, 6);
	//Dbprintf("Data command: %02x", dcmd[0]);
	//Dbprintf("Data R: %02x %02x %02x %02x %02x %02x %02x", dcmd[1],dcmd[2],dcmd[3],dcmd[4],dcmd[5],dcmd[6],dcmd[7]);

        //memcpy(ecmd, dcmd, sizeof(dcmd));
	ReaderTransmit(dcmd, sizeof(dcmd), NULL);
	int len = ReaderReceive(answer, answer_parity);
	if(!len)
	{
                if (MF_DBGLEVEL >= 1)   Dbprintf("Authentication failed. Card timeout.");
                return 2;
        }
	return len;
}

int mifare_classic_writeblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) 
{
	// variables
	int len, i;	
	uint32_t pos;
	uint8_t par[3] = {0};		// enough for 18 Bytes to send
	byte_t res;
	
	uint8_t d_block[18], d_block_enc[18];
	uint8_t* receivedAnswer = get_bigbufptr_recvrespbuf();
	uint8_t* receivedAnswerPar = receivedAnswer + MAX_FRAME_SIZE;
	
	// command MIFARE_CLASSIC_WRITEBLOCK
	len = mifare_sendcmd_short(pcs, 1, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL);

	if ((len != 1) || (receivedAnswer[0] != 0x0A)) {   //  0x0a - ACK
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd Error: %02x", receivedAnswer[0]);  
		return 1;
	}
	
	memcpy(d_block, blockData, 16);
	AppendCrc14443a(d_block, 16);
	
	// crypto
	for (pos = 0; pos < 18; pos++)
	{
		d_block_enc[pos] = crypto1_byte(pcs, 0x00, 0) ^ d_block[pos];
		par[pos>>3] |= (((filter(pcs->odd) ^ oddparity(d_block[pos])) & 0x01) << (7 - (pos&0x0007)));
	}	

	ReaderTransmitPar(d_block_enc, sizeof(d_block_enc), par, NULL);

	// Receive the response
	len = ReaderReceive(receivedAnswer, receivedAnswerPar);	

	res = 0;
	for (i = 0; i < 4; i++)
		res |= (crypto1_bit(pcs, 0, 0) ^ BIT(receivedAnswer[0], i)) << i;

	if ((len != 1) || (res != 0x0A)) {
		if (MF_DBGLEVEL >= 1)	Dbprintf("Cmd send data2 Error: %02x", res);  
		return 2;
	}
	
	return 0;
}

void MifareSendCommand(uint8_t arg0, uint8_t arg1, uint8_t *datain){
	
	/* ARG0 contains flags.
		0x01 = init card.
		0x02 = Disconnect
		0x03
	*/
	uint8_t flags = arg0;
	size_t datalen = arg1;
	uint8_t resp[RECEIVE_SIZE];
	memset(resp,0,sizeof(resp));
	
	if (MF_DBGLEVEL >= 4) {
		Dbprintf(" flags : %02X", flags);
		Dbprintf(" len   : %02X", datalen);
		print_result(" RX    : ", datain, datalen);
	}
	
	if ( flags & CLEARTRACE )
		clear_trace();
	
	if ( flags & INIT ){
		if ( !InitDesfireCard() )
			return;
	}
	
	int len = DesfireAPDU(datain, datalen, resp);
	if (MF_DBGLEVEL >= 4)
		print_result("ERR <--: ", resp, len);

	if ( !len ) {
		OnError(2);
		return;
	}
	
	// reset the pcb_blocknum,
	pcb_blocknum = 0;
	
	if ( flags & DISCONNECT )
		OnSuccess();
	
	cmd_send(CMD_ACK,1,len,0,resp,len);
}

int mifare_sendcmd_short_mfucauth(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t *data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing)
{
    uint8_t dcmd[19];
	int len; 
    dcmd[0] = cmd;
    memcpy(dcmd+1,data,16);
	AppendCrc14443a(dcmd, 17);
	
	ReaderTransmit(dcmd, sizeof(dcmd), timing);
	len = ReaderReceive(answer, answer_parity);
	if(!len) {
        if (MF_DBGLEVEL >= MF_DBG_ERROR)   Dbprintf("Authentication failed. Card timeout.");
        len = ReaderReceive(answer,answer_parity);
    }
    if(len==1)	{
		if (MF_DBGLEVEL >= MF_DBG_ERROR)   Dbprintf("NAK - Authentication failed.");
		return 1;
        }
	return len;
}

int mifare_classic_halt_ex(struct Crypto1State *pcs) {
	uint16_t len;	
	uint8_t receivedAnswer[4];
	uint8_t receivedAnswerPar[4];

	len = mifare_sendcmd_short(pcs, pcs == NULL ? false:true, 0x50, 0x00, receivedAnswer, receivedAnswerPar, NULL);
	if (len != 0) {
		if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("halt error. response len: %x", len);  
		return 1;
	}
	return 0;
}

// mifare ultralight commands
int mifare_ul_ev1_auth(uint8_t *keybytes, uint8_t *pack){

	uint16_t len;
	uint8_t resp[4];
	uint8_t respPar[1];
	uint8_t key[4] = {0x00};
	memcpy(key, keybytes, 4);

	Dbprintf("EV1 Auth : %02x%02x%02x%02x",	key[0], key[1], key[2], key[3]);
	len = mifare_sendcmd_short_mfuev1auth(NULL, 0, 0x1B, key, resp, respPar, NULL);
	if (len != 4) {
		if (MF_DBGLEVEL >= MF_DBG_ERROR) Dbprintf("Cmd Error: %02x %u", resp[0], len);
		return 0;
	}

	if (MF_DBGLEVEL >= MF_DBG_EXTENDED)
		Dbprintf("Auth Resp: %02x%02x%02x%02x", resp[0],resp[1],resp[2],resp[3]);

	memcpy(pack, resp, 4);
	return 1;
}