[proxmark3-svn] / client / cmdlfhid.c

//-----------------------------------------------------------------------------
// Copyright (C) 2010 iZsh <izsh at fail0verflow.com>
//
// This code is licensed to you under the terms of the GNU GPL, version 2 or,
// at your option, any later version. See the LICENSE.txt file for the text of
// the license.
//-----------------------------------------------------------------------------
// Low frequency HID commands
//-----------------------------------------------------------------------------

#include "cmdlfhid.h"

#ifndef BITS
# define BITS 96
#endif

static int CmdHelp(const char *Cmd);

int usage_lf_hid_wiegand(void){
	PrintAndLog("This command converts facility code/card number to Wiegand code");
  	PrintAndLog("Usage: lf hid wiegand [h] [OEM] [FC] [CN]");

	PrintAndLog("Options:");
	PrintAndLog("       h             - This help");
	PrintAndLog("       OEM           - OEM number / site code");
	PrintAndLog("       FC            - facility code");
	PrintAndLog("       CN            - card number");
	PrintAndLog("Examples:");
	PrintAndLog("      lf hid wiegand 0 101 2001");
	return 0;
}
int usage_lf_hid_sim(void){
	PrintAndLog("HID Tag simulator");
	PrintAndLog("");
	PrintAndLog("Usage:  lf hid sim [h] [ID]");
	PrintAndLog("Options:");
	PrintAndLog("       h	- This help");
	PrintAndLog("       ID  - HID id");
	PrintAndLog("Examples:");
	PrintAndLog("      lf hid sim 224");
	return 0;
}
int usage_lf_hid_clone(void){
	PrintAndLog("Clone HID to T55x7.  Tag must be on antenna. ");
	PrintAndLog("");
	PrintAndLog("Usage:  lf hid clone [h] [ID] <L>");
	PrintAndLog("Options:");
	PrintAndLog("       h	- This help");
	PrintAndLog("       ID  - HID id");
	PrintAndLog("       L   - 84bit ID");
	PrintAndLog("Examples:");
	PrintAndLog("      lf hid clone 224");
	PrintAndLog("      lf hid clone 224 L");
	return 0;
}
int usage_lf_hid_brute(void){
	PrintAndLog("Enables bruteforce of HID readers with specified facility code.");
	PrintAndLog("This is a attack against reader. if cardnumber is given, it starts with it and goes up / down one step");
	PrintAndLog("if cardnumber is not given, it starts with 1 and goes up to 65535");
	PrintAndLog("");
	PrintAndLog("Usage:  lf hid brute [h] a <format> f <facility-code> c <cardnumber> d <delay>");
	PrintAndLog("Options :");
	PrintAndLog("       h                 :  This help");	
	PrintAndLog("       a <format>        :  26|33|34|35|37|40|44|84");
	PrintAndLog("       f <facility-code> :  8-bit value HID facility code");
	PrintAndLog("       c <cardnumber>    :  (optional) cardnumber to start with, max 65535");
	PrintAndLog("       d <delay>         :  delay betweens attempts in ms. Default 1000ms");
	PrintAndLog("");
	PrintAndLog("Samples:");
	PrintAndLog("       lf hid brute a 26 f 224");
	PrintAndLog("       lf hid brute a 26 f 21 d 2000");
	PrintAndLog("       lf hid brute a 26 f 21 c 200 d 2000");
	return 0;
}

static int sendPing(void){
	UsbCommand ping = {CMD_PING, {1, 2, 3}};
	SendCommand(&ping);
	SendCommand(&ping);	
	SendCommand(&ping);	
	clearCommandBuffer();
	UsbCommand resp;
	if (WaitForResponseTimeout(CMD_ACK, &resp, 1000))
		return 0;
	return 1;
}
static bool sendTry(uint8_t fmtlen, uint32_t fc, uint32_t cn, uint32_t delay, uint8_t *bs){

	PrintAndLog("Trying FC: %u; CN: %u", fc, cn);
	
	calcWiegand( fmtlen, fc, cn, bs);

	uint64_t arg1 = bytebits_to_byte(bs,32);
	uint64_t arg2 = bytebits_to_byte(bs+32,32);
	UsbCommand c = {CMD_HID_SIM_TAG, {arg1, arg2, 0}}; 
	clearCommandBuffer();
	SendCommand(&c);
	msleep(delay);
	sendPing();
	return TRUE;
}

int CmdHIDDemodFSK(const char *Cmd) {
	uint8_t findone = ( Cmd[0] == '1' ) ? 1 : 0;
	UsbCommand c = {CMD_HID_DEMOD_FSK, {findone, 0 , 0}};
	clearCommandBuffer();
	SendCommand(&c);
	return 0;
}

