[proxmark3-svn] / client / loclass / cipherutils.c

/*****************************************************************************
 * This file is part of iClassCipher. It is a reconstructon of the cipher engine
 * used in iClass, and RFID techology.
 *
 * The implementation is based on the work performed by
 * Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
 * Milosch Meriac in the paper "Dismantling IClass".
 *
 * Copyright (C) 2014 Martin Holst Swende
 *
 * This is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as published
 * by the Free Software Foundation.
 *
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with IClassCipher.  If not, see <http://www.gnu.org/licenses/>.
 ****************************************************************************/

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "fileutils.h"
#include "cipherutils.h"
/**
 *
 * @brief Return and remove the first bit (x0) in the stream : <x0 x1 x2 x3 ... xn >
 * @param stream
 * @return
 */
bool headBit( BitstreamIn *stream)
{
	int bytepos = stream->position >> 3; // divide by 8
	int bitpos = (stream->position++) & 7; // mask out 00000111
	return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
}
/**
 * @brief Return and remove the last bit (xn) in the stream: <x0 x1 x2 ... xn>
 * @param stream
 * @return
 */
bool tailBit( BitstreamIn *stream)
{
	int bitpos = stream->numbits -1 - (stream->position++);

	int bytepos= bitpos >> 3;
	bitpos &= 7;
	return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
}
/**
 * @brief Pushes bit onto the stream
 * @param stream
 * @param bit
 */
void pushBit( BitstreamOut* stream, bool bit)
{
	int bytepos = stream->position >> 3; // divide by 8
	int bitpos = stream->position & 7;
	*(stream->buffer+bytepos) |= (bit & 1) <<  (7 - bitpos);
	stream->position++;
	stream->numbits++;
}

/**
 * @brief Pushes the lower six bits onto the stream
 * as b0 b1 b2 b3 b4 b5 b6
 * @param stream
 * @param bits
 */
void push6bits( BitstreamOut* stream, uint8_t bits)
{
	pushBit(stream, bits & 0x20);
	pushBit(stream, bits & 0x10);
	pushBit(stream, bits & 0x08);
	pushBit(stream, bits & 0x04);
	pushBit(stream, bits & 0x02);
	pushBit(stream, bits & 0x01);
}

/**
 * @brief bitsLeft
 * @param stream
 * @return number of bits left in stream
 */
int bitsLeft( BitstreamIn *stream)
{
	return stream->numbits - stream->position;
}
/**
 * @brief numBits
 * @param stream
 * @return Number of bits stored in stream
 */
int numBits(BitstreamOut *stream)
{
	return stream->numbits;
}

void x_num_to_bytes(uint64_t n, size_t len, uint8_t* dest)
{
	while (len--) {
		dest[len] = (uint8_t) n;
		n >>= 8;
	}
}

uint64_t x_bytes_to_num(uint8_t* src, size_t len)
{
	uint64_t num = 0;
	while (len--)
	{
		num = (num << 8) | (*src);
		src++;
	}
	return num;
}
uint8_t reversebytes(uint8_t b) {
	b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
	b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
	b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
   return b;
}
void reverse_arraybytes(uint8_t* arr, size_t len)
{
	uint8_t i;
	for( i =0; i< len ; i++)
	{
		arr[i] = reversebytes(arr[i]);
	}
}
void reverse_arraycopy(uint8_t* arr, uint8_t* dest, size_t len)
{
	uint8_t i;
	for( i =0; i< len ; i++)
	{
		dest[i] = reversebytes(arr[i]);
	}
}

void printarr(char * name, uint8_t* arr, int len)
{
	int cx;
	size_t outsize = 40+strlen(name)+len*5;
	char* output = malloc(outsize);
	memset(output, 0,outsize);

	int i ;
	cx = snprintf(output,outsize, "uint8_t %s[] = {", name);
	for(i =0 ;  i< len ; i++)
	{
		cx += snprintf(output+cx,outsize-cx,"0x%02x,",*(arr+i));//5 bytes per byte
	}
	cx += snprintf(output+cx,outsize-cx,"};");
	prnlog(output);
}

void printvar(char * name, uint8_t* arr, int len)
{
	int cx;
	size_t outsize = 40+strlen(name)+len*2;
	char* output = malloc(outsize);
	memset(output, 0,outsize);

	int i ;
	cx = snprintf(output,outsize,"%s = ", name);
	for(i =0 ;  i< len ; i++)
	{
		cx += snprintf(output+cx,outsize-cx,"%02x",*(arr+i));//2 bytes per byte
	}

	prnlog(output);
}

