#include <inarm.h>
#include <iolpc2129.h>

#define PLOCK 0x0400

void PLLinit(void){
 PLLCON = 1;      // Enable the PLL
 PLLFEED = 0xAA;
 PLLFEED = 0x55;
 PLLCFG = 0x23;  //M=3(x4),P=1(/2) PLLfreq = 235.9296 Mhz, CCLK = 58,9824 Mhz
 PLLFEED = 0xAA;
 PLLFEED = 0x55;
 while(!(PLLSTAT & PLOCK));    // Wait for PLL to lock
 PLLCON = 3;      // Enable & connct the PLL
 PLLFEED = 0xAA;
 PLLFEED = 0x55;
}

#define PTIM0 1
#define PTIM1 2
#define PURT0 3
#define PURT1 4
#define PPWM0 5
#define PI2C 7
#define PSPI0 8
#define PRTC 9
#define PSPI1 10
#define PAD 12
#define PCAN1 13
#define PCAN2 14

#define   OSCILLATOR_CLOCK_FREQUENCY  14745600

typedef unsigned char           U8;             /*  8 bit unsigned UPGRADE*/
typedef unsigned short          U16;            /* 16 bit unsigned */
typedef unsigned long           U32;            /* 32 bit unsigned */

void CPUinit(void){
 U32 x;
    // Управление питанием --------------------------------------------------------->
    // Выключаем питание неиспользуемых устройств: ТIMER1, UART1, PWM, RTC, SPI1, ADC, CAN2
    PCONP &= ~((1<<PTIM1)|(1<<PURT1)|(1<<PPWM0)|(1<<PRTC)|(1<<PSPI1)|(1<<11)|(1<<PAD)|(1<<PCAN2));
     // Установка карты памяти обработчика исключений ------------------------------->
#pragma segment = "INTVEC"
    // проверка размещения сегмента INTVEC
    if (( void * )0x00000000UL == __segment_begin( "INTVEC" )){
       MEMMAP = 1;    // flash mode
   }else{      // Отладка в ram
 MEMMAP = 2;
   }
    //Init MAM & Flash memory fetch------------------------------------------------->
    MAMCR = 0;  //0 - запрет акселератора перед изменением тайминга.
    x = getprocessorClockFreq();    // Текущая частота процессора
    if(x < 20000000) MAMTIM = 1;
   if(x >= 20000000 && x < 40000000) MAMTIM = 2;
   if(x > 40000000) MAMTIM = 3; // для частот больше 40МГц, период обращения к памяти не меньше 3 cclock
    MAMCR = 2;  //10 - MAM functions fully enabled
    // Установка частоты шины переферийных устройств,- полная скорость pclk = cclck
    VPBDIV = 1;  //01: VPB bus clock is the same as the processor clock.
//  VPBDIV = 2;  //10: VPB bus clock is 1/2 of the processor clock.
    // Установка портов GPIO ------------------------------------------------------->
 //GPIO init
 // P0.15:8 - GPIO      P0.7:4 SPI I2C  UART
 // 0000 0000 0000 0000 0001 0101  0101 0000
 PINSEL0=0x1550;
 // P0.31:16-GPIO  P0.25 - CAN1RX ??? - непонятно пока
 // 0000 0000 0000 0100 0000 0000 0000 0000
 PINSEL1=0x00040000;
 //IODIR in binary 0111 1101 0010 1010 1001 0100 1101 0001
 IO0DIR = 0x7D2A94DD;
 IO1DIR = 0;      // Все линии на ввод
 IO0CLR = 0x78000000;    // СБРОС СВЕТОДИОДОВ
}

U32 getprocessorClockFreq(void){
   if((PLLCON & 0x3) == 0x3)  // если PLL подключён (PLLC & PLLE = 1), то множитель = MSEL + 1
       return (((PLLCFG & 0x1F) + 1) * OSCILLATOR_CLOCK_FREQUENCY);
   return OSCILLATOR_CLOCK_FREQUENCY;
}
/*-------------------------------------------------------------------------------------------------------------------------------*/
U32 getperipheralClockFreq(void){
    U8 c;
 do{  // Для кремния ревизии А, эррата VPBDIV.1 от 2005 May 2
   c = VPBDIV & 0x03;
 }while((VPBDIV & 0x03) != c);
    switch (c)
     {
       case 1:
         c = 1;
         break;
       case 2:
         c = 2;
         break;
       default:
         c = 4;
     }
    return getprocessorClockFreq() / ((U32)c);
}