#include "SPI_Flash.h"
#include "SPI.h"
#include "BoardCfg.h"

unsigned char mSPIFlashBaudrate;
unsigned char mSPIFlashHighSpeedBaudrate;
unsigned char mFlashSectorBuffer[SPI_FLASH_SECTOR_SIZE];
unsigned int mSPIFlashOK;

int InitSPIFlash()
{
    FLASH_SS_PIN = 1;
    
    mSPIFlashBaudrate = SPICalculateBRG(PERIPHERAL_FREQ, 25000000);
//    mSPIFlashHighSpeedBaudrate = SPICalculateBRG(PERIPHERAL_FREQ, 50000000);
	mSPIFlashHighSpeedBaudrate = SPICalculateBRG(PERIPHERAL_FREQ, 35000000);
	mSPIFlashOK = 0;
}

int SPIFlashCheckAndConfigure()
{
    
    if(SPIFlashCheckChipID() == RET_OK)
    {
        SPIFlashWriteEnable();
        
        FLASH_SS_PIN = 0;
        SPITransaction(SPI_FLASH_WRITE_STATUS_REG,mSPIFlashHighSpeedBaudrate); 
        SPITransaction(0x00,mSPIFlashHighSpeedBaudrate);        //Configure for write enable the whole memory
        FLASH_SS_PIN = 1;
        
        SPIFlashReadStatusReg(1);
        
		mSPIFlashOK = 1;
        printf("SPI Flash configured\n");
        return RET_OK;
    }
    
	mSPIFlashOK = 0;
    printf("ERROR: SPI Flash not detected\n");
    return RET_ERROR;
    
}

int SPIFlashIsPresent()
{
	return mSPIFlashOK;
}

int SPIFlashWriteEnable()
{
    FLASH_SS_PIN = 0;
    SPITransaction(SPI_FLASH_WRITE_ENABLE,mSPIFlashHighSpeedBaudrate);
    FLASH_SS_PIN = 1;
    
    return RET_OK;
}



unsigned char SPIFlashReadStatusReg(int print)
{
    unsigned char result;
    
    FLASH_SS_PIN = 0;
    result = SPITransaction(SPI_FLASH_READ_STATUS_REG,mSPIFlashBaudrate);
    result = SPITransaction(0x00,mSPIFlashBaudrate);    //get data
    FLASH_SS_PIN = 1;
    
    if(print)
    {
        printf("Flash status register : 0x%x\n",result);
    }
    
    return result;
    
}

int SPIFlashCheckBusy()
{
    unsigned char status = SPIFlashReadStatusReg(0);
    if((status & SPI_FLASH_BUSY_MASK) != 0)
    {
        return 1;
    }
    
    return 0;
}


int SPIFlashCheckChipID()
{
    unsigned char VendorID, ChipID;
    
    FLASH_SS_PIN = 0;
    SPITransaction(SPI_FLASH_READ_ID,mSPIFlashBaudrate);
    SPITransaction(0x00,mSPIFlashBaudrate);
    SPITransaction(0x00,mSPIFlashBaudrate);
    SPITransaction(0x00,mSPIFlashBaudrate);    //Vendor address
    VendorID = SPITransaction(0x00,mSPIFlashBaudrate);    //Vendor ID, should be 0xBF
    ChipID = SPITransaction(0x00,mSPIFlashBaudrate);    //Device ID, should be 0x41
    FLASH_SS_PIN = 1;
    
    if(VendorID != SPI_FLASH_VENDOR_ID || ChipID != SPI_FLASH_CHIP_ID)
    {
        printf("SPI Flash detection FAILED.  Vendor: 0x%x, Chip ID: 0x%x\n",VendorID,ChipID);
        return RET_ERROR;
    }
    printf("SPI Flash detected.  Vendor: 0x%x, Chip ID: 0x%x\n",VendorID,ChipID);
    return RET_OK;
    
}


