/*****************************************************************************/
/* FILE NAME: mcu_init.c                       COPYRIGHT (c) Freescale 2013  */
/*                                                      All Rights Reserved  */
/* DESCRIPTION: Provides functions to Initialize MPC5777M MCU                */
/*                                                                           */
/*****************************************************************************/	
/* REV      AUTHOR        DATE        DESCRIPTION OF CHANGE                  */
/* ---   -----------    ----------    ---------------------                  */
/* 1.0	  C.Hillier      May 17, 2013  Initial Public Release                */
/* 1.1    A.Turner		 Dec 12, 2013  Changed Per divider ratios            */
/* 									   and added Cut2 #defines               */
/* 1.2    A.Turner		 06 Feb, 2014  Added Cut 2 detection sequences       */
/*									   that clear FT_DIS, set PRAMC to RR and*/
/*									   disable automatic clock gating		 */
/*****************************************************************************/

#include "../headers/project.h"


/*****************************************************************************/   
/* MPC5777M PLL and Clock Configuration                                      */
/* Configure the Mode and Clock Tree                                         */
/*****************************************************************************/
void MC_MODE_INIT(void){

/* 1 Clear any faults */
/* Clear faults | MC_RGM.DES, MC_RGM.FES, and MC_ME.ME */
  MC_RGM.DES.R = 0xFFFF;
  MC_RGM.FES.R = 0xFFFF;
  MC_ME.ME.R = 0x000005FF;
    
/* 2 Set up peripheral run modes */
/* Enable the modes Required | MC_ME.ME  */
  MC_ME.ME.R = 0x0000800F;

/* Add MC_ME.PCTL[x].R initializations here */

/* Set RUN Configuration Registers | MC_ME.RUN_PC[n] */
  MC_ME.RUN_PC[0].R=0x000000FE;              /* Peripheral ON in every mode  */
  MC_ME.RUN_PC[1].R=0x000000FE;              /* Peripheral ON in every mode  */
  MC_ME.RUN_PC[2].R=0x000000FE;              /* Peripheral ON in every mode  */
  MC_ME.RUN_PC[3].R=0x000000FE;              /* Peripheral ON in every mode  */
  MC_ME.RUN_PC[4].R=0x000000FE;              /* Peripheral ON in every mode  */
  MC_ME.RUN_PC[5].R=0x000000FE;              /* Peripheral ON in every mode  */
  MC_ME.RUN_PC[6].R=0x000000FE;              /* Peripheral ON in every mode  */
  MC_ME.RUN_PC[7].R=0x000000FE;              /* Peripheral ON in every mode  */

  MC_ME.MCTL.R = 0x30005AF0;                       /* Mode & Key */
  MC_ME.MCTL.R = 0x3000A50F;                /* Mode & Key inverted */
                                      
  while(MC_ME.GS.B.S_MTRANS == 1);               /* Wait for mode entry to complete */
  while(MC_ME.GS.B.S_CURRENT_MODE != 0x3);       /* Check DRUN mode has been entered */
   
  
  // only on cut 2
  if(SIUL2.MIDR1.R & 0x10)
  {
    // clear FT_DIS and use round-robin on PRAMC
    *(uint32_t*)0xFC020000 = 0;
        
    // set PCM_5 - disable automatic clock gating for peripherals  
    *(uint32_t*)0xFC028014 &= ~0x01110111;
            
    MC_CGM.SC_DIV_RC.R = 0x00000001;  /*! System clock divider ratios will change with next update.  Not required for Cut 1. | MC_CGM.SC_DIV_RC */
    MC_CGM.DIV_UPD_TYPE.R = 0x80000000;  /*! System clock divider ratios updated on writing MC_CGM.DIV_UPD_TRIG.  Not required for Cut 1. | MC_CGM.SC_UPD_TYPE */
  } 

  /*! 3 Configure System Clock Dividers  */   
  /*! Configure System clock dividers */
  /*! Full speed Core 0 / 1 = 300MHz.  PLL1 = 600MHz.   */
  MC_CGM.SC_DC2.R=0x800B0000;   /*! PBRIDGE Clock Divide by 12  (50MHz) | MC_CGM.SC_DC[2] */ 
  MC_CGM.SC_DC1.R=0x80050000;   /*! SXBAR Clock Divide by 6    (100MHz) | MC_CGM.SC_DC[1] */
  MC_CGM.SC_DC0.R=0x80020000;   /*! FXBAR Clock Divide by 3    (200MHz) | MC_CGM.SC_DC[0] */
  MC_CGM.SC_DC3.R=0x80010000;   /*! Core0/1 Clock Divide by 2  (300MHz) | MC_CGM.SC_DC[3] */   
  MC_CGM.SC_DC4.R=0x80080000;   /*! Clockout by 9  (66MHz) | MC_CGM.SC_DC[3] */  
  
  // only on cut 2
  if(SIUL2.MIDR1.R & 0x10)
  {
    /*! System clock divider ratio updates triggered.  Not required for Cut 1. | MC_CGM.SC_UPD_TRIG */
    MC_CGM.DIV_UPD_TRIG.R = 0xfeedface; 
  
    /*! Wait for System Clock Divider Update Status == 0. Not required for Cut 1. | MC_CGM.SC_UPD_STAT */
    while (MC_CGM.DIV_UPD_STAT.B.SYS_UPD_STAT == 1)
    {
      asm("nop");
    }   
  }

 /*! Enable and configure Aux clocks */
  MC_CGM.AC0_SC.B.SELCTL=2;        /*! set PLL0 PHI for Aux Clock 0 | */
  
  MC_CGM.AC0_DC0.R=0x80040000;     /*! program Aux Clock 0 divider 0 (peripheral clock) -> Divide by = 4 + 1.  400MHz/5 = 80MHz    | MC_CGM.AC0_DC[0]    */                                     
  MC_CGM.AC0_DC1.R=0x80180000;     /*! program Aux Clock 0 divider 1 (SDADC clock) -> Divide by 24 + 1.  400MHz / 25 = 16MHz       | MC_CGM.AC0_DC[1]    */
  MC_CGM.AC0_DC2.R=0x801B0000;     /*! program Aux Clock 0 divider 2 (SARADC clock) -> Divide by 24 + 1.  400MHz / 28 = 14.6MHz    | MC_CGM.AC0_DC[2]    */
  MC_CGM.AC0_DC3.R=0x80030000;     /*! program Aux Clock 0 divider 3 (DSPI_CLK0) -> Divide by 3 + 1.  400MHz / 4 = 100MHz          | MC_CGM.AC0_DC[3]    */
  MC_CGM.AC0_DC4.R=0x80030000;     /*! program Aux Clock 0 divider 4 (DSPI_CLK1/LIN_CLK) -> Divide by 3 + 1.  400MHz / 4 = 100MHz  | MC_CGM.AC0_DC[4]    */
  MC_CGM.AC2_DC0.R=0x80090000;     /*! program Aux Clock 2 divider 0 (FlexRay) -> Divide by 9 + 1.  400MHz / 10 = 40MHz | MC_CGM.AC2_DC[0] */
  MC_CGM.AC2_DC1.R=0x80090000;     /*! program Aux Clock 2 divider 1 (SENT) -> Divide by 9 + 1.  400MHz / 10 = 40MHz    | MC_CGM.AC2_DC[1] */
  MC_CGM.AC5_DC0.R=0x80090000;         /*! program Aux Clock 5 divider 0 (PSI5) -> Divide by 9 + 1.  400MHz / 10 = 40MHz  | MC_CGM.AC5_DC0 */
  MC_CGM.AC5_DC1.R=0x80090000;         /*! program Aux Clock 5 divider 1 (PSI5) -> Divide by 9 + 1.  400MHz / 10 = 40MHz  | MC_CGM.AC5_DC1 */
  MC_CGM.AC5_DC2.R=0x80090000;         /*! program Aux Clock 5 divider 2 (PSI5) -> Divide by 9 + 1.  400MHz / 10 = 40MHz  | MC_CGM.AC5_DC2 */                                 
  
  /* CAN Clock Runs from XOSC by Default */
  MC_CGM.AC8_DC0.R=0x80000000;    /*! program Aux Clock 8 divider 0 (CAN Clock)-> Divide by 1     | MC_CGM.AC8_DC0 */
  
  MC_CGM.AC9_SC.B.SELCTL=1;       /*! Select XOSC for Aux Clock 9                         | MC_CGM.AC9_SC.B.SELCTL */   
  MC_CGM.AC9_DC0.R=0x80030000;    /*! program Aux Clock 9 divider 0 (RTI/PIT)-> Divide by 4   | MC_CGM.AC9_DC0 */
  
  
  MC_CGM.AC10_SC.B.SELCTL=2;       /*! Select PLL0 PHI for Aux Clock 10                     | MC_CGM.AC9_SC.B.SELCTL */   
  MC_CGM.AC10_DC0.R=0x800F0000;   /*! program Aux Clock 10 divider 0 (ENET) -> Divide by 15 + 1.  400MHz / 16 = 25MHz | MC_CGM.AC10_DC0 */
  
  
  /*! Step 5 --- CONFIGURE X0SC PLL0 PLL1 ---   */
  
  /*! Route XOSC to the PLL's - IRC is default | MC_CGM.AC3_SC and MC_CGM.AC4_SC */
  MC_CGM.AC3_SC.B.SELCTL=1;                  /*! Connect XOSC to PLL0 */
  MC_CGM.AC4_SC.B.SELCTL=1;                  /*! Connect XOSC to PLL1 */
  
  
    
  
  /* Configure PLL0 Dividers - 400MHz from 40MHz XOSC */

// fPLL0_VCO = (fpll0_ref * 2 * MFD) / PREDIV
// fPLL0_VCO = (40 MHz * 2 * 40) / 4
// fPLL0_VCO =  800 MHz

// fPLL0_PHI = (fpll0_ref * 2 * MFD) / (PREDIV * RFDPHI * 2)
// fPLL0_PHI = (40 MHz * 2 * 40) / (4 * 1 * 2)
// fPLL0_PHI =  400 MHz

  PLLDIG.PLL0DV.B.RFDPHI = 1;  
  PLLDIG.PLL0DV.B.PREDIV = 4;  
  PLLDIG.PLL0DV.B.MFD = 40;    
   
/* Configure PLL1 Dividers - 600MHz from 40MHz XOSC */

// fPLL1_VCO = (fpll1_ref * MFD) 
// fPLL1_VCO = (40 MHz * 30) 
// fPLL1_VCO =  1200 MHz

// fPLL1_PHI = (fpll1_ref * MFD) / (RFDPHI * 2)
// fPLL1_PHI = (40 MHz * 30) / (1 * 2)
// fPLL1_PHI =  600 MHz
  
  PLLDIG.PLL1DV.B.RFDPHI = 1;  
  PLLDIG.PLL1DV.B.MFD = 30;    
  
  
  /*! 6 CONFIGURE PROGRESSIVE CLOCK SWITCHING (PCS), Configure Progressive Clock Switching (PCS) to prevent glitches - 0.05 rate 70 steps. */
  
  
  MC_CGM.PCS_SDUR.R = 100;   /*! set Switch Duration | MC_CGM.PCS_SDUR RM 30.4.1.2.2. Defines the duration of one system clock switch step. Switch Duration = Number of 16MHz clocks * SDUR * k steps */
  
  MC_ME.DRUN_MC.B.PWRLVL=3;  /*! Configure DRUN power level | MC_ME.DRUN_MC */                 
  
  /*! Configure PLL1 PCS switch | See RM section "Progressive system clock switching"  */
  MC_CGM.PCS_DIVC4.B.INIT = 851;   /*! Set the Divider Change Initial Value | MC_CGM.PCS_DIVC4.B.INIT */
  MC_CGM.PCS_DIVC4.B.RATE = 12;    /*! Set the Divider Change Rate | MC_CGM.PCS_DIVC4.B.RATE  amax = fchg / fsrc, where fchg is max allowed changed of frequency and fsrc is the source frequency (e.g. 300MHz or 200MHz).  Given amax = 0.05, then set CGM_PCS_DIVCn[RATE] to 12 per RM Table. */
  MC_CGM.PCS_DIVS4.R = 31671;      /*! Set the Divider Start Value | MC_CGM.PCS_DIVS4 this is the clock ramp-up start divider value. */
  MC_CGM.PCS_DIVE4.R = 31671;      /*! Set the Divider End Value   | MC_CGM_PCS_DIVE4 this is the clock ramp-down end divider value. */
  
  /*! Configure PLL0 PCS switch | See RM section "Progressive system clock switching"    */
  MC_CGM.PCS_DIVC2.B.INIT = 851;   /*! Set the Divider Change Initial Value | MC_CGM.PCS_DIVC4.B.INIT */
  MC_CGM.PCS_DIVC2.B.RATE = 12;    /*! Set the Divider Change Rate | MC_CGM.PCS_DIVC4.B.RATE  amax = fchg / fsrc, where fchg is max allowed changed of frequency and fsrc is the source frequency (e.g. 300MHz or 200MHz).  Given amax = 0.05, then set CGM_PCS_DIVCn[RATE] to 12 per RM Table. */
  MC_CGM.PCS_DIVS2.R = 31671;      /*! Set the Divider Start Value | MC_CGM.PCS_DIVS4 this is the clock ramp-up start divider value. */
  MC_CGM.PCS_DIVE2.R = 31671;      /*! Set the Divider End Value   | MC_CGM_PCS_DIVE4 this is the clock ramp-down end divider value. */
  

/*! 7 Initialize e200z Cores */   
/* Enable cores if running from RAM and not using the BAF */
/* Enable Cores - Will start on next mode transistion */
/* If core n is enabled, then */
/*  - Set MC_ME.CADDR[n] to the code start address (see linker file) */
/*  - Set MC_ME.CCTL[n] to enable core in all modes   */

/* Enable cores if running from RAM and not using the BAF */
/* Enable Cores - Will start on next mode transistion */
  MC_ME.CCTL0.R = 0x00FE;
  MC_ME.CCTL1.R = 0x00FE;
  MC_ME.CCTL2.R = 0x0000;   //TURN OFF LOCK STEP CORE
  MC_ME.CCTL3.R = 0x00FE;
  MC_ME.CCTL4.R = 0x0000;   //TURN OFF HSM

	
/* RAM addresses */
     MC_ME.CADDR1.R = 0x4001C001;
     MC_ME.CADDR2.R = 0x00000001;              /* Core 0_s RMC bit = 1, no ADDR field in Cut 2 */
     MC_ME.CADDR3.R = 0x40018001;
//   MC_ME.CADDR4.R = 0x40018001;   
   
/* 8 ----- Perform Mode Entry change ----- */
/* Set the System Clock. Enable XOSC and PLLs - PLL1 is sysclk, PWRLVL = 3. | MC_ME.DRUN_MC */
   MC_ME.DRUN_MC.R = 0x301300F4;     
   
/* Execute mode change: Re-enter the DRUN mode to start cores, clock tree dividers, PCS, and PLL1 */
   MC_ME.MCTL.R = 0x30005AF0;                    /* Write Mode and Key */
   MC_ME.MCTL.R = 0x3000A50F;                    /* Write Mode and Key inverted */
   while(MC_ME.GS.B.S_MTRANS == 1);              /* Wait for mode entry complete */
   while(MC_ME.GS.B.S_CURRENT_MODE != 0x3);      /* Check DRUN mode entered |MC_ME.GS */
     
}