int CmdHIDSim(const char *Cmd) {
	unsigned int hi = 0, lo = 0;
	int n = 0, i = 0;

	uint8_t ctmp = param_getchar(Cmd, 0);
	if ( strlen(Cmd) == 0 || ctmp == 'H' || ctmp == 'h' ) return usage_lf_hid_sim();
	
	while (sscanf(&Cmd[i++], "%1x", &n ) == 1) {
		hi = (hi << 4) | (lo >> 28);
		lo = (lo << 4) | (n & 0xf);
	}

	PrintAndLog("Emulating tag with ID %x%16x", hi, lo);
	PrintAndLog("Press pm3-button to abort simulation");

	UsbCommand c = {CMD_HID_SIM_TAG, {hi, lo, 0}};
	clearCommandBuffer();
	SendCommand(&c);
	return 0;
}

int CmdHIDClone(const char *Cmd) {
	
	uint32_t hi2 = 0, hi = 0, lo = 0;
	int n = 0, i = 0;
	UsbCommand c;

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

	if (strchr(Cmd,'l') != 0) {
		while (sscanf(&Cmd[i++], "%1x", &n ) == 1) {
			hi2 = (hi2 << 4) | (hi >> 28);
			hi = (hi << 4) | (lo >> 28);
			lo = (lo << 4) | (n & 0xf);
		}

		PrintAndLog("Cloning tag with long ID %x%08x%08x", hi2, hi, lo);

		c.d.asBytes[0] = 1;
	} else {
		while (sscanf(&Cmd[i++], "%1x", &n ) == 1) {
			hi = (hi << 4) | (lo >> 28);
			lo = (lo << 4) | (n & 0xf);
		}

		PrintAndLog("Cloning tag with ID %x%08x", hi, lo);

		hi2 = 0;
		c.d.asBytes[0] = 0;
	}

	c.cmd = CMD_HID_CLONE_TAG;
	c.arg[0] = hi2;
	c.arg[1] = hi;
	c.arg[2] = lo;

	clearCommandBuffer();
	SendCommand(&c);
	return 0;
}
// struct to handle wiegand
typedef struct {
	uint8_t  FormatLen;
	uint8_t  SiteCode;
	uint8_t  FacilityCode;
	uint8_t  CardNumber;
	uint8_t* Wiegand;
	size_t   Wiegand_n;
 } wiegand_t;

static void addHIDMarker(uint8_t fmtlen, uint8_t *out) {    
	// temp array
	uint8_t arr[BITS];
	memset(arr, 0, BITS);

	// copy inpu
	uint8_t pos = sizeof(arr)-fmtlen;
	memcpy(arr+pos, out, fmtlen);
			
	switch(fmtlen) {
		case 26:{
			// start sentinel, BITS-bit 27 = 1
			arr[BITS-27] = 1;
			// fmt smaller than 37 used,  bit37 = 1
			arr[BITS-38]  = 1;
			memcpy(out, arr, BITS);
			break;
		}
		case 34:
			// start sentinel, BITS-bit 27 = 1
			arr[BITS-35] = 1;
			
			// fmt smaller than 37 used,  bit37 = 1
			arr[BITS-38]  = 1;
			memcpy(out, arr, BITS);
			break;
		default:break;
	}
}

// static void getParity34(uint32_t *hi, uint32_t *lo){
	// uint32_t result = 0;
	// int i;

	// // even parity 
	// for (i = 7;i >= 0;i--)
		// result ^= (*hi >> i) & i;
	// for (i = 31;i >= 24;i--)
		// result ^= (*lo >> i) & 1;

	// *hi |= result << 2; 

	// // odd parity bit
	// result = 0;
	// for (i = 23;i >= 1;i--)
		// result ^= (*lo >> i) & 1;

	// *lo |= !result;
// }
// static void getParity37H(uint32_t *hi, uint32_t *lo){
	// uint32_t result = 0;
	// int i;

	// // even parity
	// for (i = 4;i >= 0;i--)
		// result ^= (*hi >> i) & 1;
	// for (i = 31;i >= 20;i--)
		// result ^= (*lo >> i) & 1;
	// *hi |= result << 4;

	// // odd parity
	// result = 0;
	// for (i = 19;i >= 1;i--)
		// result ^= (*lo >> i) & 1;
	// *lo |= result;
// }

//static void calc26(uint16_t fc, uint32_t cardno,  uint8_t *out){
static void calc26(uint16_t fc, uint32_t cardno, uint8_t *out){
	uint8_t wiegand[24];
	num_to_bytebits(fc, 8, wiegand);
	num_to_bytebits(cardno, 16, wiegand+8);
	wiegand_add_parity(out,  wiegand, sizeof(wiegand) );
}
// static void calc33(uint16_t fc, uint32_t cardno,  uint8_t *out){
// }
static void calc34(uint16_t fc, uint32_t cardno, uint8_t *out){
	uint8_t wiegand[32];
	num_to_bytebits(fc, 16, wiegand);
	num_to_bytebits(cardno, 16, wiegand + 16);
  	wiegand_add_parity(out,  wiegand, sizeof(wiegand) );	
}
// static void calc35(uint16_t fc, uint32_t cardno,  uint8_t *out){
	// *lo = ((cardno & 0xFFFFF) << 1) | fc << 21; 
	// *hi = (1 << 5) | ((fc >> 11) & 1);  
