arch/ppc/mpc55xx/drivers/Mcu.c

   1 /* -------------------------------- Arctic Core ------------------------------\r
   2  * Arctic Core - the open source AUTOSAR platform http://arccore.com\r
   3  *\r
   4  * Copyright (C) 2009  ArcCore AB <contact@arccore.com>\r
   5  *\r
   6  * This source code is free software; you can redistribute it and/or modify it\r
   7  * under the terms of the GNU General Public License version 2 as published by the\r
   8  * Free Software Foundation; See <http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt>.\r
   9  *\r
  10  * This program is distributed in the hope that it will be useful, but\r
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY\r
  12  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License\r
  13  * for more details.\r
  14  * -------------------------------- Arctic Core ------------------------------*/\r
  15 \r
  16 \r
  17 \r
  18 \r
  19 \r
  20 \r
  21 \r
  22 \r
  23 #include <assert.h>\r
  24 #include <string.h>\r
  25 #include "Std_Types.h"\r
  26 #include "Mcu.h"\r
  27 #include "Det.h"\r
  28 #if defined(USE_DEM)\r
  29 #include "Dem.h"\r
  30 #endif\r
  31 #include "mpc55xx.h"\r
  32 #include "Cpu.h"\r
  33 #include "Ramlog.h"\r
  34 #include "Os.h"\r
  35 #include "irq.h"\r
  36 \r
  37 //#define USE_LDEBUG_PRINTF 1\r
  38 #include "debug.h"\r
  39 \r
  40 #define SYSCLOCK_SELECT_PLL     0x2\r
  41 \r
  42 #if defined(CFG_MPC5567)\r
  43 #define CALC_SYSTEM_CLOCK(_extal,_emfd,_eprediv,_erfd)  \\r
  44             ( (_extal) * ((_emfd)+4) / (((_eprediv)+1)*(1<<(_erfd))) )\r
  45 #else\r
  46 #define CALC_SYSTEM_CLOCK(_extal,_emfd,_eprediv,_erfd)  \\r
  47             ( (_extal) * ((_emfd)+16) / (((_eprediv)+1)*((_erfd)+1)) )\r
  48 #endif\r
  49 \r
  50 typedef void (*vfunc_t)();\r
  51 \r
  52 /* Function declarations. */\r
  53 static void Mcu_ConfigureFlash(void);\r
  54 \r
  55 \r
  56 typedef struct {\r
  57         uint32 lossOfLockCnt;\r
  58         uint32 lossOfClockCnt;\r
  59 } Mcu_Stats;\r
  60 \r
  61 /**\r
  62  * Type that holds all global data for Mcu\r
  63  */\r
  64 typedef struct\r
  65 {\r
  66   // Set if Mcu_Init() have been called\r
  67   boolean initRun;\r
  68 \r
  69   // Our config\r
  70   const Mcu_ConfigType *config;\r
  71 \r
  72   Mcu_ClockType clockSetting;\r
  73 \r
  74   Mcu_Stats stats;\r
  75 \r
  76 } Mcu_GlobalType;\r
  77 \r
  78 /* Development error macros. */\r
  79 #if ( MCU_DEV_ERROR_DETECT == STD_ON )\r
  80 #define VALIDATE(_exp,_api,_err ) \\r
  81         if( !(_exp) ) { \\r
  82           Det_ReportError(MODULE_ID_MCU,0,_api,_err); \\r
  83           return; \\r
  84         }\r
  85 \r
  86 #define VALIDATE_W_RV(_exp,_api,_err,_rv ) \\r
  87         if( !(_exp) ) { \\r
  88           Det_ReportError(MODULE_ID_MCU,0,_api,_err); \\r
  89           return (_rv); \\r
  90         }\r
  91 #else\r
  92 #define VALIDATE(_exp,_api,_err )\r
  93 #define VALIDATE_W_RV(_exp,_api,_err,_rv )\r
  94 #endif\r
  95 \r
  96 // Global config\r
  97 Mcu_GlobalType Mcu_Global =\r
  98 {\r
  99                 .initRun = 0,\r
 100                 .config = &McuConfigData[0],\r
 101 };\r
 102 \r
 103 //-------------------------------------------------------------------\r
 104 \r
 105 static void Mcu_LossOfLock( void  ) {\r
 106 #if defined(USE_DEM)\r
 107         Dem_ReportErrorStatus(MCU_E_CLOCK_FAILURE, DEM_EVENT_STATUS_FAILED);\r
 108 #endif\r
 109 \r
 110   /*\r
 111    * NOTE!!!\r
 112    * This interrupt may be triggered more than expected.\r
 113    * If you are going to use this interrupt, see [Freescale Device Errata MPC5510ACE, Rev. 10 APR 2009, errata ID: 6764].\r
 114    *\r
 115    */\r
 116 \r
 117         Mcu_Global.stats.lossOfLockCnt++;\r
 118         // Clear interrupt\r
 119         FMPLL.SYNSR.B.LOLF = 1;\r
 120 \r
 121 }\r
 122 \r
 123 //-------------------------------------------------------------------\r
 124 static void Mcu_LossOfCLock( void  ) {\r
 125 \r
 126         /* Should report MCU_E_CLOCK_FAILURE with DEM here */\r
 127 \r
 128         Mcu_Global.stats.lossOfClockCnt++;\r
 129         // Clear interrupt\r
 130         FMPLL.SYNSR.B.LOCF = 1;\r
 131 }\r
 132 \r
 133 \r
 134 #define SPR_PIR 286\r
 135 #define SPR_PVR 287\r
 136 \r
 137 #define CORE_PVR_E200Z1         0x81440000UL\r
 138 #define CORE_PVR_E200Z0         0x81710000UL\r
 139 #define CORE_PVR_E200Z3         0x81120000UL\r
 140 #define CORE_PVR_E200Z6         0x81170000UL\r
 141 \r
 142 \r
 143 typedef struct {\r
 144   char *name;\r
 145   uint32 pvr;\r
 146 } core_info_t;\r
 147 \r
 148 typedef struct {\r
 149   char *name;\r
 150   uint32 pvr;\r
 151 } cpu_info_t;\r
 152 \r
 153 cpu_info_t cpu_info_list[] =\r
 154 {\r
 155 #if defined(CFG_MPC5516)\r
 156     {\r
 157     .name = "MPC5516",\r
 158     .pvr = CORE_PVR_E200Z1,\r
 159     },\r
 160     {\r
 161     .name = "MPC5516",\r
 162     .pvr = CORE_PVR_E200Z0,\r
 163     },\r
 164 #elif defined(CFG_MPC5567)\r
 165     {\r
 166         .name = "MPC5567",\r
 167         .pvr = CORE_PVR_E200Z6,\r
 168     }\r
 169 #elif defined(CFG_MPC5633)\r
 170     {\r
 171     .name = "MPC563X",\r
 172     .pvr = CORE_PVR_E200Z3,\r
 173     },\r
 174 #endif\r
 175 };\r
 176 \r
 177 core_info_t core_info_list[] = {\r
 178 #if defined(CFG_MPC5516)\r
 179         {\r
 180     .name = "CORE_E200Z1",\r
 181     .pvr = CORE_PVR_E200Z1,\r
 182     },\r
 183     {\r
 184     .name = "CORE_E200Z1",\r
 185     .pvr = CORE_PVR_E200Z1,\r
 186     },\r
 187 #elif defined(CFG_MPC5567)\r
 188     {\r
 189         .name = "CORE_E200Z6",\r
 190         .pvr = CORE_PVR_E200Z6,\r
 191     }\r
 192 #elif defined(CFG_MPC5633)\r
 193     {\r
 194     .name = "CORE_E200Z3",\r
 195     .pvr = CORE_PVR_E200Z3,\r
 196     },\r
 197 #endif\r
 198 };\r
 199 \r
 200 // TODO: move\r
 201 #if !defined(ARRAY_SIZE)\r
 202 #define ARRAY_SIZE(_x)  (sizeof(_x)/sizeof((_x)[0]))\r
 203 #endif\r
 204 \r
 205 static cpu_info_t *Mcu_IdentifyCpu(uint32 pvr)\r
 206 {\r
 207   int i;\r
 208   for (i = 0; i < ARRAY_SIZE(cpu_info_list); i++) {\r
 209     if (cpu_info_list[i].pvr == pvr) {\r
 210       return &cpu_info_list[i];\r
 211     }\r
 212   }\r
 213 \r
 214   return NULL;\r
 215 }\r
 216 \r
 217 static core_info_t *Mcu_IdentifyCore(uint32 pvr)\r
 218 {\r
 219   int i;\r
 220   for (i = 0; i < ARRAY_SIZE(core_info_list); i++) {\r
 221     if (core_info_list[i].pvr == pvr) {\r
 222       return &core_info_list[i];\r
 223     }\r
 224   }\r
 225 \r
 226   return NULL;\r
 227 }\r
 228 \r
 229 \r
 230 static uint32 Mcu_CheckCpu( void ) {\r
 231 \r
 232   uint32 pvr;\r
 233   uint32 pir;\r
 234   cpu_info_t *cpuType;\r
 235   core_info_t *coreType;\r
 236 \r
 237   // We have to registers to read here, PIR and PVR\r
 238 \r
 239   pir = get_spr(SPR_PIR);\r
 240   pvr = get_spr(SPR_PVR);\r
 241 \r
 242   cpuType = Mcu_IdentifyCpu(pvr);\r
 243   coreType = Mcu_IdentifyCore(pvr);\r
 244 \r
 245   if( (cpuType == NULL) || (coreType == NULL) ) {\r
 246     // Just hang\r
 247     while(1);\r
 248   }\r
 249 \r
 250   //DEBUG(DEBUG_HIGH,"/drivers/mcu: Cpu:  %s( 0x%08x )\n",cpuType->name,pvr);\r
 251   //DEBUG(DEBUG_HIGH,"/drivers/mcu: Core: %s( 0x%08x )\n",coreType->name,pvr);\r
 252 \r
 253   return 0;\r
 254 }\r
 255 \r
 256 \r
 257 //-------------------------------------------------------------------\r
 258 \r
 259 void Mcu_Init(const Mcu_ConfigType *configPtr)\r
 260 {\r
 261   VALIDATE( ( NULL != configPtr ), MCU_INIT_SERVICE_ID, MCU_E_PARAM_CONFIG );\r
 262 \r
 263   if( !SIMULATOR() ) {\r
 264           Mcu_CheckCpu();\r
 265   }\r
 266 \r
 267   memset(&Mcu_Global.stats,0,sizeof(Mcu_Global.stats));\r
 268 \r
 269 \r
 270   //\r
 271   // Setup memories\r
 272   //\r
 273   Mcu_ConfigureFlash();\r
 274 \r
 275   Mcu_Global.config = configPtr;\r
 276   Mcu_Global.initRun = 1;\r
 277 \r
 278   if( Mcu_Global.config->McuClockSrcFailureNotification == TRUE  ){\r
 279         // Enable loss of lock interrupt\r
 280 \r
 281         Irq_AttachIsr1(Mcu_LossOfLock, NULL, PLL_SYNSR_LOLF,10 );\r
 282 #if defined(CFG_MPC5516)\r
 283 //      FMPLL.SYNCR.B.LOCIRQ = 1; TODO: Kolla denna bortkommentering med Mårten.\r
 284         FMPLL.ESYNCR2.B.LOLIRQ = 1;\r
 285 #elif defined(CFG_MPC5554) || defined(CFG_MPC5567)\r
 286         FMPLL.SYNCR.B.LOLIRQ = 1;\r
 287 #endif\r
 288         Irq_AttachIsr1(Mcu_LossOfCLock, NULL, PLL_SYNSR_LOCF,10 );\r
 289 #if defined(CFG_MPC5516)\r
 290 //      FMPLL.SYNCR.B.LOCIRQ = 1; TODO: Kolla denna bortkommentering med Mårten.\r
 291         FMPLL.ESYNCR2.B.LOCIRQ = 1;\r
 292 #elif defined(CFG_MPC5554) || defined(CFG_MPC5567)\r
 293         FMPLL.SYNCR.B.LOCIRQ = 1;\r
 294 #endif\r
 295   }\r
 296 }\r
 297 //-------------------------------------------------------------------\r
 298 \r
 299 void Mcu_DeInit()\r
 300 {\r
 301   Mcu_Global.initRun = FALSE; // Very simple Deinit. Should we do more?\r
 302 }\r
 303 \r
 304 //-------------------------------------------------------------------\r
 305 Std_ReturnType Mcu_InitRamSection(const Mcu_RamSectionType RamSection)\r
 306 {\r
 307   VALIDATE_W_RV( ( 1 == Mcu_Global.initRun ), MCU_INITRAMSECTION_SERVICE_ID, MCU_E_UNINIT, E_NOT_OK );\r
 308   VALIDATE_W_RV( ( RamSection <= Mcu_Global.config->McuRamSectors ), MCU_INITRAMSECTION_SERVICE_ID, MCU_E_PARAM_RAMSECTION, E_NOT_OK );\r
 309 \r
 310   /* NOT SUPPORTED, reason: no support for external RAM */\r
 311 \r
 312   return E_OK;\r
 313 }\r
 314 \r
 315 \r
 316 \r
 317 //-------------------------------------------------------------------\r
 318 \r
 319 Std_ReturnType Mcu_InitClock(const Mcu_ClockType ClockSetting)\r
 320 {\r
 321   Mcu_ClockSettingConfigType *clockSettingsPtr;\r
 322   VALIDATE_W_RV( ( 1 == Mcu_Global.initRun ), MCU_INITCLOCK_SERVICE_ID, MCU_E_UNINIT, E_NOT_OK );\r
 323   VALIDATE_W_RV( ( ClockSetting < Mcu_Global.config->McuClockSettings ), MCU_INITCLOCK_SERVICE_ID, MCU_E_PARAM_CLOCK, E_NOT_OK );\r
 324 \r
 325   Mcu_Global.clockSetting = ClockSetting;\r
 326   clockSettingsPtr = &Mcu_Global.config->McuClockSettingConfig[Mcu_Global.clockSetting];\r
 327 \r
 328 \r
 329   // TODO: find out if the 5554 really works like the 5516 here\r
 330   // All three (16, 54, 67) used to run the same code here though, so i'm sticking it with 5516\r
 331 #if defined(CFG_MPC5516) || defined(CFG_MPC5554)\r
 332   /* 5516clock info:\r
 333    * Fsys - System frequency ( CPU + all periperals? )\r
 334    *\r
 335    *  Fsys = EXTAL_FREQ *(  (emfd+16) / ( (eprediv+1) * ( erfd+1 )) ) )\r
 336    */\r
 337   // Check ranges...\r
 338   assert((clockSettingsPtr->Pll2>=32) && (clockSettingsPtr->Pll2<=132));\r
 339   assert( (clockSettingsPtr->Pll1 != 6) &&\r
 340           (clockSettingsPtr->Pll1 != 8) &&\r
 341           (clockSettingsPtr->Pll1 < 10) );\r
 342   assert( clockSettingsPtr->Pll3 & 1); // Must be odd\r
 343 #elif defined(CFG_MPC5567)\r
 344   /* 5567 clock info:\r
 345    *  Fsys = EXTAL_FREQ *(  (emfd+4) / ( (eprediv+1) * ( 2^erfd )) ) )\r
 346    */\r
 347   // Check ranges...\r
 348   assert(clockSettingsPtr->Pll2 < 16);\r
 349   assert(clockSettingsPtr->Pll1 <= 4);\r
 350   assert(clockSettingsPtr->Pll3 < 8);\r
 351 #endif\r
 352 \r
 353 \r
 354 #if defined(USE_LDEBUG_PRINTF)\r
 355   {\r
 356     uint32    extal = Mcu_Global.config->McuClockSettingConfig[Mcu_Global.clockSetting].McuClockReferencePointFrequency;\r
 357     uint32    f_sys;\r
 358 \r
 359     f_sys = CALC_SYSTEM_CLOCK( extal,\r
 360         clockSettingsPtr->Pll2,\r
 361         clockSettingsPtr->Pll1,\r
 362         clockSettingsPtr->Pll3 );\r
 363 \r
 364     //DEBUG(DEBUG_HIGH,"/drivers/mcu: F_sys will be:%08d Hz\n",f_sys);\r
 365   }\r
 366 #endif\r
 367 \r
 368 #if defined(CFG_MPC5516)\r
 369   // External crystal PLL mode.\r
 370   FMPLL.ESYNCR1.B.CLKCFG = 7; //TODO: Hur ställa detta för 5567?\r
 371 \r
 372   // Write pll parameters.\r
 373   FMPLL.ESYNCR1.B.EPREDIV = clockSettingsPtr->Pll1;\r
 374   FMPLL.ESYNCR1.B.EMFD    = clockSettingsPtr->Pll2;\r
 375   FMPLL.ESYNCR2.B.ERFD    = clockSettingsPtr->Pll3;\r
 376 \r
 377   // Connect SYSCLK to FMPLL\r
 378   SIU.SYSCLK.B.SYSCLKSEL = SYSCLOCK_SELECT_PLL;\r
 379 #elif defined(CFG_MPC5554) || defined(CFG_MPC5567)\r
 380   // Partially following the steps in MPC5567 RM..\r
 381   FMPLL.SYNCR.B.DEPTH   = 0;\r
 382   FMPLL.SYNCR.B.LOLRE   = 0;\r
 383   FMPLL.SYNCR.B.LOLIRQ  = 0;\r
 384 \r
 385   FMPLL.SYNCR.B.PREDIV  = clockSettingsPtr->Pll1;\r
 386   FMPLL.SYNCR.B.MFD             = clockSettingsPtr->Pll2;\r
 387   FMPLL.SYNCR.B.RFD     = clockSettingsPtr->Pll3;\r
 388 \r
 389         // Wait for PLL to sync.\r
 390   while (Mcu_GetPllStatus() != MCU_PLL_LOCKED)\r
 391     ;\r
 392 \r
 393   FMPLL.SYNCR.B.LOLIRQ  = 1;\r
 394 #endif\r
 395 \r
 396   return E_OK;\r
 397 }\r
 398 \r
 399 //-------------------------------------------------------------------\r
 400 \r
 401 void Mcu_DistributePllClock(void)\r
 402 {\r
 403   VALIDATE( ( 1 == Mcu_Global.initRun ), MCU_DISTRIBUTEPLLCLOCK_SERVICE_ID, MCU_E_UNINIT );\r
 404   VALIDATE( ( FMPLL.SYNSR.B.LOCK == 1 ), MCU_DISTRIBUTEPLLCLOCK_SERVICE_ID, MCU_E_PLL_NOT_LOCKED );\r
 405 \r
 406   /* NOT IMPLEMENTED due to pointless function on this hardware */\r
 407 \r
 408 }\r
 409 \r
 410 //-------------------------------------------------------------------\r
 411 \r
 412 Mcu_PllStatusType Mcu_GetPllStatus(void)\r
 413 {\r
 414   VALIDATE_W_RV( ( 1 == Mcu_Global.initRun ), MCU_GETPLLSTATUS_SERVICE_ID, MCU_E_UNINIT, MCU_PLL_STATUS_UNDEFINED );\r
 415   Mcu_PllStatusType rv;\r
 416 \r
 417   if( !SIMULATOR() )\r
 418   {\r
 419     if ( !FMPLL.SYNSR.B.LOCK )\r
 420     {\r
 421       rv = MCU_PLL_UNLOCKED;\r
 422     } else\r
 423     {\r
 424       rv = MCU_PLL_LOCKED;\r
 425     }\r
 426   }\r
 427   else\r
 428   {\r
 429     /* We are running on instruction set simulator. PLL is then always in sync... */\r
 430     rv = MCU_PLL_LOCKED;\r
 431   }\r
 432 \r
 433   return rv;\r
 434 }\r
 435 \r
 436 //-------------------------------------------------------------------\r
 437 \r
 438 Mcu_ResetType Mcu_GetResetReason(void)\r
 439 {\r
 440         Mcu_ResetType rv;\r
 441 \r
 442   VALIDATE_W_RV( ( 1 == Mcu_Global.initRun ), MCU_GETRESETREASON_SERVICE_ID, MCU_E_UNINIT, MCU_RESET_UNDEFINED );\r
 443 \r
 444   if( SIU.RSR.B.SSRS ) {\r
 445         rv = MCU_SW_RESET;\r
 446   } else if( SIU.RSR.B.WDRS ) {\r
 447         rv = MCU_WATCHDOG_RESET;\r
 448   } else if( SIU.RSR.B.PORS || SIU.RSR.B.ERS ) {\r
 449         rv = MCU_POWER_ON_RESET;\r
 450   } else {\r
 451         rv = MCU_RESET_UNDEFINED;\r
 452   }\r
 453 \r
 454   return rv;\r
 455 }\r
 456 \r
 457 //-------------------------------------------------------------------\r
 458 \r
 459 Mcu_RawResetType Mcu_GetResetRawValue(void)\r
 460 {\r
 461   VALIDATE_W_RV( ( 1 == Mcu_Global.initRun ), MCU_GETRESETREASON_SERVICE_ID, MCU_E_UNINIT, MCU_GETRESETRAWVALUE_UNINIT_RV );\r
 462 \r
 463   if( !Mcu_Global.initRun ) {\r
 464         return MCU_GETRESETRAWVALUE_UNINIT_RV;\r
 465   }\r
 466 \r
 467   return SIU.RSR.R;\r
 468 }\r
 469 \r
 470 //-------------------------------------------------------------------\r
 471 \r
 472 #if ( MCU_PERFORM_RESET_API == STD_ON )\r
 473 void Mcu_PerformReset(void)\r
 474 {\r
 475   VALIDATE( ( 1 == Mcu_Global.initRun ), MCU_PERFORMRESET_SERVICE_ID, MCU_E_UNINIT );\r
 476 \r
 477   // Reset\r
 478         SIU.SRCR.B.SSR = 1;\r
 479 \r
 480 }\r
 481 #endif\r
 482 \r
 483 //-------------------------------------------------------------------\r
 484 \r
 485 void Mcu_SetMode(const Mcu_ModeType McuMode)\r
 486 {\r
 487   VALIDATE( ( 1 == Mcu_Global.initRun ), MCU_SETMODE_SERVICE_ID, MCU_E_UNINIT );\r
 488  // VALIDATE( ( McuMode <= Mcu_Global.config->McuNumberOfMcuModes ), MCU_SETMODE_SERVICE_ID, MCU_E_PARAM_MODE );\r
 489   (void) McuMode;\r
 490 \r
 491   /* NOT SUPPORTED */\r
 492 }\r
 493 \r
 494 //-------------------------------------------------------------------\r
 495 \r
 496 /**\r
 497  * Get the system clock in Hz. It calculates the clock from the\r
 498  * different register settings in HW.\r
 499  */\r
 500 uint32_t McuE_GetSystemClock(void)\r
 501 {\r
 502   /*\r
 503    * System clock calculation\r
 504    *\r
 505    * 5516 -  f_sys = extal * (emfd+16) / ( (eprediv+1) * ( erfd+1 ));\r
 506    * 5567 -  f_sys = extal * (emfd+4) / ( (eprediv+1) * ( 2^erfd ));\r
 507    * 563x -  We run in legacy mode = 5567\r
 508    */\r
 509 #if defined(CFG_MPC5516)\r
 510   uint32_t eprediv = FMPLL.ESYNCR1.B.EPREDIV;\r
 511   uint32_t emfd = FMPLL.ESYNCR1.B.EMFD;\r
 512   uint32_t erfd = FMPLL.ESYNCR2.B.ERFD;\r
 513 #elif defined(CFG_MPC5554) || defined(CFG_MPC5567) || defined(CFG_MPC5633)\r
 514   uint32_t eprediv = FMPLL.SYNCR.B.PREDIV;\r
 515   uint32_t emfd = FMPLL.SYNCR.B.MFD;\r
 516   uint32_t erfd = FMPLL.SYNCR.B.RFD;\r
 517 #endif\r
 518   uint32_t f_sys;\r
 519   uint32  extal = Mcu_Global.config->McuClockSettingConfig[Mcu_Global.clockSetting].McuClockReferencePointFrequency;\r
 520 \r
 521   f_sys =  CALC_SYSTEM_CLOCK(extal,emfd,eprediv,erfd);\r
 522 \r
 523 #if defined(CFG_MPC5516)\r
 524   // Scale according to SYSCLKDIV to get "System Clock". See RM Figure 3-1\r
 525   uint32_t sysclkdiv = SIU.SYSCLK.B.SYSCLKDIV;\r
 526   f_sys = f_sys / (1 << sysclkdiv);\r
 527 #elif defined(CFG_MPC5554) || defined(CFG_MPC5567) || defined(CFG_MPC5633)\r
 528   // TODO: Do these CPUs have an equvalent?\r
 529 #endif\r
 530 \r
 531   return f_sys;\r
 532 }\r
 533 \r
 534 imask_t McuE_EnterCriticalSection()\r
 535 {\r
 536   uint32_t msr = get_msr();\r
 537   Irq_Disable();\r
 538   return msr;\r
 539 }\r
 540 \r
 541 void McuE_ExitCriticalSection(uint32_t old_state)\r
 542 {\r
 543   set_msr(old_state);\r
 544 }\r
 545 \r
 546 /**\r
 547  * Get the peripheral clock in Hz for a specific device\r
 548  */\r
 549 \r
 550 uint32_t McuE_GetPeripheralClock(McuE_PeriperalClock_t type)\r
 551 {\r
 552 #if defined(CFG_MPC5567)\r
 553         // No peripheral dividers on 5567.\r
 554         return McuE_GetSystemClock();\r
 555 #else\r
 556         uint32_t sysClock = McuE_GetSystemClock();\r
 557         vuint32_t prescaler;\r
 558 \r
 559 \r
 560   // See table 3.1, section 3.4.5 Peripheral Clock dividers\r
 561         switch (type)\r
 562         {\r
 563         case PERIPHERAL_CLOCK_FLEXCAN_A:\r
 564         case PERIPHERAL_CLOCK_DSPI_A:\r
 565 #if defined(CFG_MPC5516)\r
 566                 prescaler = SIU.SYSCLK.B.LPCLKDIV0;\r
 567                 break;\r
 568 #endif\r
 569 \r
 570         case PERIPHERAL_CLOCK_PIT:\r
 571         case PERIPHERAL_CLOCK_ESCI_A:\r
 572         case PERIPHERAL_CLOCK_IIC_A:\r
 573 #if defined(CFG_MPC5516)\r
 574         prescaler = SIU.SYSCLK.B.LPCLKDIV1;\r
 575                 break;\r
 576 #endif\r
 577 \r
 578         case PERIPHERAL_CLOCK_FLEXCAN_B:\r
 579         case PERIPHERAL_CLOCK_FLEXCAN_C:\r
 580         case PERIPHERAL_CLOCK_FLEXCAN_D:\r
 581         case PERIPHERAL_CLOCK_FLEXCAN_E:\r
 582         case PERIPHERAL_CLOCK_FLEXCAN_F:\r
 583 #if defined(CFG_MPC5516)\r
 584                 prescaler = SIU.SYSCLK.B.LPCLKDIV2;\r
 585                 break;\r
 586 #endif\r
 587 \r
 588         case PERIPHERAL_CLOCK_DSPI_B:\r
 589         case PERIPHERAL_CLOCK_DSPI_C:\r
 590         case PERIPHERAL_CLOCK_DSPI_D:\r
 591 #if defined(CFG_MPC5516)\r
 592                 prescaler = SIU.SYSCLK.B.LPCLKDIV3;\r
 593                 break;\r
 594 #endif\r
 595 \r
 596         case PERIPHERAL_CLOCK_ESCI_B:\r
 597         case PERIPHERAL_CLOCK_ESCI_C:\r
 598         case PERIPHERAL_CLOCK_ESCI_D:\r
 599         case PERIPHERAL_CLOCK_ESCI_E:\r
 600         case PERIPHERAL_CLOCK_ESCI_F:\r
 601         case PERIPHERAL_CLOCK_ESCI_G:\r
 602         case PERIPHERAL_CLOCK_ESCI_H:\r
 603 #if defined(CFG_MPC5516)\r
 604           prescaler = SIU.SYSCLK.B.LPCLKDIV4;\r
 605           break;\r
 606 #endif\r
 607 \r
 608         case PERIPHERAL_CLOCK_EMIOS:\r
 609 #if defined(CFG_MPC5516)\r
 610           prescaler = SIU.SYSCLK.B.LPCLKDIV5;\r
 611           break;\r
 612 #endif\r
 613 \r
 614         case PERIPHERAL_CLOCK_MLB:\r
 615 #if defined(CFG_MPC5516)\r
 616           prescaler = SIU.SYSCLK.B.LPCLKDIV6;\r
 617           break;\r
 618 #endif\r
 619 \r
 620         default:\r
 621                 assert(0);\r
 622                 break;\r
 623         }\r
 624 \r
 625         return sysClock/(1<<prescaler);\r
 626 #endif\r
 627 }\r
 628 \r
 629 \r
 630 /**\r
 631  * Function to setup the internal flash for optimal performance\r
 632  */\r
 633 \r
 634 static void Mcu_ConfigureFlash(void)\r
 635 {\r
 636   /* These flash settings increases the CPU performance of 7 times compared\r
 637    to reset default settings!! */\r
 638 \r
 639 #if defined(CFG_MPC5516)\r
 640   /* Disable pipelined reads when flash options are changed. */\r
 641   FLASH.MCR.B.PRD = 1;\r
 642 \r
 643   /* Enable master prefetch for e200z1 and eDMA. */\r
 644   FLASH.PFCRP0.B.M0PFE = 1;\r
 645   FLASH.PFCRP0.B.M2PFE = 1;\r
 646 \r
 647   /* Address pipelining control. Must be set to the same value as RWSC. */\r
 648   FLASH.PFCRP0.B.APC = 2;\r
 649   FLASH.PFCRP0.B.RWSC = 2;\r
 650 \r
 651   /* Write wait states. */\r
 652   FLASH.PFCRP0.B.WWSC = 1;\r
 653 \r
 654   /* Enable data prefetch. */\r
 655   FLASH.PFCRP0.B.DPFEN = 1;\r
 656 \r
 657   /* Enable instruction prefetch. */\r
 658   FLASH.PFCRP0.B.IPFEN = 1;\r
 659 \r
 660   /* Prefetch algorithm. */\r
 661   /* TODO: Ask Freescale about this option. */\r
 662   FLASH.PFCRP0.B.PFLIM = 2;\r
 663 \r
 664   /* Enable line read buffers. */\r
 665   FLASH.PFCRP0.B.BFEN = 1;\r
 666 \r
 667   /* Enable pipelined reads again. */\r
 668   FLASH.MCR.B.PRD = 0;\r
 669 #elif defined(CFG_MPC5554) || defined(CFG_MPC5567)\r
 670   //TODO: Lägg till flash för mpc5554 &67\r
 671 #endif\r
 672 }\r
 673 \r
 674 void McuE_EnableInterrupts(void)\r
 675 {\r
 676   Irq_Enable();\r
 677 }\r
 678 \r
 679 void McuE_DisableInterrupts(void)\r
 680 {\r
 681   Irq_Disable();\r
 682 }\r