/***************************************************************
 *
 * OpenPICC - ISO 14443 Layer 3 response pretender, 
 * proof of concept
 *
 * Copyright 2008 Henryk Plötz <henryk@ploetzli.ch>
 *
 ***************************************************************

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; version 2.

    This program 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 this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

*/

#include <FreeRTOS.h>
#include <board.h>
#include <task.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>

#include "openpicc.h"
#include "ssc_buffer.h"
#include "iso14443.h"
#include "iso14443_layer2a.h"
#include "iso14443a_pretender.h"
#include "iso14443a_manchester.h"
#include "usb_print.h"
#include "cmd.h"
extern volatile int fdt_offset;
#include "led.h"

struct challenge_response {
	u_int8_t UID[5];
	u_int8_t nonce[4];
	u_int8_t waiting_for_response; 
	struct {
		u_int8_t data[8];
		u_int8_t parity[2];
		size_t len;
	} response;
};

struct challenge_response challenge_response;

static const iso14443_frame ATQA_FRAME = {
	TYPE_A,
	FRAME_PREFILLED,
	{{STANDARD_FRAME, PARITY, ISO14443A_LAST_BIT_NONE, CRC_UNCALCULATED}},
	2,
	0, 0,
	{4, 0},
	{}
};

static const iso14443_frame ATS_FRAME = {
	TYPE_A,
	FRAME_PREFILLED,
	{{STANDARD_FRAME, PARITY, ISO14443A_LAST_BIT_NONE, CRC_UNCALCULATED}},
	3,
	0, 0,
	{0x08, 0xB6, 0xDD},
	{}
};

static iso14443_frame UID_FRAME, NONCE_FRAME;
//static u_int8_t UID[]   = {0xF4, 0xAC, 0xF9, 0xD7}; // bcc = 0x76
static u_int8_t UID[]   = {0x00, 0x00, 0x00, 0x00}; 
static u_int8_t nonce[] = {0x00, 0x00, 0x00, 0x00};

#define FRAME_SIZE(bytes) (2* (1+(9*bytes)+1) )
#define BYTES_AND_BITS(bytes,bits) (bytes*9+bits)
#define GET_BASE_SIZE(type) ((size_t) &(((type*)0)->data))

static ssc_dma_tx_buffer_t *ATQA_BUFFER, *UID_BUFFER, *ATS_BUFFER, *NONCE_BUFFER;

static ssc_dma_tx_buffer_t *alloc_buffer_for_frame(const iso14443_frame *frame)
{
	int datalen = FRAME_SIZE( BYTES_AND_BITS(frame->numbytes, frame->numbits) );
	ssc_dma_tx_buffer_t *dst = pvPortMalloc( GET_BASE_SIZE(typeof(*dst)) + datalen );
	if(dst == NULL) return NULL;
	dst->len = datalen;
	return dst;
}