// }
static void calc37S(uint16_t fc, uint32_t cardno, uint8_t *out){
	// FC 2 - 17   - 16 bit  
	// cardno 18 - 36  - 19 bit
	// Even P1   1 - 19
	// Odd  P37  19 - 36
	uint8_t wiegand[35];
	num_to_bytebits(fc, 16, wiegand);
	num_to_bytebits(cardno, 19, wiegand + 16);
  	wiegand_add_parity(out,  wiegand, sizeof(wiegand) );
}
static void calc37H(uint64_t cardno, uint8_t *out){
	// SC NONE
	// cardno 1-35 34 bits 
	// Even Parity  0th bit  1-18
	// Odd  Parity 36th bit 19-35
	uint8_t wiegand[37];
	num_to_bytebits( (uint32_t)(cardno >> 32), 2, wiegand);
	num_to_bytebits( (uint32_t)(cardno >> 0), 32, wiegand + 2);
  	wiegand_add_parity(out,  wiegand, sizeof(wiegand) );
	
	printf("%x %x\n", (uint32_t)(cardno >> 32), (uint32_t)cardno );
}
// static void calc40(uint64_t cardno,  uint8_t *out){
	// cardno = (cardno & 0xFFFFFFFFFF);
	// *lo = ((cardno & 0xFFFFFFFF) << 1 ); 
	// *hi = (cardno >> 31);  
// }

void calcWiegand(uint8_t fmtlen, uint16_t fc, uint64_t cardno, uint8_t *bits){
	 uint32_t cn32 = (cardno & 0xFFFFFFFF);
	 switch ( fmtlen ) {
		case 26: calc26(fc, cn32, bits); break;
		// case 33 : calc33(fc, cn32, bits); break;
		case 34: calc34(fc, cn32, bits); break;
		// case 35 : calc35(fc, cn32, bits); break;
		case 37: calc37S(fc, cn32, bits); break;
		case 38: calc37H(cardno, bits); break;
		// case 40 : calc40(cardno, bits);	break;
		// case 44 : { break; }
		// case 84 : { break; }
		default: break;
	}
}	

int CmdHIDWiegand(const char *Cmd) {
	uint32_t oem = 0, fc = 0;
	uint64_t cardnum = 0;
	uint64_t blocks = 0, wiegand = 0; 

	uint8_t bits[BITS];
	uint8_t *bs = bits;
	memset(bs, 0, sizeof(bits));
	
	uint8_t ctmp = param_getchar(Cmd, 0);
	if ( strlen(Cmd) == 0 || strlen(Cmd) < 3 || ctmp == 'H' || ctmp == 'h' ) return usage_lf_hid_wiegand();

	oem = param_get8(Cmd, 0);
	fc = param_get32ex(Cmd, 1, 0, 10);
	cardnum = param_get64ex(Cmd, 2, 0, 10);

	uint8_t fmtlen[] = {26,33,34,35,37,38,40};
	
	PrintAndLog("HID | OEM | FC   | CN      |  Wiegand  |  HID Formatted");
	PrintAndLog("----+-----+------+---------+-----------+--------------------");
	for (uint8_t i = 0; i < sizeof(fmtlen); i++){
		memset(bits, 0x00, sizeof(bits));
		calcWiegand( fmtlen[i], fc, cardnum, bs);
		printf("ice:: %s \n", sprint_bin(bs, fmtlen[i]));
		wiegand = (uint64_t)bytebits_to_byte(bs, 32) << 32 | bytebits_to_byte(bs+32, 32);
		
		addHIDMarker(fmtlen[i], bs);	
		printf("ice:: %s\n", sprint_bin(bs, BITS));
		blocks = (uint64_t)bytebits_to_byte(bs+32, 32) << 32 | bytebits_to_byte(bs+64, 32);
		uint8_t shifts = 64-fmtlen[i];		
		wiegand >>= shifts;
		
		PrintAndLog(" %u | %03u | %03u  | %" PRIu64 "  | %" PRIX64 "  |  %" PRIX64 ,
			fmtlen[i],
			oem,
			fc,
			cardnum,
			wiegand,
			blocks
			);
	}
	PrintAndLog("----+-----+-----+-------+-----------+--------------------");
	return 0;
}

