[proxmark3-svn] / armsrc / optimized_cipher.c

/*****************************************************************************
 * WARNING
 *
 * THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
 *
 * USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
 * PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
 * AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
 *
 * THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
 *
 *****************************************************************************
 *
 * This file is part of loclass. 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, or, at your option, any later version.
 *
 * 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 loclass.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 *
 ****************************************************************************/

/**

  This file contains an optimized version of the MAC-calculation algorithm. Some measurements on
  a std laptop showed it runs in about 1/3 of the time:

	Std: 0.428962
	Opt: 0.151609

  Additionally, it is self-reliant, not requiring e.g. bitstreams from the cipherutils, thus can
  be easily dropped into a code base.

  The optimizations have been performed in the following steps:
  * Parameters passed by reference instead of by value.
  * Iteration instead of recursion, un-nesting recursive loops into for-loops.
  * Handling of bytes instead of individual bits, for less shuffling and masking
  * Less creation of "objects", structs, and instead reuse of alloc:ed memory
  * Inlining some functions via #define:s

  As a consequence, this implementation is less generic. Also, I haven't bothered documenting this.
  For a thorough documentation, check out the MAC-calculation within cipher.c instead.

  -- MHS 2015
**/

/**

  The runtime of opt_doTagMAC_2() with the MHS optimized version was 403 microseconds on Proxmark3.
  This was still to slow for some newer readers which didn't want to wait that long.

  Further optimizations to speedup the MAC calculations:
  * Optimized opt_Tt logic
  * Look up table for opt_select
  * Removing many unnecessary bit maskings (& 0x1)
  * updating state in place instead of alternating use of a second state structure
  * remove the necessity to reverse bits of input and output bytes
 
  opt_doTagMAC_2() now completes in 270 microseconds.

  -- piwi 2019
**/

#include "optimized_cipher.h"
#include <stddef.h>
#include <stdbool.h>
#include <stdint.h>
#include "string.h"

static const uint8_t opt_select_LUT[256] = {
	00, 03, 02, 01, 02, 03, 00, 01, 04, 07, 07, 04, 06, 07, 05, 04,
	01, 02, 03, 00, 02, 03, 00, 01, 05, 06, 06, 05, 06, 07, 05, 04,
	06, 05, 04, 07, 04, 05, 06, 07, 06, 05, 05, 06, 04, 05, 07, 06,
	07, 04, 05, 06, 04, 05, 06, 07, 07, 04, 04, 07, 04, 05, 07, 06,
	06, 05, 04, 07, 04, 05, 06, 07, 02, 01, 01, 02, 00, 01, 03, 02,
	03, 00, 01, 02, 00, 01, 02, 03, 07, 04, 04, 07, 04, 05, 07, 06,
	00, 03, 02, 01, 02, 03, 00, 01, 00, 03, 03, 00, 02, 03, 01, 00,
	05, 06, 07, 04, 06, 07, 04, 05, 05, 06, 06, 05, 06, 07, 05, 04,
	02, 01, 00, 03, 00, 01, 02, 03, 06, 05, 05, 06, 04, 05, 07, 06,
	03, 00, 01, 02, 00, 01, 02, 03, 07, 04, 04, 07, 04, 05, 07, 06,
	02, 01, 00, 03, 00, 01, 02, 03, 02, 01, 01, 02, 00, 01, 03, 02,
	03, 00, 01, 02, 00, 01, 02, 03, 03, 00, 00, 03, 00, 01, 03, 02,
	04, 07, 06, 05, 06, 07, 04, 05, 00, 03, 03, 00, 02, 03, 01, 00,
	01, 02, 03, 00, 02, 03, 00, 01, 05, 06, 06, 05, 06, 07, 05, 04,
	04, 07, 06, 05, 06, 07, 04, 05, 04, 07, 07, 04, 06, 07, 05, 04,
	01, 02, 03, 00, 02, 03, 00, 01, 01, 02, 02, 01, 02, 03, 01, 00
};

