C++ PrintAndLog函数代码示例

int saveFile(const char *preferredName, const char *suffix, const void* data, size_t datalen)
{
	int size = sizeof(char) * (strlen(preferredName)+strlen(suffix)+10);
	char * fileName = malloc(size);

	memset(fileName,0,size);
	int num = 1;
	sprintf(fileName,"%s.%s", preferredName, suffix);
	while(fileExists(fileName))
	{
		sprintf(fileName,"%s-%d.%s", preferredName, num, suffix);
		num++;
	}
	/* We should have a valid filename now, e.g. dumpdata-3.bin */

	/*Opening file for writing in binary mode*/
	FILE *fh=fopen(fileName,"wb");
	if(!fh) {
		PrintAndLog("Failed to write to file '%s'", fileName);
		return 1;
	}
	fwrite(data, 1,	datalen, fh);
	fclose(fh);
	PrintAndLog("Saved data to '%s'", fileName);
	free(fileName);

	return 0;
}

int CmdLFSim(const char *Cmd)
{
  int i;
  static int gap;

  sscanf(Cmd, "%i", &gap);

  /* convert to bitstream if necessary */
  ChkBitstream(Cmd);

  PrintAndLog("Sending data, please wait...");
  for (i = 0; i < GraphTraceLen; i += 48) {
    UsbCommand c={CMD_DOWNLOADED_SIM_SAMPLES_125K, {i, 0, 0}};
    int j;
    for (j = 0; j < 48; j++) {
      c.d.asBytes[j] = GraphBuffer[i+j];
    }
    SendCommand(&c);
    WaitForResponse(CMD_ACK);
  }

  PrintAndLog("Starting simulator...");
  UsbCommand c = {CMD_SIMULATE_TAG_125K, {GraphTraceLen, gap, 0}};
  SendCommand(&c);
  return 0;
}

/**
 * Waits for a certain response type. This method waits for a maximum of
 * ms_timeout milliseconds for a specified response command.
 *@brief WaitForResponseTimeout
 * @param cmd command to wait for
 * @param response struct to copy received command into.
 * @param ms_timeout
 * @return true if command was returned, otherwise false
 */
bool WaitForResponseTimeout(uint32_t cmd, UsbCommand* response, size_t ms_timeout) {
  
  if (response == NULL) {
    UsbCommand resp;
    response = &resp;
  }

  // Wait until the command is received
  for(size_t dm_seconds=0; dm_seconds < ms_timeout/10; dm_seconds++) {

      while(getCommand(response))
      {
          if(response->cmd == cmd){
          //We got what we expected
          return true;
          }

      }
        msleep(10); // XXX ugh
        if (dm_seconds == 200) { // Two seconds elapsed
          PrintAndLog("Waiting for a response from the proxmark...");
          PrintAndLog("Don't forget to cancel its operation first by pressing on the button");
        }
	}
    return false;
}

int mfCSetUID(uint8_t *uid, uint8_t *atqa, uint8_t *sak, uint8_t *oldUID, bool wantWipe) {
	uint8_t oldblock0[16] = {0x00};
	uint8_t block0[16] = {0x00};

	int old = mfCGetBlock(0, oldblock0, CSETBLOCK_SINGLE_OPER);
	if (old == 0) {
		memcpy(block0, oldblock0, 16);
		PrintAndLog("old block 0:  %s", sprint_hex(block0,16));
	} else {
		PrintAndLog("Couldn't get old data. Will write over the last bytes of Block 0.");
	}

	// fill in the new values
	// UID
	memcpy(block0, uid, 4); 
	// Mifare UID BCC
	block0[4] = block0[0]^block0[1]^block0[2]^block0[3];
	// mifare classic SAK(byte 5) and ATQA(byte 6 and 7, reversed)
	if (sak!=NULL)
		block0[5]=sak[0];
	if (atqa!=NULL) {
		block0[6]=atqa[1];
		block0[7]=atqa[0];
	}
	PrintAndLog("new block 0:  %s", sprint_hex(block0,16));
	return mfCSetBlock(0, block0, oldUID, wantWipe, CSETBLOCK_SINGLE_OPER);
}

