/** ###################################################################
**     Filename  : Cpu.C
**     Processor : 56F84xx
**     Compiler  : Metrowerks DSP C Compiler
**     Date/Time : 03/05/2009
**     Abstract  :
**
**     Settings  :
**
**     Contents  :
**
**     (c) Freescale Semiconductor
**     2004 All Rights Reserved
**
**     (c) Copyright UNIS, a.s. 1997-2008
** ###################################################################*/

/* MODULE Cpu. */
#include "Types.h"
#include "Events.h"
#include "Cpu.h"
#include "Main.h"


/*
** ===================================================================
**     Method      :  _EntryPoint 
**
**     Description :
**         Initializes the whole system like timing and so on. At the end 
**         of this function, the C startup is invoked to initialize stack,
**         memory areas and so on.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/

void _EntryPoint(void)
{
  #pragma constarray off

  /*** ### 56F84xxx "Cpu" init code ... ***/
  setReg16(INTC_VBA, 0x0000);  /*Set the interrupt vectors base address*/

  __DI();		/*Disable the maskable interrupts*/
  setReg16(COP_CTRL, 0x00);		/*Disable COP running after reset*/
  asm {
    move.l #0, R0
    move.l #0, R1
    move.l #0, R2
    move.l #0, R3
    move.l #0, R4
    move.l #0, R5
    move.l #0, N
    moveu.w #65535, N3
    moveu.w #65535, M01
    nop
    nop
    swap shadows
    /* Reset the M01 register for linear addressing */
    moveu.w #65535,M01
  }
 /*** PE initialization code after reset ***/
  /*System clock initialization*/
   PE_low_level_init();
   OCCS_Init();
    
  /*** End of PE initialization code after reset ***/
}

/*
** ===================================================================
**     Method      :  PE_low_level_init 
**
**     Description :
**         Initializes beans and provides common register initialization. 
**         The method is called automatically as a part of the 
**         application initialization code.
**         This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void PE_low_level_init(void)
{
 
  clrReg16Bits(INTC_CTRL, 0x20);		/* Enable all interrupts */
}

/*
** ===================================================================
**     Method      :  OCCS_Init (void)
**
**     Description :
**         This function implement to initialize the on-chip clock 
**         synthesis circuit for proper operation.
** ===================================================================
*/
void OCCS_Init(void)
{

 /*Configure relaxation oscillator and PLL output as master system clock*/
	  OCCS_OSCTL1 &= 0x3FF ; // clear any trim bits in ROSC for Frequency
	  OCCS_OSCTL1 |= ((*(word *)0xE42C) & 0x03FFU) ; // factory trim
  
  setReg16(OCCS_DIVBY, 0x2031);		/*Set the detection time of LOL as 2 reference clock,postscaler as 1*/
  //clrReg16Bit(OCCS_CTRL,DCCPD);			/*Enable DCCPD*/
  clrReg16Bits(OCCS_CTRL, OCCS_CTRL_PLLPD);		/*Enable PLL*/
  setReg16Bits(OCCS_CTRL, OCCS_CTRL_LCKON);		/*Enable lock detector*/
  while((OCCS_STAT & 0x60) != 0x60)		/*Wait for PLL lock*/
  {
  	asm(nop);
  }
  clrSetReg16Bits(OCCS_CTRL, OCCS_CTRL_ZSRC, OCCS_CTRL_ZSRC);		/*Switch clock source to PLL output*/
  while((OCCS_STAT & OCCS_STAT_ZSRCS) != OCCS_STAT_ZSRCS_0)		/*Wait for switch completion*/		
  {
  	asm(nop);
  }
  
  
  // Configure 32khz osc trim values
  clrSetReg16Bits(OCCS_OSCTL2, 0x01FF, 0x0111);
  // clear power down bit
  clrReg16Bits(OCCS_OSCTL2,OCCS_OSCTL2_ROPD32K);

}

/* END Cpu. */

/*
** ###################################################################
**
**     This file is created by UNIS Processor Expert 2.99 [04.17]
**     for the Freescale 56800 series of microcontrollers.
**
** ###################################################################
*/