/********************** the table above has been generated with this code: ********
#include "util.h"
static void init_opt_select_LUT(void) {
	for (int r = 0; r < 256; r++) {
		uint8_t r_ls2 = r << 2;
		uint8_t r_and_ls2 = r & r_ls2;
		uint8_t r_or_ls2  = r | r_ls2;
		uint8_t z0 = (r_and_ls2 >> 5) ^ ((r & ~r_ls2) >> 4) ^ ( r_or_ls2 >> 3);
		uint8_t z1 = (r_or_ls2 >> 6) ^ ( r_or_ls2 >> 1) ^ (r >> 5) ^ r;
		uint8_t z2 = ((r & ~r_ls2) >> 4) ^ (r_and_ls2 >> 3) ^ r;
		opt_select_LUT[r] = (z0 & 4) | (z1 & 2) | (z2 & 1);
	}
	print_result("", opt_select_LUT, 256);
}
***********************************************************************************/

#define opt__select(x,y,r)  (4 & (((r & (r << 2)) >> 5) ^ ((r & ~(r << 2)) >> 4) ^ ( (r | r << 2) >> 3)))\
	|(2 & (((r | r << 2) >> 6) ^ ( (r | r << 2) >> 1) ^ (r >> 5) ^ r ^ ((x^y) << 1)))\
	|(1 & (((r & ~(r << 2)) >> 4) ^ ((r & (r << 2)) >> 3) ^ r ^ x))

/*
 * Some background on the expression above can be found here...
uint8_t xopt__select(bool x, bool y, uint8_t r)
{

	//r:      r0 r1 r2 r3 r4 r5 r6 r7
	//r_ls2:  r2 r3 r4 r5 r6 r7  0  0
	//                       z0
	//                          z1

//  uint8_t z0 = (r0 & r2) ^ (r1 & ~r3) ^ (r2 | r4); // <-- original
	uint8_t z0 = (r_and_ls2 >> 5) ^ ((r & ~r_ls2) >> 4) ^ ( r_or_ls2 >> 3);

//  uint8_t z1 = (r0 | r2) ^ ( r5 | r7) ^ r1 ^ r6 ^ x ^ y;  // <-- original
	uint8_t z1 = (r_or_ls2 >> 6) ^ ( r_or_ls2 >> 1) ^ (r >> 5) ^ r ^ ((x^y) << 1);

//  uint8_t z2 = (r3 & ~r5) ^ (r4 & r6 ) ^ r7 ^ x;  // <-- original
	uint8_t z2 = ((r & ~r_ls2) >> 4) ^ (r_and_ls2 >> 3) ^ r ^ x;

	return (z0 & 4) | (z1 & 2) | (z2 & 1);
}
*/

static void opt_successor(const uint8_t *k, State *s, uint8_t y) {
// #define opt_T(s) (0x1 & ((s->t >> 15) ^ (s->t >> 14) ^ (s->t >> 10) ^ (s->t >> 8) ^ (s->t >> 5) ^ (s->t >> 4)^ (s->t >> 1) ^ s->t))
	// uint8_t Tt = opt_T(s);
	uint16_t Tt = s->t & 0xc533;
	Tt = Tt ^ (Tt >> 1);
	Tt = Tt ^ (Tt >> 4);
	Tt = Tt ^ (Tt >> 10);
	Tt = Tt ^ (Tt >> 8);

	s->t = (s->t >> 1);
	s->t |= (Tt ^ (s->r >> 7) ^ (s->r >> 3)) << 15;

	uint8_t opt_B = s->b;
	opt_B ^= s->b >> 6;
	opt_B ^= s->b >> 5;
	opt_B ^= s->b >> 4;

	s->b = s->b >> 1;
	s->b |= (opt_B ^ s->r) << 7;

	uint8_t opt_select = opt_select_LUT[s->r] & 0x04;
	opt_select |= (opt_select_LUT[s->r] ^ ((Tt ^ y) << 1)) & 0x02;
	opt_select |= (opt_select_LUT[s->r] ^ Tt) & 0x01;

	uint8_t r = s->r;
	s->r = (k[opt_select] ^ s->b) + s->l ;
	s->l = s->r + r;
}