int CmdHF15CmdInquiry(const char *Cmd) 
{
	UsbCommand resp;
	uint8_t *recv;
	UsbCommand c = {CMD_ISO_15693_COMMAND, {0, 1, 1}}; // len,speed,recv?
	uint8_t *req=c.d.asBytes;
	int reqlen=0;
	
	req[0]= ISO15_REQ_SUBCARRIER_SINGLE | ISO15_REQ_DATARATE_HIGH | 
	        ISO15_REQ_INVENTORY | ISO15_REQINV_SLOT1;
	req[1]=ISO15_CMD_INVENTORY;
	req[2]=0; // mask length
	reqlen=AddCrc(req,3);
	c.arg[0]=reqlen;

	SendCommand(&c);
	
	if (WaitForResponseTimeout(CMD_ACK,&resp,1000)) {
		if (resp.arg[0]>=12) {
		   recv = resp.d.asBytes;
		   PrintAndLog("UID=%s",sprintUID(NULL,&recv[2]));
		   PrintAndLog("Tag Info: %s",getTagInfo(&recv[2]));	
		} else {
			PrintAndLog("Response to short, just %i bytes. No tag?\n",resp.arg[0]);
		}
	} else {
		PrintAndLog("timeout.");
	}
	return 0;
}

int mfnested(uint8_t blockNo, uint8_t keyType, uint8_t * key, uint8_t trgBlockNo, uint8_t trgKeyType, uint8_t * resultKeys) 
{
	int i, m, len;
	uint8_t isEOF;
	uint32_t uid;
	fnVector * vector = NULL;
	countKeys	*ck;
	int lenVector = 0;
	UsbCommand * resp = NULL;
	
	memset(resultKeys, 0x00, 16 * 6);

	// flush queue
	while (WaitForResponseTimeout(CMD_ACK, 500) != NULL) ;
	
  UsbCommand c = {CMD_MIFARE_NESTED, {blockNo, keyType, trgBlockNo + trgKeyType * 0x100}};
	memcpy(c.d.asBytes, key, 6);
  SendCommand(&c);

	PrintAndLog("\n");

	// wait cycle
	while (true) {
		printf(".");
		if (ukbhit()) {
			getchar();
			printf("\naborted via keyboard!\n");
			break;
		}

		resp = WaitForResponseTimeout(CMD_ACK, 1500);

		if (resp != NULL) {
			isEOF  = resp->arg[0] & 0xff;

			if (isEOF) break;
			
			len = resp->arg[1] & 0xff;
			if (len == 0) continue;
			
			memcpy(&uid, resp->d.asBytes, 4); 
			PrintAndLog("uid:%08x len=%d trgbl=%d trgkey=%x", uid, len, resp->arg[2] & 0xff, (resp->arg[2] >> 8) & 0xff);
			vector = (fnVector *) realloc((void *)vector, (lenVector + len) * sizeof(fnVector) + 200);
			if (vector == NULL) {
				PrintAndLog("Memory allocation error for fnVector. len: %d bytes: %d", lenVector + len, (lenVector + len) * sizeof(fnVector)); 
				break;
			}
			
			for (i = 0; i < len; i++) {
				vector[lenVector + i].blockNo = resp->arg[2] & 0xff;
				vector[lenVector + i].keyType = (resp->arg[2] >> 8) & 0xff;
				vector[lenVector + i].uid = uid;

				memcpy(&vector[lenVector + i].nt,  (void *)(resp->d.asBytes + 8 + i * 8 + 0), 4);
				memcpy(&vector[lenVector + i].ks1, (void *)(resp->d.asBytes + 8 + i * 8 + 4), 4);
			}

			lenVector += len;
		}
	}

// Simulation is still not working very good
int CmdHF15Sim(const char *Cmd)
{
	char cmdp = param_getchar(Cmd, 0);
	uint8_t uid[8] = {0x00};

	//E0 16 24 00 00 00 00 00
	if (cmdp == 'h' || cmdp == 'H') {
		PrintAndLog("Usage:  hf 15 sim <UID>");
		PrintAndLog("");
		PrintAndLog("     sample: hf 15 sim E016240000000000");
		return 0;
	}

	if (param_gethex(Cmd, 0, uid, 16)) {
		PrintAndLog("UID must include 16 HEX symbols");
		return 0;
	}
	
	PrintAndLog("Starting simulating UID %02X %02X %02X %02X %02X %02X %02X %02X",
			uid[0],uid[1],uid[2],uid[3],uid[4], uid[5], uid[6], uid[7]);

	UsbCommand c = {CMD_SIMTAG_ISO_15693, {0, 0, 0}};
	memcpy(c.d.asBytes,uid,8);
	
	SendCommand(&c);
	return 0;
}

int CmdLFPCF7931Write(const char *Cmd)
{
  UsbCommand c = {CMD_PCF7931_WRITE};

  int res = 0;
  res = sscanf(Cmd, "%x %x %x", &c.arg[0], &c.arg[1], &c.arg[2]);

  if(res < 1) {
      PrintAndLog("Please specify the block address in hex");
      return 0;
  }
  if (res == 1){ 
    PrintAndLog("Please specify the byte address in hex");
     return 0;
  }
  if(res == 2) {
    PrintAndLog("Please specify the data in hex (1 byte)");
     return 0;
  }
  if(res == 3) {
    uint8_t n=0;
    for(n=0;n<7;n++)  c.d.asDwords[n] = configPcf.password[n];
    c.d.asDwords[7] = (configPcf.offset[0]+128);
    c.d.asDwords[8] = (configPcf.offset[1]+128);
    c.d.asDwords[9] = configPcf.init_delay;
    SendCommand(&c);
    return 0;
  }

  PrintAndLog("INCORRECT FORMAT");
  return 0;
}

int CmdPyramidSim(const char *Cmd) {

	char cmdp = param_getchar(Cmd, 0);
	if (strlen(Cmd) == 0 || cmdp == 'h' || cmdp == 'H') return usage_lf_pyramid_sim();

	uint32_t facilitycode = 0, cardnumber = 0, fc = 0, cn = 0;

	uint8_t bs[128];
	size_t size = sizeof(bs);
	memset(bs, 0x00, size);

	// Pyramid uses:  fcHigh: 10, fcLow: 8, clk: 50, invert: 0
	uint64_t arg1, arg2;
	arg1 = (10 << 8) + 8;
	arg2 = 50 | 0;

	if (sscanf(Cmd, "%u %u", &fc, &cn ) != 2) return usage_lf_pyramid_sim();

	facilitycode = (fc & 0x000000FF);
	cardnumber = (cn & 0x0000FFFF);

	if ( !GetPyramidBits(facilitycode, cardnumber, bs)) {
		PrintAndLog("Error with tag bitstream generation.");
		return 1;
	}

	PrintAndLog("Simulating Farpointe/Pyramid - Facility Code: %u, CardNumber: %u", facilitycode, cardnumber );

	UsbCommand c = {CMD_FSK_SIM_TAG, {arg1, arg2, size}};
	memcpy(c.d.asBytes, bs, size);
	clearCommandBuffer();
	SendCommand(&c);
	return 0;
}

//-----------------------------------------------------------------------------
// Entry point into our code: called whenever we received a packet over USB
// that we weren't necessarily expecting, for example a debug print.
//-----------------------------------------------------------------------------
void UsbCommandReceived(UsbCommand *UC)
{
	switch(UC->cmd) {
		// First check if we are handling a debug message
		case CMD_DEBUG_PRINT_STRING: {
			char s[USB_CMD_DATA_SIZE+1] = {0x00};
			size_t len = MIN(UC->arg[0],USB_CMD_DATA_SIZE);
			memcpy(s,UC->d.asBytes,len);
			PrintAndLog("#db# %s       ", s);
			return;
		} break;

		case CMD_DEBUG_PRINT_INTEGERS: {
			PrintAndLog("#db# %08x, %08x, %08x       \r\n", UC->arg[0], UC->arg[1], UC->arg[2]);
			return;
		} break;

		case CMD_DOWNLOADED_RAW_ADC_SAMPLES_125K: {
			memcpy(sample_buf+(UC->arg[0]),UC->d.asBytes,UC->arg[1]);
			return;
		} break;

		default:
			storeCommand(UC);
			break;
	}

}

int mfCSetUID(uint8_t *uid, uint8_t *atqa, uint8_t *sak, uint8_t *oldUID, uint8_t wipecard) {

	uint8_t params = MAGIC_SINGLE;
	uint8_t block0[16];
	memset(block0, 0x00, sizeof(block0));

	int old = mfCGetBlock(0, block0, params);
	if (old == 0)
		PrintAndLog("old block 0:  %s", sprint_hex(block0, sizeof(block0)));
	else 
		PrintAndLog("Couldn't get old data. Will write over the last bytes of Block 0.");	

	// fill in the new values
	// UID
	memcpy(block0, uid, 4); 
	// Mifare UID BCC
	block0[4] = block0[0]^block0[1]^block0[2]^block0[3];
	// mifare classic SAK(byte 5) and ATQA(byte 6 and 7, reversed)
	if ( sak != NULL )
		block0[5]=sak[0];
	
	if ( atqa != NULL ) {
		block0[6]=atqa[1];
		block0[7]=atqa[0];
	}
	PrintAndLog("new block 0:  %s", sprint_hex(block0,16));
	
	if ( wipecard )		 params |= MAGIC_WIPE;	
	if ( oldUID == NULL) params |= MAGIC_UID;
	
	return mfCSetBlock(0, block0, oldUID, params);
}

int pcf7931_printConfig() {
    PrintAndLog("Password (LSB first on bytes) : %s", sprint_hex( configPcf.Pwd, sizeof(configPcf.Pwd)));
    PrintAndLog("Tag initialization delay      : %d us", configPcf.InitDelay);
    PrintAndLog("Offset low pulses width       : %d us", configPcf.OffsetWidth);
    PrintAndLog("Offset low pulses position    : %d us", configPcf.OffsetPosition);
    return 0;
}

int CmdLFPCF7931Write(const char *Cmd) {

    uint8_t ctmp = param_getchar(Cmd, 0);
    if (strlen(Cmd) < 1 || ctmp == 'h' || ctmp == 'H') return usage_pcf7931_write();

    uint8_t block = 0, bytepos = 0, data = 0;

    if ( param_getdec(Cmd, 0, &block) ) return usage_pcf7931_write();
    if ( param_getdec(Cmd, 1, &bytepos) ) return usage_pcf7931_write();

    if ( (block > 7) || (bytepos > 15) ) return usage_pcf7931_write();

    data  = param_get8ex(Cmd, 2, 0, 16);

    PrintAndLog("Writing block: %d", block);
    PrintAndLog("          pos: %d", bytepos);
    PrintAndLog("         data: 0x%02X", data);

    UsbCommand c = {CMD_PCF7931_WRITE, { block, bytepos, data} };
    memcpy(c.d.asDwords, configPcf.Pwd, sizeof(configPcf.Pwd) );
    c.d.asDwords[7] = (configPcf.OffsetWidth + 128);
    c.d.asDwords[8] = (configPcf.OffsetPosition + 128);
    c.d.asDwords[9] = configPcf.InitDelay;

    clearCommandBuffer();
    SendCommand(&c);
    //no ack?
    return 0;
}

// clearing the topbit needed for the preambl detection. 
static void verify_values(uint32_t countryid, uint64_t animalid){
	if ((animalid & 0x3FFFFFFFFF) != animalid) {
		animalid &= 0x3FFFFFFFFF;
		PrintAndLog("Animal ID Truncated to 38bits: %"PRIx64, animalid);
	}	
	if ( (countryid & 0x3ff) != countryid ) {
		countryid &= 0x3ff;
		PrintAndLog("Country ID Truncated to 10bits: %03d", countryid);
	}
}

/*
 * Sets the divisor for LF frequency clock: lets the user choose any LF frequency below
 * 600kHz.
 */
int CmdSetDivisor(const char *Cmd)
{
  UsbCommand c = {CMD_SET_LF_DIVISOR, {strtol(Cmd, NULL, 0), 0, 0}};
  if (c.arg[0] < 19 || c.arg[0] > 255) {
    PrintAndLog("divisor must be between 19 and 255");
  } else {
    SendCommand(&c);
    PrintAndLog("Divisor set, expected freq=%dHz", 12000000 / (c.arg[0]+1));
  }
  return 0;
}