#include "xPeriphSP6_loader.h"

#include "DSP281x_Examples.h"
#include "DSP281x_Device.h"
#include "MemoryFunctions.h"
#include "Spartan2E_Adr.h"
#include "Spartan2E_Functions.h"
#include "TuneUpPlane.h"
#include "x_parallel_bus.h"
#include "xp_project.h"



Byte byte; //used 8 most significant bits
Word word;
ControlReg controlReg;

volatile AddrToSent addrToSent;
WordToReverse wordToReverse;
WordReversed wordReversed;  


volatile int fail = 0;
volatile unsigned long length = 0;
volatile int tryNumb = 0;
int manufactorerAndProductID = 0;
static int countInMemWrite = 0;

void initState(int adr_device){
	controlReg.all = 0x0000;
	controlReg.bit.cs = 1;
	controlReg.bit.rw = 1;
	controlReg.bit.plane_addr = adr_device;
	controlReg.bit.clock = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all);
}



void sendByte(void){
	int bitCnt = 8;

	controlReg.bit.clock = 0;
	controlReg.bit.data = byte.bit.data;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 

	while (bitCnt > 0) {
		if (controlReg.bit.clock == 1) {
			controlReg.bit.clock = 0;
			controlReg.bit.data = byte.bit.data;
		//	bitCnt--;				
		} else {
			controlReg.bit.clock = 1;
			byte.all = byte.all << 1;
			bitCnt--;
		}

		WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	}
}


void sendWord(void){
	int bitCnt = 16;

	controlReg.bit.clock = 0;
	controlReg.bit.data = word.bit.data;
//	controlReg.bit.data = word.bit.dataReceived;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 

	while (bitCnt > 0) {
		if (controlReg.bit.clock == 1) {
			controlReg.bit.clock = 0;
//			controlReg.bit.data = word.bit.dataReceived;
			controlReg.bit.data = word.bit.data;				
		} else {
			controlReg.bit.clock = 1;
//			word.all = word.all >> 1;
			word.all = word.all << 1;
			bitCnt--;
		}

		WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	}
}



void readByte(void){
	int bitCnt = 8;
	controlReg.bit.clock = 0;
	controlReg.bit.rw = 0;
	controlReg.bit.data = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all);
//	byte.all = 0x0000;
	while (bitCnt > 0) {
		if (controlReg.bit.clock == 1) {
			byte.all = byte.all << 1;			
			controlReg.all = i_ReadMemory(ADR_CONTR_REG_FOR_READ);
			byte.bit.dataReceived = controlReg.bit.eeprom_read;								
			controlReg.bit.clock = 0;
			bitCnt--;				
		} else {			
			controlReg.all = i_ReadMemory(ADR_CONTR_REG_FOR_READ);
			controlReg.bit.clock = 1;			
		}

		WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	}		
}

void readWord(void){
	int bitCnt = 16;
	word.all = 0x0000;
	while (bitCnt > 0) {
		if (controlReg.bit.clock == 1) {
			word.all = word.all << 1;
		//	word.all = word.all >> 1;			
			controlReg.all = i_ReadMemory(ADR_CONTR_REG_FOR_READ);
			word.bit.dataReceived = controlReg.bit.eeprom_read;	
		//	word.bit.data = controlReg.bit.eeprom_read;							
			controlReg.bit.clock = 0;
			bitCnt--;				
		} else {				
			controlReg.all = i_ReadMemory(ADR_CONTR_REG_FOR_READ);
			controlReg.bit.clock = 1;			
		}

		WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	}		
}




void WREN(void) {
	controlReg.bit.cs = 0;
	byte.all= 0x0600;  
	sendByte();

	controlReg.bit.clock = 0;
	controlReg.bit.data = 0;	
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	controlReg.bit.cs = 1;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all);  
}