int CmdHIDBrute(const char *Cmd){
	
	bool errors = false;
	uint32_t fc = 0, cn = 0, delay = 1000;
	uint8_t fmtlen = 0;
	uint8_t bits[96];
	uint8_t *bs = bits;
	memset(bs, 0, sizeof(bits));
	uint8_t cmdp = 0;
		
	while(param_getchar(Cmd, cmdp) != 0x00 && !errors) {
		switch(param_getchar(Cmd, cmdp)) {
		case 'h':
		case 'H':
			return usage_lf_hid_brute();
		case 'f':
		case 'F':
		  	fc =  param_get32ex(Cmd ,cmdp+1, 0, 10);
			if ( !fc )
				errors = true;
			cmdp += 2;
			break;
		case 'd':
		case 'D':
			// delay between attemps,  defaults to 1000ms. 
			delay = param_get32ex(Cmd, cmdp+1, 1000, 10);
			cmdp += 2;
			break;
		case 'c':
		case 'C':
			cn = param_get32ex(Cmd, cmdp+1, 0, 10);
			// truncate cardnumber.
			cn &= 0xFFFF;
			cmdp += 2;
			break;
		case 'a':
		case 'A':
			fmtlen = param_get8(Cmd, cmdp+1);			
			cmdp += 2;
			bool is_ftm_ok = FALSE;
			uint8_t ftms[] = {26,33,34,35,37};
			for ( uint8_t i = 0; i < sizeof(ftms); i++){
				if ( ftms[i] == fmtlen ) {
					is_ftm_ok = TRUE;
				}
			}
			// negated
			errors = !is_ftm_ok;
			break;
		default:
			PrintAndLog("Unknown parameter '%c'", param_getchar(Cmd, cmdp));
			errors = true;
			break;
		}
	}
	if ( fc == 0 ) errors = true;
	if ( errors ) return usage_lf_hid_brute();
	
	PrintAndLog("Brute-forcing HID reader");
	PrintAndLog("Press pm3-button to abort simulation or run another command");
	
	uint16_t up = cn;
	uint16_t down = cn;
	
	for (;;){
		
		if ( offline ) {
			printf("Device offline\n");
			return  2;
		}
		
		if (ukbhit()) {
			PrintAndLog("aborted via keyboard!");
			return sendPing();
		}
		
		// Do one up
		if ( up < 0xFFFF )
			if ( !sendTry(fmtlen, fc, up++, delay, bs)) return 1;
		
		// Do one down  (if cardnumber is given)
		if ( cn > 1 )
			if ( down > 1 )
				if ( !sendTry(fmtlen, fc, --down, delay, bs)) return 1;
	}
	return 0;
}

static command_t CommandTable[] = {
	{"help",    CmdHelp,        1, "This help"},
	{"fskdemod",CmdHIDDemodFSK, 0, "Realtime HID FSK demodulator"},
	{"sim",     CmdHIDSim,      0, "HID tag simulator"},
	{"clone",   CmdHIDClone,    0, "Clone HID to T55x7"},
	{"wiegand", CmdHIDWiegand,  1, "Convert facility code/card number to Wiegand code"},
	{"brute",   CmdHIDBrute,	0, "Bruteforce card number against reader"},
	{NULL, NULL, 0, NULL}
};

int CmdLFHID(const char *Cmd) {
	clearCommandBuffer();
	CmdsParse(CommandTable, Cmd);
	return 0;
}

int CmdHelp(const char *Cmd) {
	CmdsHelp(CommandTable);
	return 0;
}
Commit	Line	Data
a553f267	1	//-----------------------------------------------------------------------------
	2	// Copyright (C) 2010 iZsh <izsh at fail0verflow.com>
	3	//
	4	// This code is licensed to you under the terms of the GNU GPL, version 2 or,
	5	// at your option, any later version. See the LICENSE.txt file for the text of
	6	// the license.
	7	//-----------------------------------------------------------------------------
	8	// Low frequency HID commands
	9	//-----------------------------------------------------------------------------
	10
7fe9b0b7	11	#include "cmdlfhid.h"
7fe9b0b7	12
464f6dc5	13	#ifndef BITS
	14	# define BITS 96
	15	#endif
	16
7fe9b0b7	17	static int CmdHelp(const char *Cmd);
f4fbfb83	18
3a532acf	19	int usage_lf_hid_wiegand(void){
e13ccb6b	20	PrintAndLog("This command converts facility code/card number to Wiegand code");
758f5ee3	21	PrintAndLog("Usage: lf hid wiegand [h] [OEM] [FC] [CN]");
758f5ee3	22
f4fbfb83	23	PrintAndLog("Options:");
e13ccb6b	24	PrintAndLog(" h - This help");
758f5ee3	25	PrintAndLog(" OEM - OEM number / site code");
e13ccb6b	26	PrintAndLog(" FC - facility code");
e13ccb6b	27	PrintAndLog(" CN - card number");
f4fbfb83	28	PrintAndLog("Examples:");
3a532acf	29	PrintAndLog(" lf hid wiegand 0 101 2001");
	30	return 0;
	31	}
758f5ee3	32	int usage_lf_hid_sim(void){
	33	PrintAndLog("HID Tag simulator");
	34	PrintAndLog("");
	35	PrintAndLog("Usage: lf hid sim [h] [ID]");
	36	PrintAndLog("Options:");
	37	PrintAndLog(" h - This help");
	38	PrintAndLog(" ID - HID id");
	39	PrintAndLog("Examples:");
	40	PrintAndLog(" lf hid sim 224");
	41	return 0;
	42	}
	43	int usage_lf_hid_clone(void){
	44	PrintAndLog("Clone HID to T55x7. Tag must be on antenna. ");
	45	PrintAndLog("");
	46	PrintAndLog("Usage: lf hid clone [h] [ID] <L>");
	47	PrintAndLog("Options:");
	48	PrintAndLog(" h - This help");
	49	PrintAndLog(" ID - HID id");
	50	PrintAndLog(" L - 84bit ID");
	51	PrintAndLog("Examples:");
	52	PrintAndLog(" lf hid clone 224");
	53	PrintAndLog(" lf hid clone 224 L");
	54	return 0;
	55	}
