#if defined(__PIC24F__)
#include <p24Fxxxx.h>
#endif
#include "PwrMgnt.h"

#ifdef _PWRMGNT

extern int __attribute__((space(prog))) _CONFIG2;

/******************************************************************************
 * Function:        BOOL PwrMgnt_OscSel(UINT16 new_oscsel)
 *
 * PreCondition:    None   
 *
 * Input:           new_oscsel - This parameter is the value of the new 
 *                                      oscillator to be selected.
 *                  
 * Output:          BOOL - TRUE : If successfully changed new  oscillator select 
 *                                bits.
 *                         FALSE: If 'FCKSM1 = 1' or new oscillator value is same 
 *                                as old oscillator value or PLL is not locked.
 *
 * Side Effects:    None
 *
 * Overview:        This function should be used to select the appropriate      
 *                  oscillator.
 *
 * Note:            None
 *****************************************************************************/
BOOL PwrMgnt_OscSel(UINT16 new_oscsel)
{
   UINT16 LocalConfigword = 0;
   UINT16 delay = 0;
   UINT16 addr;
   UINT8  CPU_IPL;
   UINT8  tempOSCCONL;
/* Disable Global Interrupt */
   mSET_AND_SAVE_CPU_IP(CPU_IPL,7);

  TBLPAG =  __builtin_tblpage(&_CONFIG2);
  addr = __builtin_tbloffset(&_CONFIG2);
  asm( "tblrdl.w  [%1] , %0" : "=r"(LocalConfigword) : "r"(addr));

   if((LocalConfigword & 0x0080) || (new_oscsel == OSCCONbits.COSC))
   {
      mRESTORE_CPU_IP(CPU_IPL);
      return(FALSE);
   }
   tempOSCCONL = (OSCCON & 0xFF);
   tempOSCCONL |= 0x1;

   __builtin_write_OSCCONH(new_oscsel);
   __builtin_write_OSCCONL(tempOSCCONL);

   while((0 != OSCCONbits.OSWEN)&&(200 > delay))  /* appropriate delay need to be calculated */
   {
    delay++;
   }
   
   if(0 != OSCCONbits.OSWEN)
   {
      mRESTORE_CPU_IP(CPU_IPL);
      return(FALSE);
   }
/* Enable Global Interrupt */
      mRESTORE_CPU_IP(CPU_IPL);

   return(TRUE);
}




#else
#warning "Does not build on this target"
#endif