void WRDI(void) {
	controlReg.bit.cs = 0;
	byte.all= 0x0400;  
	sendByte();

	controlReg.bit.clock = 0;
	controlReg.bit.data = 0;	
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	controlReg.bit.cs = 1;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
}

void WRSR(void) {
	controlReg.bit.cs = 0;
	byte.all= 0x0100;  
	sendByte();

	byte.all= 0x0200;  
	sendByte();

	controlReg.bit.clock = 0;
	controlReg.bit.data = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all);
	controlReg.bit.cs = 1;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
}

void RDSR(void) {
	controlReg.bit.cs = 0;
	controlReg.bit.rw = 1;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all);
	byte.all= 0x0500;
	sendByte();


	readByte();

	controlReg.bit.cs = 1;
	controlReg.bit.rw = 1;
	controlReg.bit.clock = 0;
	controlReg.bit.data = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all);

} 

void RDID(void) {
    controlReg.bit.cs = 0;
    controlReg.bit.rw = 1;
    WriteMemory(ADR_CONTR_REG_FOR_WRITE,    controlReg.all);
    if (manufactorerAndProductID == 0)
        byte.all = 0x1500;
    else
        byte.all = 0x9F00;
    sendByte();


    readByte();

    controlReg.bit.cs = 1;
    controlReg.bit.rw = 1;
    controlReg.bit.clock = 0;
    controlReg.bit.data = 0;
    WriteMemory(ADR_CONTR_REG_FOR_WRITE,    controlReg.all);

}

void ERASE(void) {
	controlReg.bit.cs = 0;
	if (manufactorerAndProductID == 0)
	    byte.all = 0x6200;
	else
	    byte.all = 0xC700;
	sendByte();

	controlReg.bit.clock = 0;
	controlReg.bit.data = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	controlReg.bit.cs = 1;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
}



void READ(void) {
	controlReg.bit.cs = 0;
	byte.all= 0x0300;  
	sendByte();
} 

void PROGRAM(void) {
	controlReg.bit.cs = 0;
	byte.all= 0x0200;  
	sendByte();
}

void ADDR3bytes(FlashAddr flashAddr) {
	int bitCnt = 24;
	addrToSent.all= flashAddr.all;
	addrToSent.all= addrToSent.all << 8;  
	

	controlReg.bit.clock = 0;
	controlReg.bit.data = addrToSent.bit.data;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 

	while (bitCnt > 0) {
		if (controlReg.bit.clock == 1) {
			controlReg.bit.clock = 0;
			controlReg.bit.data = addrToSent.bit.data;
		//	bitCnt--;				
		} else {
			controlReg.bit.clock = 1;
			addrToSent.all = addrToSent.all << 1;
			bitCnt--;
		}

		WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
	}	

}



void DataW256Bytes(volatile unsigned long addrToRead) {
	unsigned long WordNum = 0;

	while (WordNum < 128) {		
		wordToReverse.all= i_ReadMemory(addrToRead + WordNum);

		wordReversed.bit.bit0 = wordToReverse.bit.bit7;
		wordReversed.bit.bit1 = wordToReverse.bit.bit6;
		wordReversed.bit.bit2 = wordToReverse.bit.bit5;
		wordReversed.bit.bit3 = wordToReverse.bit.bit4;
		wordReversed.bit.bit4 = wordToReverse.bit.bit3;
		wordReversed.bit.bit5 = wordToReverse.bit.bit2;
		wordReversed.bit.bit6 = wordToReverse.bit.bit1;
		wordReversed.bit.bit7 = wordToReverse.bit.bit0;

		wordReversed.bit.bit8 = wordToReverse.bit.bit15;
		wordReversed.bit.bit9 = wordToReverse.bit.bit14;
		wordReversed.bit.bit10 = wordToReverse.bit.bit13;
		wordReversed.bit.bit11 = wordToReverse.bit.bit12;
		wordReversed.bit.bit12 = wordToReverse.bit.bit11;
		wordReversed.bit.bit13 = wordToReverse.bit.bit10;
		wordReversed.bit.bit14 = wordToReverse.bit.bit9;
		wordReversed.bit.bit15 = wordToReverse.bit.bit8;

		word.all= wordReversed.all;
		sendWord();		
		WordNum++;		
	}

	controlReg.bit.clock = 0;
	controlReg.bit.data = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all);
	controlReg.bit.cs = 1;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 

}