3a532acf	56	int usage_lf_hid_brute(void){
e13ccb6b	57	PrintAndLog("Enables bruteforce of HID readers with specified facility code.");
89603cbd	58	PrintAndLog("This is a attack against reader. if cardnumber is given, it starts with it and goes up / down one step");
89603cbd	59	PrintAndLog("if cardnumber is not given, it starts with 1 and goes up to 65535");
3a532acf	60	PrintAndLog("");
89603cbd	61	PrintAndLog("Usage: lf hid brute [h] a <format> f <facility-code> c <cardnumber> d <delay>");
3a532acf	62	PrintAndLog("Options :");
89603cbd	63	PrintAndLog(" h : This help");
	64	PrintAndLog(" a <format> : 26\|33\|34\|35\|37\|40\|44\|84");
	65	PrintAndLog(" f <facility-code> : 8-bit value HID facility code");
	66	PrintAndLog(" c <cardnumber> : (optional) cardnumber to start with, max 65535");
	67	PrintAndLog(" d <delay> : delay betweens attempts in ms. Default 1000ms");
3a532acf	68	PrintAndLog("");
89603cbd	69	PrintAndLog("Samples:");
	70	PrintAndLog(" lf hid brute a 26 f 224");
	71	PrintAndLog(" lf hid brute a 26 f 21 d 2000");
	72	PrintAndLog(" lf hid brute a 26 f 21 c 200 d 2000");
f4fbfb83	73	return 0;
f4fbfb83	74	}
758f5ee3	75
89603cbd	76	static int sendPing(void){
	77	UsbCommand ping = {CMD_PING, {1, 2, 3}};
	78	SendCommand(&ping);
	79	SendCommand(&ping);
	80	SendCommand(&ping);
	81	clearCommandBuffer();
	82	UsbCommand resp;
	83	if (WaitForResponseTimeout(CMD_ACK, &resp, 1000))
	84	return 0;
	85	return 1;
	86	}
	87	static bool sendTry(uint8_t fmtlen, uint32_t fc, uint32_t cn, uint32_t delay, uint8_t *bs){
	88
	89	PrintAndLog("Trying FC: %u; CN: %u", fc, cn);
	90
	91	calcWiegand( fmtlen, fc, cn, bs);
	92
	93	uint64_t arg1 = bytebits_to_byte(bs,32);
	94	uint64_t arg2 = bytebits_to_byte(bs+32,32);
	95	UsbCommand c = {CMD_HID_SIM_TAG, {arg1, arg2, 0}};
	96	clearCommandBuffer();
	97	SendCommand(&c);
	98	msleep(delay);
	99	sendPing();
	100	return TRUE;
	101	}
	102
3a532acf	103	int CmdHIDDemodFSK(const char *Cmd) {
464f6dc5	104	uint8_t findone = ( Cmd[0] == '1' ) ? 1 : 0;
f4fbfb83	105	UsbCommand c = {CMD_HID_DEMOD_FSK, {findone, 0 , 0}};
	106	clearCommandBuffer();
	107	SendCommand(&c);
	108	return 0;
7fe9b0b7	109	}
7fe9b0b7	110
3a532acf	111	int CmdHIDSim(const char *Cmd) {
f4fbfb83	112	unsigned int hi = 0, lo = 0;
f4fbfb83	113	int n = 0, i = 0;
38b20f75	114
758f5ee3	115	uint8_t ctmp = param_getchar(Cmd, 0);
	116	if ( strlen(Cmd) == 0 \|\| ctmp == 'H' \|\| ctmp == 'h' ) return usage_lf_hid_sim();
	117
f4fbfb83	118	while (sscanf(&Cmd[i++], "%1x", &n ) == 1) {
	119	hi = (hi << 4) \| (lo >> 28);
	120	lo = (lo << 4) \| (n & 0xf);
	121	}
38b20f75	122
f4fbfb83	123	PrintAndLog("Emulating tag with ID %x%16x", hi, lo);
f4fbfb83	124	PrintAndLog("Press pm3-button to abort simulation");
38b20f75	125
f4fbfb83	126	UsbCommand c = {CMD_HID_SIM_TAG, {hi, lo, 0}};
	127	clearCommandBuffer();
	128	SendCommand(&c);
	129	return 0;
38b20f75	130	}
38b20f75	131
3a532acf	132	int CmdHIDClone(const char *Cmd) {
758f5ee3	133
8cdf15c2	134	uint32_t hi2 = 0, hi = 0, lo = 0;
f4fbfb83	135	int n = 0, i = 0;
f4fbfb83	136	UsbCommand c;
38b20f75	137
758f5ee3	138	uint8_t ctmp = param_getchar(Cmd, 0);
	139	if ( strlen(Cmd) == 0 \|\| ctmp == 'H' \|\| ctmp == 'h' ) return usage_lf_hid_clone();
	140
f4fbfb83	141	if (strchr(Cmd,'l') != 0) {
	142	while (sscanf(&Cmd[i++], "%1x", &n ) == 1) {
	143	hi2 = (hi2 << 4) \| (hi >> 28);
	144	hi = (hi << 4) \| (lo >> 28);
	145	lo = (lo << 4) \| (n & 0xf);
	146	}
38b20f75	147
f4fbfb83	148	PrintAndLog("Cloning tag with long ID %x%08x%08x", hi2, hi, lo);
38b20f75	149
f4fbfb83	150	c.d.asBytes[0] = 1;
	151	} else {
	152	while (sscanf(&Cmd[i++], "%1x", &n ) == 1) {
	153	hi = (hi << 4) \| (lo >> 28);
	154	lo = (lo << 4) \| (n & 0xf);
	155	}
38b20f75	156
f4fbfb83	157	PrintAndLog("Cloning tag with ID %x%08x", hi, lo);
38b20f75	158
f4fbfb83	159	hi2 = 0;
	160	c.d.asBytes[0] = 0;
	161	}
ec09b62d	162
f4fbfb83	163	c.cmd = CMD_HID_CLONE_TAG;
	164	c.arg[0] = hi2;
	165	c.arg[1] = hi;
	166	c.arg[2] = lo;
	167
	168	clearCommandBuffer();
	169	SendCommand(&c);
	170	return 0;
ec09b62d	171	}
758f5ee3	172	// struct to handle wiegand
	173	typedef struct {
	174	uint8_t FormatLen;
	175	uint8_t SiteCode;
	176	uint8_t FacilityCode;
	177	uint8_t CardNumber;
	178	uint8_t* Wiegand;
	179	size_t Wiegand_n;
	180	} wiegand_t;
	181