static void opt_suc(const uint8_t *k, State *s, uint8_t *in, uint8_t length, bool add32Zeroes) {
	for (int i = 0; i < length; i++) {
		uint8_t head;
		head = in[i];
		opt_successor(k, s, head);

		head >>= 1;
		opt_successor(k, s, head);

		head >>= 1;
		opt_successor(k, s, head);

		head >>= 1;
		opt_successor(k, s, head);

		head >>= 1;
		opt_successor(k, s, head);

		head >>= 1;
		opt_successor(k, s, head);

		head >>= 1;
		opt_successor(k, s, head);

		head >>= 1;
		opt_successor(k, s, head);
	}
	//For tag MAC, an additional 32 zeroes
	if (add32Zeroes) {
		for(int i = 0; i < 16; i++) {
			opt_successor(k, s, 0);
			opt_successor(k, s, 0);
		}
	}
}

static void opt_output(const uint8_t *k, State *s,  uint8_t *buffer) {
	for (uint8_t times = 0; times < 4; times++) {
		uint8_t bout = 0;
		bout |= (s->r & 0x4) >> 2;
		opt_successor(k, s, 0);
		bout |= (s->r & 0x4) >> 1;
		opt_successor(k, s, 0);
		bout |= (s->r & 0x4);
		opt_successor(k, s, 0);
		bout |= (s->r & 0x4) << 1;
		opt_successor(k, s, 0);
		bout |= (s->r & 0x4) << 2;
		opt_successor(k, s, 0);
		bout |= (s->r & 0x4) << 3;
		opt_successor(k, s, 0);
		bout |= (s->r & 0x4) << 4;
		opt_successor(k, s, 0);
		bout |= (s->r & 0x4) << 5;
		opt_successor(k, s, 0);
		buffer[times] = bout;
	}
}

static void opt_MAC(uint8_t *k, uint8_t *input, uint8_t *out) {
	State _init  =  {
		((k[0] ^ 0x4c) + 0xEC) & 0xFF,// l
		((k[0] ^ 0x4c) + 0x21) & 0xFF,// r
		0x4c, // b
		0xE012 // t
	};

	opt_suc(k, &_init, input, 12, false);
	//printf("\noutp ");
	opt_output(k, &_init, out);
}

void opt_doReaderMAC(uint8_t *cc_nr_p, uint8_t *div_key_p, uint8_t mac[4]) {
	uint8_t dest[] = {0, 0, 0, 0, 0, 0, 0, 0};
	opt_MAC(div_key_p, cc_nr_p, dest);
	memcpy(mac, dest, 4);
	return;
}