void DataR256Bytes(volatile unsigned long addrToRead) {

	unsigned long WordNum = 0;	
	
	controlReg.bit.clock = 0;
	controlReg.bit.rw = 0;
	controlReg.bit.data = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 

	while (WordNum < 128) {
		if ((addrToRead + WordNum) <= length) { 					
			readWord();

			wordToReverse.all= i_ReadMemory(addrToRead + WordNum);

			wordReversed.bit.bit0 = wordToReverse.bit.bit7;
			wordReversed.bit.bit1 = wordToReverse.bit.bit6;
			wordReversed.bit.bit2 = wordToReverse.bit.bit5;
			wordReversed.bit.bit3 = wordToReverse.bit.bit4;
			wordReversed.bit.bit4 = wordToReverse.bit.bit3;
			wordReversed.bit.bit5 = wordToReverse.bit.bit2;
			wordReversed.bit.bit6 = wordToReverse.bit.bit1;
			wordReversed.bit.bit7 = wordToReverse.bit.bit0;

			wordReversed.bit.bit8 = wordToReverse.bit.bit15;
			wordReversed.bit.bit9 = wordToReverse.bit.bit14;
			wordReversed.bit.bit10 = wordToReverse.bit.bit13;
			wordReversed.bit.bit11 = wordToReverse.bit.bit12;
			wordReversed.bit.bit12 = wordToReverse.bit.bit11;
			wordReversed.bit.bit13 = wordToReverse.bit.bit10;
			wordReversed.bit.bit14 = wordToReverse.bit.bit9;
			wordReversed.bit.bit15 = wordToReverse.bit.bit8;

			if (word.all != wordReversed.all) {
				fail++;
				WordNum =128;
			} else {
				fail = 0;
			}
			WordNum++;
		} else {
		 //  flashAddr.bit.addr2 = 0xFF; 		    
		   WordNum =128; //finish flash writing
		}
	}	
	
	controlReg.bit.cs = 1;
	controlReg.bit.rw = 1;
	controlReg.bit.clock = 0;
	controlReg.bit.data = 0;
	WriteMemory(ADR_CONTR_REG_FOR_WRITE,	controlReg.all); 
}