fbc2bace	182	static void addHIDMarker(uint8_t fmtlen, uint8_t *out) {
464f6dc5	183	// temp array
	184	uint8_t arr[BITS];
	185	memset(arr, 0, BITS);
	186
	187	// copy inpu
	188	uint8_t pos = sizeof(arr)-fmtlen;
	189	memcpy(arr+pos, out, fmtlen);
	190
a0a61c91	191	switch(fmtlen) {
fbc2bace	192	case 26:{
464f6dc5	193	// start sentinel, BITS-bit 27 = 1
	194	arr[BITS-27] = 1;
	195	// fmt smaller than 37 used, bit37 = 1
	196	arr[BITS-38] = 1;
	197	memcpy(out, arr, BITS);
a0a61c91	198	break;
fbc2bace	199	}
a0a61c91	200	case 34:
464f6dc5	201	// start sentinel, BITS-bit 27 = 1
	202	arr[BITS-35] = 1;
	203
	204	// fmt smaller than 37 used, bit37 = 1
	205	arr[BITS-38] = 1;
	206	memcpy(out, arr, BITS);
a0a61c91	207	break;
	208	default:break;
	209	}
	210	}
758f5ee3	211
758f5ee3	212	// static void getParity34(uint32_t hi, uint32_t lo){
	213	// uint32_t result = 0;
	214	// int i;
	215
	216	// // even parity
	217	// for (i = 7;i >= 0;i--)
	218	// result ^= (*hi >> i) & i;
	219	// for (i = 31;i >= 24;i--)
	220	// result ^= (*lo >> i) & 1;
	221
	222	// *hi \|= result << 2;
	223
	224	// // odd parity bit
	225	// result = 0;
	226	// for (i = 23;i >= 1;i--)
	227	// result ^= (*lo >> i) & 1;
	228
	229	// *lo \|= !result;
	230	// }
758f5ee3	231	// static void getParity37H(uint32_t hi, uint32_t lo){
	232	// uint32_t result = 0;
	233	// int i;
	234
	235	// // even parity
	236	// for (i = 4;i >= 0;i--)
	237	// result ^= (*hi >> i) & 1;
	238	// for (i = 31;i >= 20;i--)
	239	// result ^= (*lo >> i) & 1;
	240	// *hi \|= result << 4;
	241
	242	// // odd parity
	243	// result = 0;
	244	// for (i = 19;i >= 1;i--)
	245	// result ^= (*lo >> i) & 1;
	246	// *lo \|= result;
	247	// }
	248
fbc2bace	249	//static void calc26(uint16_t fc, uint32_t cardno, uint8_t *out){
a0a61c91	250	static void calc26(uint16_t fc, uint32_t cardno, uint8_t *out){
758f5ee3	251	uint8_t wiegand[24];
	252	num_to_bytebits(fc, 8, wiegand);
	253	num_to_bytebits(cardno, 16, wiegand+8);
	254	wiegand_add_parity(out, wiegand, sizeof(wiegand) );
f4fbfb83	255	}
fbc2bace	256	// static void calc33(uint16_t fc, uint32_t cardno, uint8_t *out){
758f5ee3	257	// }
a0a61c91	258	static void calc34(uint16_t fc, uint32_t cardno, uint8_t *out){
	259	uint8_t wiegand[32];
	260	num_to_bytebits(fc, 16, wiegand);
	261	num_to_bytebits(cardno, 16, wiegand + 16);
	262	wiegand_add_parity(out, wiegand, sizeof(wiegand) );
	263	}
