#include "config.h"

/* Configuration words */
_FOSC(FCKSM_CSECMD & POSCMD_XT);
_FOSCSEL(FNOSC_FRC);
_FWDT(FWDTEN_OFF);

void config_init(void)
{
	/* Disable analog inputs */
	AD1PCFGL = 0xffff;
	AD1PCFGH = 0xffff;
	AD2PCFGL = 0xffff;

	/* Configure PLL to multiply from 8 -> 40 MHz */
	PLLFBD = 38;
	CLKDIVbits.PLLPRE = 0;
	CLKDIVbits.PLLPOST = 0;

	/* Switch to XTPLL clock */
	__builtin_write_OSCCONH(0x03);
	__builtin_write_OSCCONL(0x01);

	/* Wait for lock */
	while (!OSCCONbits.LOCK)
		continue;
}