//basic PIC32 Flash EEPROM emulation
#include <xc.h>
#include "flash.h"


void loadToRAM(){
    uint32_t i;
    for(i=0;i<FLASH_PAGE_SIZE*FLASH_PAGES_IN_USE/4;i++){
        RAMImage[i]=flashImage[i];
    }
}

uint32_t saveToFlash(){
    uint32_t r,i;
    //NVMADDR = (uint32_t)&flashImage;        //destination
    NVMADDR = VirtToPhys((void*)flashImage);
    r=flashOperation(NVM_PAGE_ERASE);       //do erase
    if(r){return r;}                        //fail    
    for(i=0;i<(FLASH_PAGE_SIZE*FLASH_PAGES_IN_USE/FLASH_ROW_SIZE);i++){
        NVMADDR = VirtToPhys((void*)(flashImage))+FLASH_ROW_SIZE*i;
        NVMSRCADDR = VirtToPhys((void*)(RAMImage))+FLASH_ROW_SIZE*i;    
        r=flashOperation(NVM_ROW_WRITE);       //do erase
        if(r){return r+i+1;}
    }
    return r;
}

uint32_t flashOperation(uint32_t nvmOp){    //perform unlock sequence and operation as required
    __builtin_disable_interrupts();
    NVMCON = nvmOp;                         //operation
    while(NVMCONbits.LVDERR==1){}           //wait for LVD to rise
    NVMKEY = 0xAA996655;                    //unlock
    NVMKEY = 0x556699AA;                    //unlock
    NVMCONSET = 0x8000;                     //start
    while (NVMCON & 0x8000){}               //wait til done
    NVMCONCLR = 0x4000;                     //disable writes
    __builtin_enable_interrupts();          //resume
    return (NVMCON & 0x3000);               //result
}

unsigned int VirtToPhys(volatile void* p) {
  return (int)p<0?((int)p&0x1fffffffL):(unsigned int)((unsigned char*)p+0x40000000L);
}