void printarr_human_readable(char * title, uint8_t* arr, int len)
{
	int cx;
	size_t outsize = 100+strlen(title)+len*4;
	char* output = malloc(outsize);
	memset(output, 0,outsize);


	int i;
	cx = snprintf(output,outsize,  "\n\t%s\n", title);
	for(i =0 ;  i< len ; i++)
	{
		if(i % 16 == 0)
			cx += snprintf(output+cx,outsize-cx,"\n%02x| ", i );
		cx += snprintf(output+cx,outsize-cx, "%02x ",*(arr+i));
	}
	prnlog(output);
	free(output);
}

//-----------------------------
// Code for testing below
//-----------------------------


int testBitStream()
{
	uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
	uint8_t output [] = {0,0,0,0,0,0,0,0};
	BitstreamIn in = { input, sizeof(input) * 8,0};
	BitstreamOut out ={ output, 0,0}
					  ;
	while(bitsLeft(&in) > 0)
	{
		pushBit(&out, headBit(&in));
		//printf("Bits left: %d\n", bitsLeft(&in));
		//printf("Bits out: %d\n", numBits(&out));
	}
	if(memcmp(input, output, sizeof(input)) == 0)
	{
		prnlog("    Bitstream test 1 ok");
	}else
	{
		prnlog("    Bitstream test 1 failed");
		uint8_t i;
		for(i = 0 ; i < sizeof(input) ; i++)
		{
			prnlog("    IN %02x, OUT %02x", input[i], output[i]);
		}
		return 1;
	}
	return 0;
}

int testReversedBitstream()
{
	uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
	uint8_t reverse [] = {0,0,0,0,0,0,0,0};
	uint8_t output [] = {0,0,0,0,0,0,0,0};
	BitstreamIn in = { input, sizeof(input) * 8,0};
	BitstreamOut out ={ output, 0,0};
	BitstreamIn reversed_in ={ reverse, sizeof(input)*8,0};
	BitstreamOut reversed_out ={ reverse,0 ,0};

	while(bitsLeft(&in) > 0)
	{
		pushBit(&reversed_out, tailBit(&in));
	}
	while(bitsLeft(&reversed_in) > 0)
	{
		pushBit(&out, tailBit(&reversed_in));
	}
	if(memcmp(input, output, sizeof(input)) == 0)
	{
		prnlog("    Bitstream test 2 ok");
	}else
	{
		prnlog("    Bitstream test 2 failed");
		uint8_t i;
		for(i = 0 ; i < sizeof(input) ; i++)
		{
			prnlog("    IN %02x, MIDDLE: %02x, OUT %02x", input[i],reverse[i], output[i]);
		}
		return 1;
	}
	return 0;
}


int testCipherUtils(void)
{
	prnlog("[+] Testing some internals...");
	int retval = 0;
	retval |= testBitStream();
	retval |= testReversedBitstream();
	return retval;
}
Commit	Line	Data
a66fca86 AD	1	/*****************************************************************************
	2	* This file is part of iClassCipher. It is a reconstructon of the cipher engine
	3	* used in iClass, and RFID techology.
	4	*
	5	* The implementation is based on the work performed by
	6	* Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
	7	* Milosch Meriac in the paper "Dismantling IClass".
	8	*
	9	* Copyright (C) 2014 Martin Holst Swende
	10	*
	11	* This is free software: you can redistribute it and/or modify
	12	* it under the terms of the GNU General Public License version 2 as published
	13	* by the Free Software Foundation.
	14	*
	15	* This file is distributed in the hope that it will be useful,
	16	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	17	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	18	* GNU General Public License for more details.
	19	*
	20	* You should have received a copy of the GNU General Public License
	21	* along with IClassCipher. If not, see <http://www.gnu.org/licenses/>.
	22	****************************************************************************/
	23
a66fca86 AD	24	#include <stdint.h>
	25	#include <stdio.h>
	26	#include <string.h>