void opt_doTagMAC(uint8_t *cc_p, const uint8_t *div_key_p, uint8_t mac[4]) {
	State _init = {
		((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l
		((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r
		0x4c, // b
		0xE012 // t
	};
	opt_suc(div_key_p, &_init, cc_p, 12, true);
	opt_output(div_key_p, &_init, mac);
	return;
}

/**
 * The tag MAC can be divided (both can, but no point in dividing the reader mac) into
 * two functions, since the first 8 bytes are known, we can pre-calculate the state
 * reached after feeding CC to the cipher.
 * @param cc_p
 * @param div_key_p
 * @return the cipher state
 */
State opt_doTagMAC_1(uint8_t *cc_p, const uint8_t *div_key_p) {
	State _init = {
		((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l
		((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r
		0x4c, // b
		0xE012 // t
	};
	opt_suc(div_key_p, &_init, cc_p, 8, false);
	return _init;
}

/**
 * The second part of the tag MAC calculation, since the CC is already calculated into the state,
 * this function is fed only the NR, and internally feeds the remaining 32 0-bits to generate the tag
 * MAC response.
 * @param _init - precalculated cipher state
 * @param nr - the reader challenge
 * @param mac - where to store the MAC
 * @param div_key_p - the key to use
 */
void opt_doTagMAC_2(State _init, uint8_t *nr, uint8_t mac[4], const uint8_t *div_key_p) {
	opt_suc(div_key_p, &_init, nr, 4, true);
	opt_output(div_key_p, &_init, mac);
	return;
}
Commit	Line	Data
c99dc845 MHS	1	/*****************************************************************************
	2	* WARNING
	3	*
17505ce2	4	* THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
	5	*
	6	* USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
	7	* PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
	8	* AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
	9	*
	10	* THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
c99dc845 MHS	11	*
	12	*****************************************************************************
	13	*
	14	* This file is part of loclass. It is a reconstructon of the cipher engine
	15	* used in iClass, and RFID techology.
	16	*
	17	* The implementation is based on the work performed by
	18	* Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
	19	* Milosch Meriac in the paper "Dismantling IClass".
	20	*
	21	* Copyright (C) 2014 Martin Holst Swende
	22	*
	23	* This is free software: you can redistribute it and/or modify
	24	* it under the terms of the GNU General Public License version 2 as published
298e1a2d	25	* by the Free Software Foundation, or, at your option, any later version.
c99dc845 MHS	26	*
	27	* This file is distributed in the hope that it will be useful,
	28	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	29	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	30	* GNU General Public License for more details.
	31	*
	32	* You should have received a copy of the GNU General Public License
	33	* along with loclass. If not, see <http://www.gnu.org/licenses/>.
17505ce2	34	*
	35	*
	36	*
c99dc845 MHS	37	****************************************************************************/
	38
	39	/**
	40
	41	This file contains an optimized version of the MAC-calculation algorithm. Some measurements on
	42	a std laptop showed it runs in about 1/3 of the time:
	43
	44	Std: 0.428962
	45	Opt: 0.151609
	46
	47	Additionally, it is self-reliant, not requiring e.g. bitstreams from the cipherutils, thus can
	48	be easily dropped into a code base.
	49
	50	The optimizations have been performed in the following steps:
	51	* Parameters passed by reference instead of by value.
	52	* Iteration instead of recursion, un-nesting recursive loops into for-loops.
	53	* Handling of bytes instead of individual bits, for less shuffling and masking
	54	* Less creation of "objects", structs, and instead reuse of alloc:ed memory
	55	* Inlining some functions via #define:s
	56
	57	As a consequence, this implementation is less generic. Also, I haven't bothered documenting this.
	58	For a thorough documentation, check out the MAC-calculation within cipher.c instead.
	59
	60	-- MHS 2015
	61	**/
	62
1477ba8a	63	/**
	64
	65	The runtime of opt_doTagMAC_2() with the MHS optimized version was 403 microseconds on Proxmark3.
	66	This was still to slow for some newer readers which didn't want to wait that long.
	67
	68	Further optimizations to speedup the MAC calculations:
	69	* Optimized opt_Tt logic
	70	* Look up table for opt_select
	71	* Removing many unnecessary bit maskings (& 0x1)
	72	* updating state in place instead of alternating use of a second state structure
	73	* remove the necessity to reverse bits of input and output bytes
	74
	75	opt_doTagMAC_2() now completes in 270 microseconds.
	76
	77	-- piwi 2019
	78	**/
	79
c99dc845	80	#include "optimized_cipher.h"
b8e461ff	81	#include <stddef.h>
c99dc845 MHS	82	#include <stdbool.h>
c99dc845 MHS	83	#include <stdint.h>
1477ba8a	84	#include "string.h"
	85
	86	static const uint8_t opt_select_LUT[256] = {
	87	00, 03, 02, 01, 02, 03, 00, 01, 04, 07, 07, 04, 06, 07, 05, 04,
	88	01, 02, 03, 00, 02, 03, 00, 01, 05, 06, 06, 05, 06, 07, 05, 04,
	89	06, 05, 04, 07, 04, 05, 06, 07, 06, 05, 05, 06, 04, 05, 07, 06,
	90	07, 04, 05, 06, 04, 05, 06, 07, 07, 04, 04, 07, 04, 05, 07, 06,
	91	06, 05, 04, 07, 04, 05, 06, 07, 02, 01, 01, 02, 00, 01, 03, 02,
	92	03, 00, 01, 02, 00, 01, 02, 03, 07, 04, 04, 07, 04, 05, 07, 06,
	93	00, 03, 02, 01, 02, 03, 00, 01, 00, 03, 03, 00, 02, 03, 01, 00,
	94	05, 06, 07, 04, 06, 07, 04, 05, 05, 06, 06, 05, 06, 07, 05, 04,
	95	02, 01, 00, 03, 00, 01, 02, 03, 06, 05, 05, 06, 04, 05, 07, 06,
	96	03, 00, 01, 02, 00, 01, 02, 03, 07, 04, 04, 07, 04, 05, 07, 06,
	97	02, 01, 00, 03, 00, 01, 02, 03, 02, 01, 01, 02, 00, 01, 03, 02,
	98	03, 00, 01, 02, 00, 01, 02, 03, 03, 00, 00, 03, 00, 01, 03, 02,
	99	04, 07, 06, 05, 06, 07, 04, 05, 00, 03, 03, 00, 02, 03, 01, 00,
	100	01, 02, 03, 00, 02, 03, 00, 01, 05, 06, 06, 05, 06, 07, 05, 04,
	101	04, 07, 06, 05, 06, 07, 04, 05, 04, 07, 07, 04, 06, 07, 05, 04,
	102	01, 02, 03, 00, 02, 03, 00, 01, 01, 02, 02, 01, 02, 03, 01, 00
	103	};
	104
	105	/******************** the table above has been generated with this code: ******
	106	#include "util.h"
	107	static void init_opt_select_LUT(void) {
	108	for (int r = 0; r < 256; r++) {
	109	uint8_t r_ls2 = r << 2;
	110	uint8_t r_and_ls2 = r & r_ls2;
	111	uint8_t r_or_ls2 = r \| r_ls2;
	112	uint8_t z0 = (r_and_ls2 >> 5) ^ ((r & ~r_ls2) >> 4) ^ ( r_or_ls2 >> 3);
	113	uint8_t z1 = (r_or_ls2 >> 6) ^ ( r_or_ls2 >> 1) ^ (r >> 5) ^ r;
	114	uint8_t z2 = ((r & ~r_ls2) >> 4) ^ (r_and_ls2 >> 3) ^ r;
	115	opt_select_LUT[r] = (z0 & 4) \| (z1 & 2) \| (z2 & 1);
	116	}
	117	print_result("", opt_select_LUT, 256);
	118	}
	119	***********************************************************************************/
c99dc845 MHS	120
	121	#define opt__select(x,y,r) (4 & (((r & (r << 2)) >> 5) ^ ((r & ~(r << 2)) >> 4) ^ ( (r \| r << 2) >> 3)))\
	122	\|(2 & (((r \| r << 2) >> 6) ^ ( (r \| r << 2) >> 1) ^ (r >> 5) ^ r ^ ((x^y) << 1)))\
	123	\|(1 & (((r & ~(r << 2)) >> 4) ^ ((r & (r << 2)) >> 3) ^ r ^ x))
	124
	125	/*
	126	* Some background on the expression above can be found here...
	127	uint8_t xopt__select(bool x, bool y, uint8_t r)
	128	{
c99dc845 MHS	129
	130	//r: r0 r1 r2 r3 r4 r5 r6 r7
	131	//r_ls2: r2 r3 r4 r5 r6 r7 0 0
	132	// z0
	133	// z1
	134
17505ce2	135	// uint8_t z0 = (r0 & r2) ^ (r1 & ~r3) ^ (r2 \| r4); // <-- original
c99dc845 MHS	136	uint8_t z0 = (r_and_ls2 >> 5) ^ ((r & ~r_ls2) >> 4) ^ ( r_or_ls2 >> 3);
c99dc845 MHS	137
17505ce2	138	// uint8_t z1 = (r0 \| r2) ^ ( r5 \| r7) ^ r1 ^ r6 ^ x ^ y; // <-- original
c99dc845 MHS	139	uint8_t z1 = (r_or_ls2 >> 6) ^ ( r_or_ls2 >> 1) ^ (r >> 5) ^ r ^ ((x^y) << 1);
c99dc845 MHS	140
17505ce2	141	// uint8_t z2 = (r3 & ~r5) ^ (r4 & r6 ) ^ r7 ^ x; // <-- original
c99dc845 MHS	142	uint8_t z2 = ((r & ~r_ls2) >> 4) ^ (r_and_ls2 >> 3) ^ r ^ x;
	143
	144	return (z0 & 4) \| (z1 & 2) \| (z2 & 1);
	145	}
	146	*/
	147
1477ba8a	148	static void opt_successor(const uint8_t k, State s, uint8_t y) {
	149	// #define opt_T(s) (0x1 & ((s->t >> 15) ^ (s->t >> 14) ^ (s->t >> 10) ^ (s->t >> 8) ^ (s->t >> 5) ^ (s->t >> 4)^ (s->t >> 1) ^ s->t))
	150	// uint8_t Tt = opt_T(s);
	151	uint16_t Tt = s->t & 0xc533;
	152	Tt = Tt ^ (Tt >> 1);
	153	Tt = Tt ^ (Tt >> 4);
	154	Tt = Tt ^ (Tt >> 10);
	155	Tt = Tt ^ (Tt >> 8);
	156
	157	s->t = (s->t >> 1);
	158	s->t \|= (Tt ^ (s->r >> 7) ^ (s->r >> 3)) << 15;
	159
	160	uint8_t opt_B = s->b;
	161	opt_B ^= s->b >> 6;
	162	opt_B ^= s->b >> 5;
	163	opt_B ^= s->b >> 4;
	164
	165	s->b = s->b >> 1;
	166	s->b \|= (opt_B ^ s->r) << 7;
	167
	168	uint8_t opt_select = opt_select_LUT[s->r] & 0x04;
	169	opt_select \|= (opt_select_LUT[s->r] ^ ((Tt ^ y) << 1)) & 0x02;
	170	opt_select \|= (opt_select_LUT[s->r] ^ Tt) & 0x01;
	171
	172	uint8_t r = s->r;
	173	s->r = (k[opt_select] ^ s->b) + s->l ;
	174	s->l = s->r + r;
c99dc845 MHS	175	}
c99dc845 MHS	176
1477ba8a	177	static void opt_suc(const uint8_t k, State s, uint8_t *in, uint8_t length, bool add32Zeroes) {
17505ce2	178	for (int i = 0; i < length; i++) {
17505ce2	179	uint8_t head;
1477ba8a	180	head = in[i];
1477ba8a	181	opt_successor(k, s, head);
c99dc845	182
1477ba8a	183	head >>= 1;
1477ba8a	184	opt_successor(k, s, head);
c99dc845	185
1477ba8a	186	head >>= 1;
1477ba8a	187	opt_successor(k, s, head);
c99dc845	188
1477ba8a	189	head >>= 1;
1477ba8a	190	opt_successor(k, s, head);
c99dc845	191
1477ba8a	192	head >>= 1;
1477ba8a	193	opt_successor(k, s, head);
c99dc845	194
1477ba8a	195	head >>= 1;
1477ba8a	196	opt_successor(k, s, head);
c99dc845	197
1477ba8a	198	head >>= 1;
1477ba8a	199	opt_successor(k, s, head);
c99dc845	200
1477ba8a	201	head >>= 1;
1477ba8a	202	opt_successor(k, s, head);
c99dc845	203	}
61fe9073	204	//For tag MAC, an additional 32 zeroes
17505ce2	205	if (add32Zeroes) {
17505ce2	206	for(int i = 0; i < 16; i++) {
1477ba8a	207	opt_successor(k, s, 0);
1477ba8a	208	opt_successor(k, s, 0);
61fe9073	209	}
17505ce2	210	}
c99dc845 MHS	211	}
c99dc845 MHS	212
1477ba8a	213	static void opt_output(const uint8_t k, State s, uint8_t *buffer) {
17505ce2	214	for (uint8_t times = 0; times < 4; times++) {
17505ce2	215	uint8_t bout = 0;
1477ba8a	216	bout \|= (s->r & 0x4) >> 2;
1477ba8a	217	opt_successor(k, s, 0);
c99dc845	218	bout \|= (s->r & 0x4) >> 1;
1477ba8a	219	opt_successor(k, s, 0);
	220	bout \|= (s->r & 0x4);
	221	opt_successor(k, s, 0);
	222	bout \|= (s->r & 0x4) << 1;
	223	opt_successor(k, s, 0);
	224	bout \|= (s->r & 0x4) << 2;
	225	opt_successor(k, s, 0);
	226	bout \|= (s->r & 0x4) << 3;
	227	opt_successor(k, s, 0);
	228	bout \|= (s->r & 0x4) << 4;
	229	opt_successor(k, s, 0);
	230	bout \|= (s->r & 0x4) << 5;
	231	opt_successor(k, s, 0);
c99dc845 MHS	232	buffer[times] = bout;
c99dc845 MHS	233	}
c99dc845 MHS	234	}
c99dc845 MHS	235
1477ba8a	236	static void opt_MAC(uint8_t k, uint8_t input, uint8_t *out) {
c99dc845	237	State _init = {
17505ce2	238	((k[0] ^ 0x4c) + 0xEC) & 0xFF,// l
	239	((k[0] ^ 0x4c) + 0x21) & 0xFF,// r
	240	0x4c, // b
	241	0xE012 // t
	242	};
c99dc845	243
17505ce2	244	opt_suc(k, &_init, input, 12, false);
c99dc845	245	//printf("\noutp ");
17505ce2	246	opt_output(k, &_init, out);
c99dc845	247	}
17505ce2	248
17505ce2	249	void opt_doReaderMAC(uint8_t cc_nr_p, uint8_t div_key_p, uint8_t mac[4]) {
17505ce2	250	uint8_t dest[] = {0, 0, 0, 0, 0, 0, 0, 0};
1477ba8a	251	opt_MAC(div_key_p, cc_nr_p, dest);
1477ba8a	252	memcpy(mac, dest, 4);
61fe9073 MHS	253	return;
61fe9073 MHS	254	}
17505ce2	255
17505ce2	256	void opt_doTagMAC(uint8_t cc_p, const uint8_t div_key_p, uint8_t mac[4]) {
17505ce2	257	State _init = {
	258	((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l
	259	((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r
	260	0x4c, // b
	261	0xE012 // t
	262	};
1477ba8a	263	opt_suc(div_key_p, &_init, cc_p, 12, true);
1477ba8a	264	opt_output(div_key_p, &_init, mac);
61fe9073	265	return;
61fe9073	266	}
17505ce2	267
61fe9073 MHS	268	/**
	269	* The tag MAC can be divided (both can, but no point in dividing the reader mac) into
	270	* two functions, since the first 8 bytes are known, we can pre-calculate the state
	271	* reached after feeding CC to the cipher.
	272	* @param cc_p
	273	* @param div_key_p
	274	* @return the cipher state
	275	*/
17505ce2	276	State opt_doTagMAC_1(uint8_t cc_p, const uint8_t div_key_p) {
17505ce2	277	State _init = {
	278	((div_key_p[0] ^ 0x4c) + 0xEC) & 0xFF,// l
	279	((div_key_p[0] ^ 0x4c) + 0x21) & 0xFF,// r
	280	0x4c, // b
	281	0xE012 // t
	282	};
1477ba8a	283	opt_suc(div_key_p, &_init, cc_p, 8, false);
61fe9073 MHS	284	return _init;
61fe9073 MHS	285	}
17505ce2	286
61fe9073 MHS	287	/**
	288	* The second part of the tag MAC calculation, since the CC is already calculated into the state,
	289	* this function is fed only the NR, and internally feeds the remaining 32 0-bits to generate the tag
	290	* MAC response.
	291	* @param _init - precalculated cipher state
	292	* @param nr - the reader challenge
	293	* @param mac - where to store the MAC
	294	* @param div_key_p - the key to use
	295	*/
17505ce2	296	void opt_doTagMAC_2(State _init, uint8_t nr, uint8_t mac[4], const uint8_t div_key_p) {
1477ba8a	297	opt_suc(div_key_p, &_init, nr, 4, true);
1477ba8a	298	opt_output(div_key_p, &_init, mac);
c99dc845 MHS	299	return;
c99dc845 MHS	300	}