fbc2bace	264	// static void calc35(uint16_t fc, uint32_t cardno, uint8_t *out){
758f5ee3	265	// *lo = ((cardno & 0xFFFFF) << 1) \| fc << 21;
	266	// *hi = (1 << 5) \| ((fc >> 11) & 1);
	267	// }
a0a61c91	268	static void calc37S(uint16_t fc, uint32_t cardno, uint8_t *out){
	269	// FC 2 - 17 - 16 bit
	270	// cardno 18 - 36 - 19 bit
	271	// Even P1 1 - 19
	272	// Odd P37 19 - 36
	273	uint8_t wiegand[35];
	274	num_to_bytebits(fc, 16, wiegand);
	275	num_to_bytebits(cardno, 19, wiegand + 16);
	276	wiegand_add_parity(out, wiegand, sizeof(wiegand) );
	277	}
	278	static void calc37H(uint64_t cardno, uint8_t *out){
	279	// SC NONE
	280	// cardno 1-35 34 bits
	281	// Even Parity 0th bit 1-18
	282	// Odd Parity 36th bit 19-35
	283	uint8_t wiegand[37];
	284	num_to_bytebits( (uint32_t)(cardno >> 32), 2, wiegand);
	285	num_to_bytebits( (uint32_t)(cardno >> 0), 32, wiegand + 2);
	286	wiegand_add_parity(out, wiegand, sizeof(wiegand) );
	287
	288	printf("%x %x\n", (uint32_t)(cardno >> 32), (uint32_t)cardno );
	289	}
fbc2bace	290	// static void calc40(uint64_t cardno, uint8_t *out){
758f5ee3	291	// cardno = (cardno & 0xFFFFFFFFFF);
	292	// *lo = ((cardno & 0xFFFFFFFF) << 1 );
	293	// *hi = (cardno >> 31);
	294	// }
	295