3ad48540 MHS	27	#include "fileutils.h"
3ad48540 MHS	28	#include "cipherutils.h"
a66fca86 AD	29	/**
	30	*
	31	* @brief Return and remove the first bit (x0) in the stream : <x0 x1 x2 x3 ... xn >
	32	* @param stream
	33	* @return
	34	*/
	35	bool headBit( BitstreamIn *stream)
	36	{
	37	int bytepos = stream->position >> 3; // divide by 8
	38	int bitpos = (stream->position++) & 7; // mask out 00000111
	39	return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
	40	}
	41	/**
	42	* @brief Return and remove the last bit (xn) in the stream: <x0 x1 x2 ... xn>
	43	* @param stream
	44	* @return
	45	*/
	46	bool tailBit( BitstreamIn *stream)
	47	{
	48	int bitpos = stream->numbits -1 - (stream->position++);
	49
	50	int bytepos= bitpos >> 3;
	51	bitpos &= 7;
	52	return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
	53	}
	54	/**
	55	* @brief Pushes bit onto the stream
	56	* @param stream
	57	* @param bit
	58	*/
	59	void pushBit( BitstreamOut* stream, bool bit)
	60	{
	61	int bytepos = stream->position >> 3; // divide by 8
	62	int bitpos = stream->position & 7;
	63	*(stream->buffer+bytepos) \|= (bit & 1) << (7 - bitpos);
	64	stream->position++;
	65	stream->numbits++;
	66	}
	67
	68	/**
	69	* @brief Pushes the lower six bits onto the stream
	70	* as b0 b1 b2 b3 b4 b5 b6
	71	* @param stream
	72	* @param bits
	73	*/
	74	void push6bits( BitstreamOut* stream, uint8_t bits)
	75	{
	76	pushBit(stream, bits & 0x20);
	77	pushBit(stream, bits & 0x10);
	78	pushBit(stream, bits & 0x08);
	79	pushBit(stream, bits & 0x04);
	80	pushBit(stream, bits & 0x02);
	81	pushBit(stream, bits & 0x01);
	82	}
	83
	84	/**
	85	* @brief bitsLeft
	86	* @param stream
	87	* @return number of bits left in stream
	88	*/
	89	int bitsLeft( BitstreamIn *stream)
	90	{
	91	return stream->numbits - stream->position;
	92	}
93	/**
94	* @brief numBits
95	* @param stream
96	* @return Number of bits stored in stream
97	*/
98	int numBits(BitstreamOut *stream)
99	{
100	return stream->numbits;
101	}
102
3ad48540 MHS	103	void x_num_to_bytes(uint64_t n, size_t len, uint8_t* dest)
	104	{
	105	while (len--) {
	106	dest[len] = (uint8_t) n;
	107	n >>= 8;
	108	}
	109	}
	110
	111	uint64_t x_bytes_to_num(uint8_t* src, size_t len)
	112	{
	113	uint64_t num = 0;
	114	while (len--)
	115	{
	116	num = (num << 8) \| (*src);
	117	src++;
	118	}
	119	return num;
	120	}
a66fca86 AD	121	uint8_t reversebytes(uint8_t b) {
	122	b = (b & 0xF0) >> 4 \| (b & 0x0F) << 4;
	123	b = (b & 0xCC) >> 2 \| (b & 0x33) << 2;
	124	b = (b & 0xAA) >> 1 \| (b & 0x55) << 1;
	125	return b;
	126	}
	127	void reverse_arraybytes(uint8_t* arr, size_t len)
	128	{
	129	uint8_t i;
	130	for( i =0; i< len ; i++)
	131	{
	132	arr[i] = reversebytes(arr[i]);
	133	}
	134	}