static void fast_receive_callback(ssc_dma_rx_buffer_t *buffer, iso14443_frame *frame, u_int8_t in_irq)
{
	(void)buffer; (void)in_irq;
	u_int32_t cv = *AT91C_TC2_CV;
	
	ssc_dma_tx_buffer_t *tx_buffer=NULL;
	int fdt = 0;
	
	switch(frame->parameters.a.last_bit) {
	case ISO14443A_LAST_BIT_0:
		fdt = 20;
		break;
	case ISO14443A_LAST_BIT_1:
		fdt = 84;
		break;
	case ISO14443A_LAST_BIT_NONE:
		fdt = 0;
		break;
	}
	
	
	if(cv < 870) /* Anticollision is time-critical, do not even try to send if we're too late anyway */
		switch(BYTES_AND_BITS(frame->numbytes,frame->numbits)) {
		case BYTES_AND_BITS(0, 7): /* REQA or WUPA (7 bits) */
			if(frame->data[0] == 0x26 || frame->data[0] == 0x52)
				tx_buffer = ATQA_BUFFER;
			fdt += 9*128;
			break;
		case BYTES_AND_BITS(2, 0): /* ANTICOL (2 bytes) */
			if(frame->data[0] == 0x93 && frame->data[1] == 0x20)
				tx_buffer = UID_BUFFER;
			fdt += 9*128;
			break;
		case BYTES_AND_BITS(9, 0): /* SELECT (9 bytes) */
			if(frame->data[0] == 0x93 && frame->data[1] == 0x70 && frame->parameters.a.crc &&
					( *((u_int32_t*)&frame->data[2]) ==  *((u_int32_t*)&UID) ) )
				tx_buffer = ATS_BUFFER;
			fdt += 9*128;
			break;
		}
	
	if(tx_buffer == NULL) 
		switch(BYTES_AND_BITS(frame->numbytes, frame->numbits)) {
		case BYTES_AND_BITS(4, 0):
			if( (frame->data[0] & 0xfe) == 0x60 && frame->parameters.a.crc) {
				/* AUTH1A or AUTH1B */
				if(NONCE_BUFFER == NULL) {
					NONCE_BUFFER = alloc_buffer_for_frame(&NONCE_FRAME);
				}
				if(NONCE_BUFFER != NULL) {
					int ret = manchester_encode(NONCE_BUFFER->data,  NONCE_BUFFER->len,  &NONCE_FRAME);
					if(ret < 0) {
					} else {
						NONCE_BUFFER->len = ret;
						tx_buffer = NONCE_BUFFER;
					}
				}
				
				/*fdt += ((cv / 128) + 10) * 128;*/
				fdt += 50*128;  /* 52 ok, 26 not, 39 not, 45 not, 48 not, 50 ok, 49 not */
			}
			break;
		}
	
	/* Add some extra room to the fdt for testing */
	//fdt += 3*128;
	fdt += fdt_offset;
	
	if(tx_buffer != NULL) {
		tx_buffer->state = SSC_FULL;
		if(	iso14443_transmit(tx_buffer, fdt, 1, 0) < 0) {
			usb_print_string_f("Tx failed ", 0);
		}
	}
	
#if 0
	u_int32_t cv2 = *AT91C_TC2_CV;
	usb_print_string_f("\r\n",0);
	if(tx_buffer == &NONCE_BUFFER) usb_print_string_f("---> ",0);
	int old=usb_print_set_default_flush(0);
	DumpUIntToUSB(cv);
	DumpStringToUSB(":");
	DumpUIntToUSB(cv2);
	usb_print_set_default_flush(old);
#endif
	
	switch(BYTES_AND_BITS(frame->numbytes,frame->numbits)) {
	case BYTES_AND_BITS(4, 0):
		if(frame->parameters.a.crc && frame->data[0] == 0x50 && frame->data[1] == 0x00) {
			/* HLTA */
			UID[3]++;
			if(UID[3]==0) {
				UID[2]++;
				if(UID[2]==0) {
					UID[1]++;
					if(UID[1]==0) {
						UID[0]++;
					}
				}
			}
			set_UID(UID, sizeof(UID));
		}
	break;
	}
	
	if(tx_buffer) usb_print_string_f("\r\n",0);
	if(tx_buffer == UID_BUFFER) {
		memcpy(&challenge_response.UID, UID_FRAME.data, 5);
		usb_print_string_f("uid", 0);
	} else if(tx_buffer == NONCE_BUFFER) {
		memcpy(&challenge_response.nonce, NONCE_FRAME.data, 4);
		challenge_response.waiting_for_response = 1;
		usb_print_string_f("nonce", 0);
	} else if(challenge_response.waiting_for_response) {
		challenge_response.waiting_for_response = 0;
		if(frame->numbytes != 8) {
			usb_print_string_f("tilt ",0);
		}
		challenge_response.response.len = frame->numbytes + (frame->numbits+7)/8;
		memcpy(&challenge_response.response.data, frame->data, challenge_response.response.len);
		memcpy(&challenge_response.response.parity, frame->parity, 1);

		int old=usb_print_set_default_flush(0);
		DumpStringToUSB("[[");
		DumpBufferToUSB((char*)challenge_response.UID, 5);
		DumpStringToUSB(" ");
		DumpBufferToUSB((char*)challenge_response.nonce, 4);
		DumpStringToUSB(" ");
		DumpBufferToUSB((char*)challenge_response.response.data, challenge_response.response.len);
		DumpStringToUSB("]]");
		usb_print_set_default_flush(old);
	}
	//usb_print_string_f("%", 0);
}

