#include "map.h"
#include "flash.h"
/**********************************************************************/
#pragma DATA_SEG DEFAULT
//==============================================================================
static unsigned char CCRbackup;
//==============================================================================
#pragma CODE_SEG NON_BANKED
//==============================================================================
// UBYTE Flash_CatchError(UBYTE err, UBYTE _place) 
//==============================================================================
UBYTE Flash_CatchError(UBYTE err, UBYTE _place) //routine used to catch error during debug
{
  volatile UBYTE place;


  if (err != FLASHerrOK) 
    {
      place = _place;
      return(err);
    }
  return FLASHerrOK;
}
//==============================================================================
// UBYTE Flash_Write_Word(ULONG addr,UWORD data)
//==============================================================================
UBYTE Flash_Write_Word(ULONG addr,UWORD data)
{
  UBYTE err;
  UWORD off;
  UBYTE page;
  
  page= (UBYTE)(addr>>16);              // check addresable ranges 
  off = (UWORD)(addr & 0xFFFF);
  if(((off>=0x4000)&&(off<=0x7FFE))&&(page!=0x00)) return(FLASHerrRANGE);
  if(((off>=0xC000)&&(off<=0xFFFE))&&(page!=0x00)) return(FLASHerrRANGE);
  if(((off>=0x8000)&&(off<=0xBFFE))&&((page<Flash_Page_Min)||(page>Flash_Page_Max))) return(FLASHerrRANGE);
  if (addr & 1) return FLASHerrNOTAVAIL;   // check alignment
    
   
  getCCR_disI();                       // disable interrupts save CCR


  if(page==0) page=0x3F;


  #if Flash_Blocks==0
    #error "Flash_Blocks must be >= 1"
  #endif 

  #if Flash_Blocks==1 
     FSTAT=0x30;
  #else
    for(err=0;err<Flash_Blocks;err++)
     { 
       FCNFG_BKSEL=err;
       FSTAT=0x30;
     } // PVIOL=ACERR=0

   FCNFG_BKSEL=(UBYTE) ((0x3F-page)/Flash_Pages_Per_Block);             // Set bank

  #endif




  if (FSTAT_CBEIF == 0) {restoreCCR(); return FLASHerrBUSY;} // Is command buffer full ?
  *(UWORD *far) addr = data;           // prepare data to be written
  FCMD = 0x20;                         // command - program word
  asm JSR WaitForFlash; asm STAB err;
  restoreCCR();                        // restore CCR
  if (err) return FLASHerrNOTAVAIL;    // Return error code of WaitForFlash
  if (*(UWORD *far) addr != data) return FLASHerrVALUE;      // Compare required and writen data
  return FLASHerrOK;                   // 
}
//==============================================================================
// UBYTE Flash_Write_Block(ULONG address_source,ULONG address_destination,UWORD count)
//==============================================================================
UBYTE Flash_Write_Block(ULONG address_source,ULONG address_destination,UWORD count)
{
  unsigned long i; // long supports > 64K words
  UWORD *far p_uwf;
  signed char err;
  
  p_uwf = (unsigned int *far)address_source;
  
  for (i = 0;i<count;i++)
    {
      err = Flash_Write_Word(address_destination,*p_uwf++);
      address_destination+=2;
      if(err) return err;
    }
   return FLASHerrOK;
}
//==============================================================================
// UBYTE Flash_Erase_Sector(ULONG addr)
//==============================================================================
UBYTE Flash_Erase_Sector(ULONG addr)
{
  UBYTE err;

  UWORD off;
  UBYTE page;

  page= (UBYTE)(addr>>16);             // check addresable ranges 
  off = (UWORD)(addr & 0xFFFF);
  if(((off>=0x4000)&&(off<=0x7FFE))&&(page!=0x00)) return(FLASHerrRANGE);
  if(((off>=0xC000)&&(off<=0xFFFE))&&(page!=0x00)) return(FLASHerrRANGE);
  if(((off>=0x8000)&&(off<=0xBFFE))&&((page<Flash_Page_Min)||(page>Flash_Page_Max))) return(FLASHerrRANGE);
  
  getCCR_disI();                       // disable interrupts save CCR


  if(page==0) page=0x3F;

  #if Flash_Blocks==0
    #error "Flash_Blocks must be >= 1"
  #endif 

  #if Flash_Blocks==1 
     FSTAT=0x30;
  #else
    for(err=0;err<Flash_Blocks;err++)
     { 
       FCNFG_BKSEL=err;
       FSTAT=0x30;
     } // PVIOL=ACERR=0

   FCNFG_BKSEL=(UBYTE) ((0x3F-page)/Flash_Pages_Per_Block);             // Set bank

  #endif
     
  

  if (FSTAT_CBEIF == 0) {restoreCCR(); return FLASHerrBUSY;} // Is command buffer full ?
  *(UWORD *far) addr = 0xFFFF;         // Write dummy word
  FCMD = 0x40;                         // command - Sector Erase
  asm JSR WaitForFlash; asm STAB err;
  restoreCCR();                        // restore CCR
  if (err) return err;                 // Return error code of WaitForFlash
  return FLASHerrOK;                   //
}
//==============================================================================
#pragma CODE_SEG DEFAULT
//==============================================================================
// signed char Flash_Init(unsigned long oscclk, unsigned long tbus) // [kHz,ns]
//==============================================================================
UBYTE Flash_Init(unsigned long oscclk, unsigned long tbus) // [kHz,ns]
{
  unsigned long fclk_val;
  unsigned long temp;

  if(tbus > 1000L) return FLASHerrNOTAVAIL; // tbus > 1us (1000ns) (page 35 S12FTS256KV2.pdf )
  temp = (oscclk*1000) % 8;
  fclk_val = (oscclk  * (5000UL+tbus)) / 1000000UL;

  if (oscclk >= 12800UL)
    {
      fclk_val = fclk_val / 8;
      if(temp)
       { // oscclk % 8 is not an integer
         fclk_val = fclk_val - 1;
       }
      FCLKDIV = (unsigned char) (fclk_val | 0x40);
    }
  else
    {
      if(temp)
       { // oscclk % 8 is not an integer
         fclk_val = (fclk_val-1);
       }
      FCLKDIV = (unsigned char) fclk_val;
    }  

  // test computed parameters
  if (oscclk >= 12800L)
    {
      temp = 8UL*1000000UL*(1+FCLKDIV&0x3F) / oscclk + tbus;
    }
  else
    {
      temp = 1000000UL*(1+FCLKDIV&0x3F) / oscclk + tbus;
    }  
  temp = temp / 1000;
  
  if(temp < 5UL) return FLASHerrNOTAVAIL; 


  FSTAT= 0x30;
  return FLASHerrOK;
}
//==============================================================================
