
#define _SUPPRESS_PLIB_WARNING
#define _DISABLE_OPENADC10_CONFIGPORT_WARNING
#include <plib.h>
#include "sleep.h"

void LowPowerSleep() {
    unsigned int status1, status2;

    ClearWDT();
    mSYSTEMUnlock(status1, status2);
    if( OSCCONbits.COSC != 5 ) {
      OSCCONbits.NOSC = 5; // switch to LPRC
      OSCCONbits.OSWEN = 1;
    }
    // prepare to sleep
    OSCCONbits.SLPEN = 1;
    // set power down bits for most peripherals
    CFGCONbits.PMDLOCK = 0;
    PMD1 = 0xFFFFFFFF;
    PMD2 = 0xFFFFFFFF;
    PMD3 = 0xFFFFFFFF;
    PMD4 = 0xFFFFFFFF;
    PMD5 = 0xFFFFFFFF;
    PMD6 = 0xFFFFFFFE; // leave RTCC on
    mSYSTEMLock(status1, status2);
    // wait for clock switch to complete (if in progress)
    while( OSCCONbits.OSWEN )
        ;
    // sleep
    EnableWDT();
    asm volatile("wait");
    DisableWDT();
    // turn power-off bits off
    mSYSTEMUnlock(status1, status2);
    PMD1 = 0;
    PMD2 = 0;
    PMD3 = 0;
    PMD4 = 0;
    PMD5 = 0;
    PMD6 = 0;
    CFGCONbits.PMDLOCK = 1;
    OSCCONbits.SLPEN = 0;
    mSYSTEMLock(status1, status2);
}

__attribute__((mips16)) void ResumeNormalOscillator() {
  if( OSCCONbits.COSC != 7 ) {
    unsigned int status1, status2;
    mSYSTEMUnlock(status1, status2);
    OSCCONbits.NOSC = 7; // switch back to FRC
    OSCCONbits.OSWEN = 1;
    mSYSTEMLock(status1, status2);
    while( OSCCONbits.OSWEN )
      ;
  }
}