static void prepare_frame(iso14443_frame *frame, int len)
{
	memset(frame, 0, sizeof(*frame));
	frame->type = TYPE_A;
	frame->parameters.a.format = STANDARD_FRAME;
	frame->parameters.a.parity = PARITY;
	frame->parameters.a.last_bit = ISO14443A_LAST_BIT_NONE;
	frame->parameters.a.crc = CRC_UNCALCULATED;
	
	frame->numbytes = len;
}

int set_UID(u_int8_t *uid, size_t len)
{
	prepare_frame(&UID_FRAME, len+1);
	
	u_int8_t bcc = 0;
	unsigned int i;
	for(i=0; i<len; i++) {
		UID_FRAME.data[i] = uid[i];
		bcc ^= uid[i];
	}
	UID_FRAME.data[i] = bcc;
	UID_FRAME.state = FRAME_PREFILLED;
	memcpy(UID, uid, len);
	
	if(UID_BUFFER == NULL) {
		UID_BUFFER = alloc_buffer_for_frame(&UID_FRAME);
		if(UID_BUFFER == NULL) return -ENOMEM;
	}
	memset(UID_BUFFER->data, 0, UID_BUFFER->len);
	
	int ret = manchester_encode(UID_BUFFER->data,  UID_BUFFER->len,  &UID_FRAME);
	if(ret < 0) return ret;
	else UID_BUFFER->len = ret;
	return 0;
}

int get_UID(u_int8_t *uid, size_t len)
{
	if(len < 4 || len > 4) return -1;
	memcpy(uid, UID, len);
	return 0;
}

int set_nonce(u_int8_t *_nonce, size_t len)
{
	prepare_frame(&NONCE_FRAME, len);
	
	memcpy(&NONCE_FRAME.data, _nonce, len);
	NONCE_FRAME.state = FRAME_PREFILLED;
	memcpy(nonce, _nonce, len);
	
	if(NONCE_BUFFER == NULL) {
		NONCE_BUFFER = alloc_buffer_for_frame(&NONCE_FRAME);
		if(NONCE_BUFFER == NULL) return -ENOMEM;
	}
	memset(NONCE_BUFFER->data, 0, NONCE_BUFFER->len);
	
	int ret = manchester_encode(NONCE_BUFFER->data,  NONCE_BUFFER->len,  &NONCE_FRAME);
	if(ret < 0) return ret;
	else NONCE_BUFFER->len = ret;
	return 0;
}

int get_nonce(u_int8_t *_nonce, size_t len)
{
	if(len < 4 || len > 4) return -1;
	memcpy(_nonce, nonce, len);
	return 0;
}

