void MC_MODE_INIT(void) {
    // TODO: Check
    MC_ME.ME.R = 0x000005FF;                        /* Enable All Modes */

    /* FlexCAN 0: select peri. cfg. RUN_PC[1] */
    MC_ME.PCTL79.B.RUN_CFG = 0x1;
    MC_ME.PCTL204.B.RUN_CFG = 0x1;

    MC_ME.RUN_PC[0].R = 0x00000000;  /* gate off clock for all RUN modes */
    MC_ME.RUN_PC[1].R = 0x000000FE;  /* config. peri clock for all RUN modes */

    /* PBRIDGEx_CLK Divide */
    MC_CGM.SC_DC0.B.DIV = 3;  /* Freq = sysclk / (0+1) = sysclk */
    MC_CGM.SC_DC0.B.DE  = 1;  /* Enable divided clock */

    /* Connect XOSC to PLL. We ultimately use the output of PLL1. PLL1 must be fed the output of PLL0 */
    MC_CGM.AC3_SC.B.SELCTL = 1; //40 MHz XOSC selected as input of PLL0

    MC_CGM.AC4_SC.B.SELCTL=0b11; //PLL0_PHI1 selected as input of PHI1

    /* Configure PLL0 Dividers - 160MHz from 40Mhx XOSC */
    /* PLL input = FXOSC = 40MHz
       VCO range = 600-1200MHz
       MPC5744P uses PLL1 for fractional divide options.
       Configure PLL1 first, because it depends on PLL0. So configure while
       PLL0 still off
    */

    /* Program PLL1 to same frequency as PLL0.
     * MFD multiplies input by at least 10. So multiply by 10 and divide by 10.
     * 10/10 = 1, so same frequency as PLL0
     */
    PLLDIG.PLL1DV.B.RFDPHI = 10;
    PLLDIG.PLL1DV.B.MFD = 10;

    /* Configure PLL0 to 160 MHz. */
    PLLDIG.PLL0DV.B.RFDPHI1 = 4;
    PLLDIG.PLL0DV.B.RFDPHI = 4;
    PLLDIG.PLL0DV.B.PREDIV  = 1;
    PLLDIG.PLL0DV.B.MFD     = 16;

    /* switch to PLL */
    MC_ME.DRUN_MC.R = 0x00130072;
    MC_ME.MCTL.R = 0x30005AF0;
    MC_ME.MCTL.R = 0x3000A50F;
    while(MC_ME.GS.B.S_MTRANS == 1);      /* Wait for mode transition complete */
}