
#include <xc.h> // include processor files - each processor file is guarded.  
#include "Clock.h"

//+++ Modtronics
void SetupClock(void)
{
    unsigned int pllCounter;
    OSCCONBITS OSCCONbitsCopy;

    // Copy the current Clock Setup
    OSCCONbitsCopy = OSCCONbits;
    // Slow output clock down to 4Mhz
    CLKDIVbits.CPDIV = 3;
    // Enable the PLL - Note: Fuse bits don't do this
    CLKDIVbits.PLLEN = 1;
    // Wait for the PLL to stabilise
    for (pllCounter = 0; pllCounter < 600; pllCounter++);

    // Check to see what clock setup is defined - either internal or external
    #ifdef USE_FRC_CLOCK
        // Setup the uC to use the internal FRCPLL mode
        OSCCONbitsCopy.NOSC = 1;
        OSCCONbitsCopy.OSWEN = 1;
    #else
        // Setup the uC to use the external crystal with the PLL
        OSCCONbitsCopy.NOSC = 3;
        OSCCONbitsCopy.OSWEN = 1;
    #endif

    // Switch over to the new clock setup
    __builtin_write_OSCCONH( BITS2BYTEH( OSCCONbitsCopy ) );
    __builtin_write_OSCCONL( BITS2BYTEL( OSCCONbitsCopy ) );
    // Wait for this transfer to take place
    while (OSCCONbits.COSC != OSCCONbits.NOSC);
    // Setup the DIV bits for the FRC, this values means the config word needs to be: PLLDIV_DIV2
    CLKDIVbits.RCDIV0 = 0;

    // Setup the PLL divider for the correct clock frequency
    if (CLOCK_FREQ == 32000000)
        CLKDIVbits.CPDIV = 0;
    else if (CLOCK_FREQ == 16000000)
        CLKDIVbits.CPDIV = 1;
    else if (CLOCK_FREQ == 8000000)
        CLKDIVbits.CPDIV = 2;
    else if (CLOCK_FREQ == 4000000)
        CLKDIVbits.CPDIV = 3;

    
    CLKDIVbits.PLLEN = 1;// Check that the PLL is enabled again and locked properly to the new setup
    // Note - don't want to do this check if we are running in the MPLAB X simulator as it won't work
    #ifndef __MPLAB_SIM
        while(_LOCK != 1);
    #endif

    // At this point the PIC24FJ64GB004 clock setup will be complete with the PLL
    // enabled and ready for operation with USB2.0
}