void iso14443a_pretender (void *pvParameters)
{
	(void)pvParameters;
	int res;
	
	/* Delay until USB print etc. are ready */
	int i;
	for(i=0; i<=1000; i++) {
		vLedSetBrightness(LED_RED, i);
		vLedSetBrightness(LED_GREEN, i);
		vTaskDelay(1*portTICK_RATE_MS);
	}
	
	for(i=0; i<=1000; i+=4) {
		vLedSetBrightness(LED_GREEN, 1000-i);
		vTaskDelay(1*portTICK_RATE_MS);
	}
	
	int ret;
#define PREFILL_BUFFER(dst, src) dst = alloc_buffer_for_frame(&src); \
	if(dst == 0) goto prefill_failed; \
	ret = manchester_encode(dst->data,  dst->len,  &src); \
	if(ret < 0) goto prefill_failed; else dst->len = ret;
	
	PREFILL_BUFFER(ATQA_BUFFER,  ATQA_FRAME);
	//PREFILL_BUFFER(UID_BUFFER,   UID_FRAME);
	PREFILL_BUFFER(ATS_BUFFER,   ATS_FRAME);
	//PREFILL_BUFFER(NONCE_BUFFER, NONCE_FRAME);
	
	set_UID(UID, sizeof(UID));
	set_nonce(nonce, sizeof(nonce));
	
	if(0) {
prefill_failed:
		usb_print_string("Buffer prefilling failed\n\r");
		while(1) {
			for(i=1000; i<=3000; i++) {
				vLedSetBrightness(LED_GREEN, abs(1000-(i%2000)));
				vTaskDelay(1*portTICK_RATE_MS);
			}
		}
	}
	
	do {
		res = iso14443_layer2a_init(1);
		if(res < 0) {
			usb_print_string("Pretender: Initialization failed: -");
			DumpUIntToUSB(-res);
			usb_print_string("\n\r");
			for(i=0; i<=10000; i++) {
				vLedSetBrightness(LED_RED, abs(1000-(i%2000)));
				vTaskDelay(1*portTICK_RATE_MS);
			}
		}
	} while(res < 0);
	
	
	usb_print_string("Waiting for carrier. ");
	while(iso14443_wait_for_carrier(1000 * portTICK_RATE_MS) != 0) {
	}
	usb_print_string("Carrier detected.\n\r");
	
	for(i=0; i<=1000; i+=4) {
		vLedSetBrightness(LED_RED, 1000-i);
		vTaskDelay(1*portTICK_RATE_MS);
	}
	
	vTaskDelay(250*portTICK_RATE_MS);
	vLedSetGreen(0);
	vLedSetRed(0);
	
	int current_detected = 0, last_detected = 0;
	portTickType last_switched = 0;
#define DETECTED_14443A_3 1
#define DETECTED_14443A_4 2
#define DETECTED_MIFARE 4
	
	while(true) {
		iso14443_frame *frame = 0;
		res = iso14443_receive(fast_receive_callback, &frame, 1000 * portTICK_RATE_MS);
		if(res >= 0) {
			switch(BYTES_AND_BITS(frame->numbytes, frame->numbits) ) {
			case BYTES_AND_BITS(0, 7): /* REQA (7 bits) */
				current_detected |= DETECTED_14443A_3;
				break;
			case BYTES_AND_BITS(2, 0): /* ANTICOL (2 bytes) */
			case BYTES_AND_BITS(9, 0): /* SELECT (9 bytes) */
				current_detected |= DETECTED_14443A_3;
				break;
			case BYTES_AND_BITS(4, 0): /* AUTH1A (or any other four byte frame) */
			case BYTES_AND_BITS(8, 0): /* AUTH2 (8 bytes) */
				current_detected |= DETECTED_MIFARE;
				break;
			}
			
			portENTER_CRITICAL();
			frame->state = FRAME_FREE;
			portEXIT_CRITICAL();
		} else {
			if(res != -ETIMEDOUT) {
				usb_print_string("Receive error: ");
				switch(res) {
					case -ENETDOWN:   usb_print_string("PLL is not locked or PLL lock lost\n\r"); break;
					case -EBUSY:      usb_print_string("A Tx is currently running or pending, can't receive\n\r"); break;
					case -EALREADY:   usb_print_string("There's already an iso14443_receive() invocation running\n\r"); break;
				}
				vTaskDelay(1000 * portTICK_RATE_MS); // FIXME Proper error handling, e.g. wait for Tx end in case of EBUSY
			} else if(0) {
				DumpStringToUSB("\n\r");
				DumpTimeToUSB(xTaskGetTickCount());
				usb_print_string(": -- Mark --");
			}
		}
		
		if( last_switched < xTaskGetTickCount()-(1000*portTICK_RATE_MS) ) {
			last_detected = current_detected;
			current_detected = 0;
			last_switched = xTaskGetTickCount();
		}

#if 0
		if(last_detected & (DETECTED_14443A_3 | DETECTED_14443A_4 | DETECTED_MIFARE)) {
			if(last_detected & DETECTED_MIFARE) {
				vLedSetGreen(0);
				vLedSetRed(1);
			} else if(last_detected & DETECTED_14443A_4) {
				vLedSetGreen(1);
				vLedSetRed(0);
			} else {
				vLedSetGreen(1);
				vLedSetRed(1);
			}
		} else {
			vLedSetGreen(0);
			vLedSetRed(0);
		}
#endif
	}
}