void memWrite (unsigned int adr_device, volatile unsigned long adr, 
               volatile unsigned long adr_eeprom, volatile unsigned long size, unsigned long *ok_write, 
               unsigned long *write_error, unsigned long *repeat_error )
{

/////////**********************
/////////before start procedure 
/////////**********************
	volatile int failNumb = 0;
	volatile int checkNumb = 0;
	volatile unsigned long addrToRead = 0;
	volatile FlashAddr flashAddr;


	Led1_Toggle();
	Led2_Toggle();

	*ok_write= size;

	
	countInMemWrite++;
		
	failNumb = 0;
	checkNumb = 0;
	addrToRead = adr;
	length = size + addrToRead;
	addrToRead = adr +8;

	*write_error = 0;
	*repeat_error = 0;

	project.stop_parallel_bus();

	initState(adr_device);


	WREN();
	manufactorerAndProductID = 0;
	RDID();
	if (byte.all != 0x1F00) {
	    manufactorerAndProductID = 1;
	    RDID();
	    if ((byte.all != 0x2000) && (byte.all != 0xEF00)) *write_error = 5; //TODO: make defines with flash ID NAMES
	}
	WREN();
	WRSR();
	RDSR();

	while (byte.all > 0) {
		if (checkNumb < 3) {
			DELAY_US(150);
			RDSR(); //check that flash is not busy

//			byte.all  = 1; // for test!

			if (failNumb > 1000) {
//			if (failNumb > 30000) {	 //1000   // for test!
				WREN();
				WRSR();
				RDSR();
				failNumb = 0;
				checkNumb++;
			}
			failNumb++;
		} else {
			*write_error = 1;
			*ok_write= 0;
			failNumb = 1000;
			byte.all = 0x0000;
			tryNumb++;
		}
	}


//	failNumb = 1000; // for test!

	if (failNumb < 1000) {

		failNumb = 0;
		checkNumb = 0;


		WREN();
		ERASE();
		RDSR();
		while (byte.all > 0) {
			DELAY_US(70000);
			RDSR(); //check that flash is not busy
			if (failNumb > 1000) {
				*ok_write= 0;
				*write_error = 2;
				byte.all = 0x0000;
				tryNumb++;
			}
			failNumb++;
		}
	}

	if (failNumb < 1000) {
		failNumb = 0;

		flashAddr.all = 0;


	/////////**********************
	/////////before start procedure finished
	/////////**********************

		while (flashAddr.bit.addr2 < 0x08){
		    WREN();
			PROGRAM();
			ADDR3bytes(flashAddr);
			DataW256Bytes(addrToRead);
			RDSR();
			Led1_Toggle();
			failNumb = 0;
			checkNumb = 0;
			while (byte.all != 0x0000){
				if (byte.all != 0x0200) {
					if (checkNumb < 30) {
						DELAY_US(3500);
						RDSR(); //check that flash is not busy
						checkNumb++;
					} else {
						byte.all = 0x0200;
					}

				} else {    //programming the page not completed, it's still "data ready"
					if (failNumb < 20) {
					    WREN();
						PROGRAM(); //complete procedure again
						ADDR3bytes(flashAddr);
						DataW256Bytes(addrToRead);
						RDSR();
						checkNumb = 0;
						failNumb++;
					} else {
						*ok_write= addrToRead - adr-8;
						*write_error = 3;
						byte.all = 0x0000;
						flashAddr.bit.addr2 = 0x08;
						tryNumb++;
					  //	asm ("   ESTOP0");	//for save flash life
					}
				}
			}

			if (failNumb < 20){
				READ();
				ADDR3bytes(flashAddr);
				DataR256Bytes(addrToRead);
				Led2_Toggle();
				if (fail ==0) { //if page written correctly, go to the next
					if  (flashAddr.bit.addr1 < 0xff) {
						flashAddr.bit.addr1++;
					} else {
						flashAddr.bit.addr1 = 0;
						flashAddr.bit.addr2++;
					}
					addrToRead += 0x00000080;
				} else if (fail > 7) {
					*ok_write= addrToRead - adr - 8;
					*write_error = 4;
					*repeat_error = fail;
					flashAddr.bit.addr2 = 0x08;
					tryNumb++;
				//	asm ("   ESTOP0");	//for save flash life
				}
			 }

		}
		if ((*write_error != 0) && (tryNumb < 3) && (countInMemWrite < 3)) {
		   memWrite (adr_device, adr, adr_eeprom,  size, ok_write, write_error, repeat_error );
		}
		countInMemWrite = 0;
	}


	tryNumb =0;
	
	WriteMemory(ADR_CONTR_REG_FOR_WRITE, 0xffff);
	WriteMemory(ADR_CONTR_REG_FOR_READ, 0xffff);

	project.reload_all_plates_without_reset_no_stop_error();// wait_start_cds + load_cfg

	

	WriteMemory(ADR_BUS_ERROR_READ,	0);

	if(i_ReadMemory(ADR_ERRORS_TOTAL_INFO)) //Åñëè íà ëèíèßõ îñòàëèñü îøèáêè.
	{
		xerror(main_er_ID(3),(void *)0);
	}
		

	project.start_parallel_bus();
}