89603cbd	296	void calcWiegand(uint8_t fmtlen, uint16_t fc, uint64_t cardno, uint8_t *bits){
a0a61c91	297	uint32_t cn32 = (cardno & 0xFFFFFFFF);
a0a61c91	298	switch ( fmtlen ) {
fbc2bace	299	case 26: calc26(fc, cn32, bits); break;
	300	// case 33 : calc33(fc, cn32, bits); break;
	301	case 34: calc34(fc, cn32, bits); break;
	302	// case 35 : calc35(fc, cn32, bits); break;
	303	case 37: calc37S(fc, cn32, bits); break;
	304	case 38: calc37H(cardno, bits); break;
	305	// case 40 : calc40(cardno, bits); break;
758f5ee3	306	// case 44 : { break; }
758f5ee3	307	// case 84 : { break; }
a0a61c91	308	default: break;
a0a61c91	309	}
3a532acf	310	}
	311
	312	int CmdHIDWiegand(const char *Cmd) {
758f5ee3	313	uint32_t oem = 0, fc = 0;
3a532acf	314	uint64_t cardnum = 0;
a0a61c91	315	uint64_t blocks = 0, wiegand = 0;
758f5ee3	316
464f6dc5	317	uint8_t bits[BITS];
758f5ee3	318	uint8_t *bs = bits;
	319	memset(bs, 0, sizeof(bits));
	320
3a532acf	321	uint8_t ctmp = param_getchar(Cmd, 0);
e1ad67ea	322	if ( strlen(Cmd) == 0 \|\| strlen(Cmd) < 3 \|\| ctmp == 'H' \|\| ctmp == 'h' ) return usage_lf_hid_wiegand();
3a532acf	323
	324	oem = param_get8(Cmd, 0);
	325	fc = param_get32ex(Cmd, 1, 0, 10);
	326	cardnum = param_get64ex(Cmd, 2, 0, 10);
	327
a0a61c91	328	uint8_t fmtlen[] = {26,33,34,35,37,38,40};
758f5ee3	329
a0a61c91	330	PrintAndLog("HID \| OEM \| FC \| CN \| Wiegand \| HID Formatted");
	331	PrintAndLog("----+-----+------+---------+-----------+--------------------");
	332	for (uint8_t i = 0; i < sizeof(fmtlen); i++){
	333	memset(bits, 0x00, sizeof(bits));
	334	calcWiegand( fmtlen[i], fc, cardnum, bs);
464f6dc5	335	printf("ice:: %s \n", sprint_bin(bs, fmtlen[i]));
a0a61c91	336	wiegand = (uint64_t)bytebits_to_byte(bs, 32) << 32 \| bytebits_to_byte(bs+32, 32);
fbc2bace	337
fbc2bace	338	addHIDMarker(fmtlen[i], bs);
464f6dc5	339	printf("ice:: %s\n", sprint_bin(bs, BITS));
464f6dc5	340	blocks = (uint64_t)bytebits_to_byte(bs+32, 32) << 32 \| bytebits_to_byte(bs+64, 32);
a0a61c91	341	uint8_t shifts = 64-fmtlen[i];
	342	wiegand >>= shifts;
	343
9c624f67	344	PrintAndLog(" %u \| %03u \| %03u \| %" PRIu64 " \| %" PRIX64 " \| %" PRIX64 ,
a0a61c91	345	fmtlen[i],
e92948c6	346	oem,
758f5ee3	347	fc,
758f5ee3	348	cardnum,
a0a61c91	349	wiegand,
a0a61c91	350	blocks
758f5ee3	351	);
3a532acf	352	}
e92948c6	353	PrintAndLog("----+-----+-----+-------+-----------+--------------------");
3a532acf	354	return 0;
	355	}
	356
	357	int CmdHIDBrute(const char *Cmd){
	358
89603cbd	359	bool errors = false;
	360	uint32_t fc = 0, cn = 0, delay = 1000;
	361	uint8_t fmtlen = 0;
758f5ee3	362	uint8_t bits[96];
	363	uint8_t *bs = bits;
	364	memset(bs, 0, sizeof(bits));
89603cbd	365	uint8_t cmdp = 0;
	366
	367	while(param_getchar(Cmd, cmdp) != 0x00 && !errors) {
	368	switch(param_getchar(Cmd, cmdp)) {
	369	case 'h':
	370	case 'H':
	371	return usage_lf_hid_brute();
	372	case 'f':
	373	case 'F':
	374	fc = param_get32ex(Cmd ,cmdp+1, 0, 10);
	375	if ( !fc )
	376	errors = true;
	377	cmdp += 2;
	378	break;
	379	case 'd':
	380	case 'D':
	381	// delay between attemps, defaults to 1000ms.
	382	delay = param_get32ex(Cmd, cmdp+1, 1000, 10);
	383	cmdp += 2;
	384	break;
	385	case 'c':
	386	case 'C':
	387	cn = param_get32ex(Cmd, cmdp+1, 0, 10);
	388	// truncate cardnumber.
	389	cn &= 0xFFFF;
	390	cmdp += 2;
	391	break;
	392	case 'a':
	393	case 'A':
	394	fmtlen = param_get8(Cmd, cmdp+1);
	395	cmdp += 2;
7aa24806	396	bool is_ftm_ok = FALSE;
89603cbd	397	uint8_t ftms[] = {26,33,34,35,37};
	398	for ( uint8_t i = 0; i < sizeof(ftms); i++){
	399	if ( ftms[i] == fmtlen ) {
7aa24806	400	is_ftm_ok = TRUE;
89603cbd	401	}
89603cbd	402	}
7aa24806	403	// negated
7aa24806	404	errors = !is_ftm_ok;
89603cbd	405	break;
	406	default:
	407	PrintAndLog("Unknown parameter '%c'", param_getchar(Cmd, cmdp));
	408	errors = true;
	409	break;
3a532acf	410	}
3a532acf	411	}
89603cbd	412	if ( fc == 0 ) errors = true;
89603cbd	413	if ( errors ) return usage_lf_hid_brute();
3a532acf	414
e13ccb6b	415	PrintAndLog("Brute-forcing HID reader");
3a532acf	416	PrintAndLog("Press pm3-button to abort simulation or run another command");
89603cbd	417
	418	uint16_t up = cn;
	419	uint16_t down = cn;
	420
	421	for (;;){
	422
	423	if ( offline ) {
	424	printf("Device offline\n");
	425	return 2;
	426	}
	427
3a532acf	428	if (ukbhit()) {
3a532acf	429	PrintAndLog("aborted via keyboard!");
89603cbd	430	return sendPing();
3a532acf	431	}
3a532acf	432
89603cbd	433	// Do one up
	434	if ( up < 0xFFFF )
	435	if ( !sendTry(fmtlen, fc, up++, delay, bs)) return 1;
	436
	437	// Do one down (if cardnumber is given)
	438	if ( cn > 1 )
	439	if ( down > 1 )
	440	if ( !sendTry(fmtlen, fc, --down, delay, bs)) return 1;
3a532acf	441	}
f4fbfb83	442	return 0;
	443	}
	444
	445	static command_t CommandTable[] = {
e13ccb6b	446	{"help", CmdHelp, 1, "This help"},
89603cbd	447	{"fskdemod",CmdHIDDemodFSK, 0, "Realtime HID FSK demodulator"},
	448	{"sim", CmdHIDSim, 0, "HID tag simulator"},
	449	{"clone", CmdHIDClone, 0, "Clone HID to T55x7"},
8cdf15c2	450	{"wiegand", CmdHIDWiegand, 1, "Convert facility code/card number to Wiegand code"},
89603cbd	451	{"brute", CmdHIDBrute, 0, "Bruteforce card number against reader"},
f4fbfb83	452	{NULL, NULL, 0, NULL}
7fe9b0b7	453	};
7fe9b0b7	454
f4fbfb83	455	int CmdLFHID(const char *Cmd) {
4c36581b	456	clearCommandBuffer();
f4fbfb83	457	CmdsParse(CommandTable, Cmd);
f4fbfb83	458	return 0;
7fe9b0b7	459	}
7fe9b0b7	460
f4fbfb83	461	int CmdHelp(const char *Cmd) {
	462	CmdsHelp(CommandTable);
	463	return 0;
7fe9b0b7	464	}