3ad48540 MHS	135	void reverse_arraycopy(uint8_t* arr, uint8_t* dest, size_t len)
	136	{
	137	uint8_t i;
	138	for( i =0; i< len ; i++)
	139	{
	140	dest[i] = reversebytes(arr[i]);
	141	}
	142	}
	143
	144	void printarr(char * name, uint8_t* arr, int len)
	145	{
	146	int cx;
	147	size_t outsize = 40+strlen(name)+len*5;
	148	char* output = malloc(outsize);
	149	memset(output, 0,outsize);
	150
	151	int i ;
	152	cx = snprintf(output,outsize, "uint8_t %s[] = {", name);
	153	for(i =0 ; i< len ; i++)
	154	{
	155	cx += snprintf(output+cx,outsize-cx,"0x%02x,",*(arr+i));//5 bytes per byte
	156	}
	157	cx += snprintf(output+cx,outsize-cx,"};");
	158	prnlog(output);
	159	}
	160
	161	void printvar(char * name, uint8_t* arr, int len)
	162	{
	163	int cx;
	164	size_t outsize = 40+strlen(name)+len*2;
	165	char* output = malloc(outsize);
	166	memset(output, 0,outsize);
	167
	168	int i ;
	169	cx = snprintf(output,outsize,"%s = ", name);
	170	for(i =0 ; i< len ; i++)
	171	{
	172	cx += snprintf(output+cx,outsize-cx,"%02x",*(arr+i));//2 bytes per byte
	173	}
	174
	175	prnlog(output);
	176	}
	177
	178	void printarr_human_readable(char * title, uint8_t* arr, int len)
	179	{
	180	int cx;
	181	size_t outsize = 100+strlen(title)+len*4;
	182	char* output = malloc(outsize);
	183	memset(output, 0,outsize);
a66fca86 AD	184
a66fca86 AD	185
3ad48540 MHS	186	int i;
	187	cx = snprintf(output,outsize, "\n\t%s\n", title);
	188	for(i =0 ; i< len ; i++)
	189	{
	190	if(i % 16 == 0)
	191	cx += snprintf(output+cx,outsize-cx,"\n%02x\| ", i );
	192	cx += snprintf(output+cx,outsize-cx, "%02x ",*(arr+i));
	193	}
	194	prnlog(output);
ca4714cd	195	free(output);
3ad48540 MHS	196	}
3ad48540 MHS	197
a66fca86 AD	198	//-----------------------------
	199	// Code for testing below
	200	//-----------------------------
	201
	202
	203	int testBitStream()
	204	{
	205	uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
	206	uint8_t output [] = {0,0,0,0,0,0,0,0};
	207	BitstreamIn in = { input, sizeof(input) * 8,0};
	208	BitstreamOut out ={ output, 0,0}
	209	;
	210	while(bitsLeft(&in) > 0)
	211	{
	212	pushBit(&out, headBit(&in));
	213	//printf("Bits left: %d\n", bitsLeft(&in));
	214	//printf("Bits out: %d\n", numBits(&out));
	215	}
	216	if(memcmp(input, output, sizeof(input)) == 0)
	217	{
3ad48540	218	prnlog(" Bitstream test 1 ok");
a66fca86 AD	219	}else
a66fca86 AD	220	{
3ad48540	221	prnlog(" Bitstream test 1 failed");
a66fca86 AD	222	uint8_t i;
	223	for(i = 0 ; i < sizeof(input) ; i++)
	224	{
3ad48540	225	prnlog(" IN %02x, OUT %02x", input[i], output[i]);
a66fca86 AD	226	}
	227	return 1;
	228	}
	229	return 0;
	230	}
	231
	232	int testReversedBitstream()
	233	{
	234	uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
	235	uint8_t reverse [] = {0,0,0,0,0,0,0,0};
	236	uint8_t output [] = {0,0,0,0,0,0,0,0};
	237	BitstreamIn in = { input, sizeof(input) * 8,0};
	238	BitstreamOut out ={ output, 0,0};
	239	BitstreamIn reversed_in ={ reverse, sizeof(input)*8,0};
	240	BitstreamOut reversed_out ={ reverse,0 ,0};
	241
	242	while(bitsLeft(&in) > 0)
	243	{
	244	pushBit(&reversed_out, tailBit(&in));
	245	}
	246	while(bitsLeft(&reversed_in) > 0)
	247	{
	248	pushBit(&out, tailBit(&reversed_in));
	249	}
	250	if(memcmp(input, output, sizeof(input)) == 0)
	251	{
3ad48540	252	prnlog(" Bitstream test 2 ok");
a66fca86 AD	253	}else
a66fca86 AD	254	{
3ad48540	255	prnlog(" Bitstream test 2 failed");
a66fca86 AD	256	uint8_t i;
	257	for(i = 0 ; i < sizeof(input) ; i++)
	258	{
3ad48540	259	prnlog(" IN %02x, MIDDLE: %02x, OUT %02x", input[i],reverse[i], output[i]);
a66fca86 AD	260	}
	261	return 1;
	262	}
	263	return 0;
	264	}
	265
	266
	267	int testCipherUtils(void)
	268	{
3ad48540	269	prnlog("[+] Testing some internals...");
a66fca86 AD	270	int retval = 0;
	271	retval \|= testBitStream();
	272	retval \|= testReversedBitstream();
	273	return retval;
	274	}