/****************************************************************************/
/* Copyright 2009 MBARI.                                                    */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* memory.c derived from Microchip AN1157                                   */
/****************************************************************************/
#include <p24fxxxx.h>
#include "memory.h"


/********************************************************************
; Function:         void WriteMem(WORD cmd)
;
; PreCondition: Appropriate data written to latches with WriteLatch
;
; Input:            cmd - type of memory operation to perform
;                               
; Output:           None.
;
; Side Effects: 
;
; Overview:         Write stored registers to flash memory
;*********************************************************************/
void WriteMem(WORD cmd)
{
    NVMCON = cmd;

    __builtin_write_NVM();

    while(NVMCONbits.WR == 1)
        ; /* wait for write to complete */

}    
/********************************************************************
; Function:     void WriteLatch(WORD page, WORD addrLo, 
;                                WORD dataHi, WORD dataLo)
;
; PreCondition: None.
;
; Input:        page     - upper byte of address
;                addrLo     - lower word of address
;                dataHi     - upper byte of data
;                addrLo    - lower word of data
;                               
; Output:       None.
;
; Side Effects: TBLPAG changed
;
; Overview:     Stores data to write in hardware latches
;*********************************************************************/    
void WriteLatch(WORD page, WORD addrLo, WORD dataHi, WORD dataLo)
{
    TBLPAG = page;

    __builtin_tblwtl(addrLo,dataLo);
    __builtin_tblwth(addrLo,dataHi);
}    


/********************************************************************
; Function:     DWORD ReadLatch(WORD page, WORD addrLo)
;
; PreCondition: None.
;
; Input:        page     - upper byte of address
;                addrLo     - lower word of address
;                               
; Output:       data     - 32-bit data in W1:W0
;
; Side Effects: TBLPAG changed
;
; Overview:     Read from location in flash memory
;*********************************************************************/
DWORD ReadLatch(WORD page, WORD addrLo)
{
    DWORD_VAL temp;

    TBLPAG = page;

    temp.word.LW = __builtin_tblrdl(addrLo);
    temp.word.HW = __builtin_tblrdh(addrLo);

    return temp.Val;
}

/*********************************************************************
; Function:     void ResetDevice(WORD addr);
;
; PreCondition: None.
;
; Input:        addr     - 16-bit address to vector to
;                               
; Output:       None.
;
; Side Effects: None.
;
; Overview:     used to vector to user code
;**********************************************************************/
void ResetDevice(WORD addr)
{
    asm("goto %0" : : "r"(addr));
}


/********************************************************************
; Function:     void Erase(WORD page, WORD addrLo, WORD cmd);
;
; PreCondition: None.
;
; Input:        page     - upper byte of address
;                addrLo     - lower word of address
;                cmd        - type of memory operation to perform
;                               
; Output:       None.
;
; Side Effects: TBLPAG changed
;
; Overview:     Erases page of flash memory at input address
*********************************************************************/    
void Erase(WORD page, WORD addrLo, WORD cmd)
{
    WORD temp;    

    temp = TBLPAG;
    TBLPAG = page;

    NVMCON = cmd;

    __builtin_tblwtl(addrLo,addrLo);
    __builtin_write_NVM();


    while(NVMCONbits.WR == 1)
        ; /* wait for erase to complete */

    TBLPAG = temp;
}