int SPIFlashReadBuffer(unsigned char *Buf, int Size, int StartAddress)
{
    if(StartAddress + Size - 1  > SPI_FLASH_MAX_ADDRESS)
    {
        return RET_ERROR;
    }
    
    FLASH_SS_PIN = 0;
    SPITransaction(SPI_FLASH_HI_SPEED_READ,mSPIFlashBaudrate);
	SPITransaction(((StartAddress & 0xFF0000) >> 16),mSPIFlashHighSpeedBaudrate);
	SPITransaction(((StartAddress & 0x00FF00) >> 8),mSPIFlashHighSpeedBaudrate);
	SPITransaction((StartAddress & 0x0000FF),mSPIFlashHighSpeedBaudrate);
    SPITransaction((0x00),mSPIFlashHighSpeedBaudrate);      //Chip requires a dummy read in high speed
    
    int i;
    for(i = 0; i < Size; i++)
    {
		unsigned char tmp;
		tmp =SPITransaction(0xDE,mSPIFlashHighSpeedBaudrate);
		*Buf++ = tmp;
 //       *Buf++ = SPITransaction(0xDE,mSPIFlashHighSpeedBaudrate);
    }
    
    FLASH_SS_PIN = 1;
    
    return RET_OK;
}

int SPIFlashEraseSector(int SectorAddress)
{
    if(SectorAddress % SPI_FLASH_SECTOR_SIZE != 0)  //Sectors are aligned on 0x1000
    {
        return RET_ERROR;      
    }
    
    SPIFlashWriteEnable();
    
    FLASH_SS_PIN = 0;
    SPITransaction(SPI_FLASH_4KB_SECOTR_ERASE,mSPIFlashHighSpeedBaudrate); 
    SPITransaction(((SectorAddress & 0xFF0000) >> 16),mSPIFlashHighSpeedBaudrate);
    SPITransaction(((SectorAddress & 0x00FF00) >> 8),mSPIFlashHighSpeedBaudrate);
    SPITransaction((SectorAddress & 0x0000FF),mSPIFlashHighSpeedBaudrate);
    FLASH_SS_PIN = 1;
    
    SectorAddress++;
    
    while( SPIFlashCheckBusy() == true);
	//SPIFlashWriteEnable();
    
    return RET_OK;
}

int SPIFlashErase64KSector(int SectorAddress, int Blocking)
{
	if(SectorAddress % SPI_FLASH_64K_SECTOR_SIZE != 0)  //Sectors are aligned on 0x1000
    {
        return RET_ERROR;      
    }
	if((SectorAddress + SPI_FLASH_64K_SECTOR_SIZE - 1) > SPI_FLASH_MAX_ADDRESS)
    {
        return RET_ERROR;      
    }
	
    SPIFlashWriteEnable();
    
    FLASH_SS_PIN = 0;
    SPITransaction(SPI_FLASH_64KB_BLOCK_ERASE,mSPIFlashHighSpeedBaudrate); 
    SPITransaction(((SectorAddress & 0xFF0000) >> 16),mSPIFlashHighSpeedBaudrate);
    SPITransaction(((SectorAddress & 0x00FF00) >> 8),mSPIFlashHighSpeedBaudrate);
    SPITransaction((SectorAddress & 0x0000FF),mSPIFlashHighSpeedBaudrate);
    FLASH_SS_PIN = 1;
    
    if(Blocking != 0)
	{
		while( SPIFlashCheckBusy() == true); 
	//	SPIFlashWriteEnable();
	}
    
    return RET_OK;
}



int SPIFlashWriteSectorWorkingBuffer(int SectorAddress, int Erase)
{
    if(SectorAddress % SPI_FLASH_SECTOR_SIZE != 0)  //Sectors are aligned on 0x1000
    {
        return RET_ERROR;      
    }

    if(Erase == 1)
    {
        SPIFlashEraseSector(SectorAddress);
    }
    
    unsigned char *DataPtr = &mFlashSectorBuffer[0];
    int j;
    for(j = 0; j < SPI_FLASH_SECTOR_SIZE; j++)
    {
		unsigned char curbyte;
		curbyte = *DataPtr;
        SPIFlashWriteEnable();
		

		
		char Add1, Add2, Add3;
		Add1 = (unsigned char)((SectorAddress & 0xFF0000) >> 16);
		Add2 = ((unsigned char)((SectorAddress & 0x00FF00) >> 8));
		Add3 = ((unsigned char)(SectorAddress & 0x0000FF));
		
		int t;
		t = 0;	
        
        FLASH_SS_PIN = 0;
        SPITransaction(SPI_FLASH_BYTE_PROGRAM,mSPIFlashHighSpeedBaudrate); 
//        SPITransaction((unsigned char)((SectorAddress & 0xFF0000) >> 16),mSPIFlashHighSpeedBaudrate);
//        SPITransaction((unsigned char)((SectorAddress & 0x00FF00) >> 8),mSPIFlashHighSpeedBaudrate);
//        SPITransaction((unsigned char)(SectorAddress & 0x0000FF),mSPIFlashHighSpeedBaudrate);
		SPITransaction(Add1,mSPIFlashHighSpeedBaudrate);
        SPITransaction(Add2,mSPIFlashHighSpeedBaudrate);
        SPITransaction(Add3,mSPIFlashHighSpeedBaudrate);

		SPITransaction(curbyte,mSPIFlashHighSpeedBaudrate);
//        SPITransaction(*DataPtr,mSPIFlashHighSpeedBaudrate);
        FLASH_SS_PIN = 1;
        
		DataPtr++;
        SectorAddress++;
        
        while( SPIFlashCheckBusy() == true);
    }
    
     return RET_OK;
}

int SPIFlashWriteByte(unsigned int ByteAddress, char byte, int blocking)
{
	if(ByteAddress > SPI_FLASH_MAX_ADDRESS)
	{
		return RET_ERROR;
	}
	
	SPIFlashWriteEnable();
        
    FLASH_SS_PIN = 0;
    SPITransaction(SPI_FLASH_BYTE_PROGRAM,mSPIFlashHighSpeedBaudrate); 
    SPITransaction(((ByteAddress & 0xFF0000) >> 16),mSPIFlashHighSpeedBaudrate);
    SPITransaction(((ByteAddress & 0x00FF00) >> 8),mSPIFlashHighSpeedBaudrate);
    SPITransaction((ByteAddress & 0x0000FF),mSPIFlashHighSpeedBaudrate);
    SPITransaction(byte,mSPIFlashHighSpeedBaudrate);
    FLASH_SS_PIN = 1;
        
    if(blocking)
	{        
		while( SPIFlashCheckBusy() == true);
	}
	
	
	return RET_OK;
	
}



//int SPIFlashEraseSectorForWrite(unsigned char StartAddress, int Size)
//{
//    //First, let's determine which sector to erase.
//    
//    int NbSectors = 1;
//    int FirstSector = StartAddress / 0x1000;
//    int LastSector = (StartAddress + Size) / 0x1000;
//    
//    if(LastSector > FirstSector)
//    {
//        NbSectors = LastSector - FirstSector;
//    }
//    
//    int i;
//    for(i = FirstSector; i < LastSector; i++ ) //Erase each sector one by one
//    {
//        
//    }
//    
//}

//int SPIFlashWriteBuffer(unsigned char *Buf, int Size, int StartAddress)
//{
//    //First, we need to determine if the data overlaps or uses more than one sector
//    //First, let's determine which sector to erase.
//    int EndAddress = StartAddress + Size - 1;
//    if(EndAddress > SPI_FLASH_MAX_ADDRESS)
//    {
//        return RET_ERROR;
//    }
//    
//    int NbSectors = 1;
//    int FirstSector = StartAddress / SPI_FLASH_SECTOR_SIZE;
//    int LastSector = EndAddress / SPI_FLASH_SECTOR_SIZE;
//    
//    if(LastSector > FirstSector)
//    {
//        NbSectors = LastSector - FirstSector + 1;
//    }
//    
//    int i;
//    int FlashAddress = StartAddress;
//    int CurSector = FirstSector; 
//    for(i = 0; i < NbSectors; i++ ) //Read, erase and write each sector one by one
//    {
//        //first we need to backup the data outside our buffer.  
//        //TODO: optimize
//        int SectorStartAddress = CurSector++ * SPI_FLASH_SECTOR_SIZE;
//        int SectorEndAddress = SectorStartAddress + SPI_FLASH_SECTOR_SIZE - 1;
//        if(SectorEndAddress > EndAddress)
//        {
//            SectorEndAddress = EndAddress;
//        }
//        
//        SPIFlashReadBuffer(mFlashSectorBuffer,SPI_FLASH_SECTOR_SIZE,SectorStartAddress);    //Get local RAM buffer of the sector
//        
//        //Update the data to write.
//        int RAMAddress = FlashAddress - SectorStartAddress;
//        int RAMSectorSize = SectorEndAddress - FlashAddress + 1;
//        FlashAddress += RAMSectorSize;
//        
//        unsigned char* RAMPtr = &mFlashSectorBuffer[RAMAddress];
//        int j;
//        for(j = 0; j < RAMSectorSize; j++)
//        {
//            *RAMPtr++ = *Buf++;
//        }
//        
//        int SectorAddress = SectorStartAddress;
//        RAMPtr = mFlashSectorBuffer;
//        
//        for(j = 0; j < SPI_FLASH_SECTOR_SIZE; j++)
//        {
//            SPIFlashWriteEnable();
//            
//            FLASH_SS_PIN = 0;
//            SPITransaction(SPI_FLASH_BYTE_PROGRAM,mSPIFlashHighSpeedBaudrate); 
//            SPITransaction(((SectorAddress & 0xFF0000) >> 16),mSPIFlashBaudrate);
//            SPITransaction(((SectorAddress & 0x00FF00) >> 8),mSPIFlashBaudrate);
//            SPITransaction((SectorAddress & 0x0000FF),mSPIFlashBaudrate);
//            SPITransaction(*RAMPtr++,mSPIFlashBaudrate);
//            FLASH_SS_PIN = 1;
//            
//            SectorAddress++;
//            
//            while( SPIFlashCheckBusy() == true);
//        }
//    }
//}

int SPIFlashWriteBuffer(unsigned char *Buf, int Size, int StartAddress)
{
    int CurDataFlashAddress, DataFlashEndAddress;
    char *DataBufPtr, *WorkPagePtr;
    int WriteFinished;
    
    
    //Init stuff
    WriteFinished = 0;
    DataFlashEndAddress = StartAddress + Size;      //Start + size means the data at "DataFlashEndAddress" should not be written.
    CurDataFlashAddress = StartAddress;
    DataBufPtr = Buf;
    
    if(DataFlashEndAddress > SPI_FLASH_MAX_ADDRESS)
    {
        return RET_ERROR;
    }
    
    while(WriteFinished == 0)
    {
        //Determine the current sector start address.
        int SectorStartAddress;
        SectorStartAddress = ((CurDataFlashAddress / SPI_FLASH_SECTOR_SIZE) * SPI_FLASH_SECTOR_SIZE);      //Weird but it works
        
        //Load the sector in RAM working buffer
        if(SPIFlashReadBuffer(mFlashSectorBuffer,SPI_FLASH_SECTOR_SIZE,SectorStartAddress) != RET_OK)
        {
            return RET_ERROR;
        }
        
        //Set the working buffer pointer to the right value.
        WorkPagePtr = &mFlashSectorBuffer[CurDataFlashAddress - SectorStartAddress];
        
        //Modify the working buffer with data to write.
        int SectorFinished = 0;
        while(SectorFinished == 0)
        {
            *WorkPagePtr++ = *DataBufPtr++;
            CurDataFlashAddress++;
            
            //Are we at the end of the buffer to write?
            if(CurDataFlashAddress == DataFlashEndAddress)
            {
                SectorFinished = 1;
                WriteFinished = 1;
//                SPIFlashEraseSector(SectorStartAddress);
                SPIFlashWriteSectorWorkingBuffer(SectorStartAddress,1);
                
                break;
            }
            else if(CurDataFlashAddress % SPI_FLASH_SECTOR_SIZE == 0)   //Are we at the beginning of the next sector?
            {
                SectorFinished = 1;
//                SPIFlashEraseSector(SectorStartAddress);
                SPIFlashWriteSectorWorkingBuffer(SectorStartAddress,1);
                
                break;
            }
        }
        
    }
    
    return 1;
}