/*
 * init_ll.c
 *
 *  Created on: 27.4.2014
 *      Author: Stefan
 */

/* Includes ------------------------------------------------------------------*/
#include <system.h>

#include <stm32f4xx_gpio.h>
#include <stm32f4xx_flash.h>
#include <misc.h>


/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
#define INIT_EXTMEM_BUSTURN     5
#define INIT_EXTMEM_DATASET     5
#define INIT_EXTMEM_ADDRHOLD    5
#define INIT_EXTMEM_ADDRSETUP   5

#define INIT_CLK_PLLM    4   /* Preffered VCO input freq. = 2MHz (= 8 / 4) */
#define INIT_CLK_PLLN    168 /* 2Mhz * 168 = 336MHz */
#define INIT_CLK_PLLP    0   /* Main Sys clk = 336MHz / 2 = 168MHz */
#define INIT_CLK_PLLQ    7   /* Perif clk = 336MHz / 7 = 48MHz */
#define INIT_CLK_HPRE    0   /* AHB clk = 168MHz / 1 = 168MHz */
#define INIT_CLK_PPRE1   5   /* APB1 clk = 168MHz / 4 = 42MHz */
#define INIT_CLK_PPRE2   4   /* APB2 clk = 168MHz / 2 = 84MHz */
#define INIT_FLASH_LAT   5   /* Flash latency value */

/* Private variables ---------------------------------------------------------*/
/* Public variables ----------------------------------------------------------*/
uint32_t SystemCoreClock = 16000000;

/* Private functions ---------------------------------------------------------*/
static bool_t init_clock(void)
{
    bool_t response  = FALSE;

    /* Reset RCC_CFGR */
    RCC->CFGR = 0;

    /* HSE Setup */
    /* Disable HSE bypass */
    RCC->CR &= ~RCC_CR_HSEBYP;
    /* Enable HSE oscillator */
    RCC->CR |= RCC_CR_HSEON;
    __DSB();

    /* Wait for HSE to stabilize */
    while(0 == (RCC->CR & RCC_CR_HSERDY))
    {
    }

    /* PLL Setup */
    #define RCC_PLLCFGR_PLLM_OFFSET     0
    #define RCC_PLLCFGR_PLLN_OFFSET     6
    #define RCC_PLLCFGR_PLLP_OFFSET     16
    #define RCC_PLLCFGR_PLLQ_OFFSET     24

    /* Disable PLL and PLLI2S */
    RCC->CR &= ~RCC_CR_PLLON;
    RCC->CR &= ~RCC_CR_PLLI2SON;

    /* Set HSE as PLL input */
    RCC->PLLCFGR &= ~RCC_PLLCFGR_PLLSRC;
    RCC->PLLCFGR |= RCC_PLLCFGR_PLLSRC_HSE;

    /* Set PLLM */
    RCC->PLLCFGR &= ~RCC_PLLCFGR_PLLM;
    RCC->PLLCFGR |=
            ((INIT_CLK_PLLM << RCC_PLLCFGR_PLLM_OFFSET) & RCC_PLLCFGR_PLLM);

    /* PLLN */
    RCC->PLLCFGR &= ~RCC_PLLCFGR_PLLN;
    RCC->PLLCFGR |=
            ((INIT_CLK_PLLN << RCC_PLLCFGR_PLLN_OFFSET) & RCC_PLLCFGR_PLLN);

    /* PLLP */
    RCC->PLLCFGR &= ~RCC_PLLCFGR_PLLP;
    RCC->PLLCFGR |=
            ((INIT_CLK_PLLP << RCC_PLLCFGR_PLLP_OFFSET) & RCC_PLLCFGR_PLLP);

    /* PLLQ */
    RCC->PLLCFGR &= ~RCC_PLLCFGR_PLLQ;
    RCC->PLLCFGR |=
            ((INIT_CLK_PLLQ << RCC_PLLCFGR_PLLQ_OFFSET) & RCC_PLLCFGR_PLLQ);

    /* Turn on Main PLL */
    RCC->CR |= RCC_CR_PLLON;
    __DSB();

    /* Wait for PLL to stabilize */
    while(0 == (RCC->CR & RCC_CR_PLLRDY))
    {
    }

    /* Setup SYSCLK */
    #define RCC_CFGR_SW_OFFSET      0
    #define RCC_CFGR_SWS_OFFSET     2
    #define RCC_CFGR_HPRE_OFFSET    4
    #define RCC_CFGR_PPRE1_OFFSET   10
    #define RCC_CFGR_PPRE2_OFFSET   13

    /* PPRE1 */
    RCC->CFGR &= ~RCC_CFGR_PPRE1;
    RCC->CFGR |=
            ((INIT_CLK_PPRE1 << RCC_CFGR_PPRE1_OFFSET) & RCC_CFGR_PPRE1);

    /* PPRE1 */
    RCC->CFGR &= ~RCC_CFGR_PPRE2;
    RCC->CFGR |=
            ((INIT_CLK_PPRE2 << RCC_CFGR_PPRE2_OFFSET) & RCC_CFGR_PPRE2);

    /* HPRE */
    RCC->CFGR &= ~RCC_CFGR_HPRE;
    RCC->CFGR |=
            ((INIT_CLK_HPRE << RCC_CFGR_HPRE_OFFSET) & RCC_CFGR_HPRE);

    /* Before switching clock source, set new flash latency value */
    #define FLASH_ACR_LATENCY_OFFSET        0
    /* Set the new latency value */
    FLASH->ACR &= ~FLASH_ACR_LATENCY;
    FLASH->ACR |= ((INIT_FLASH_LAT << FLASH_ACR_LATENCY_OFFSET)
        & FLASH_ACR_LATENCY);
    /* Wait for IO operation to complete */
    __DSB();

    /* Set PLL as System clock source */
    RCC->CFGR &= ~RCC_CFGR_SW;
    RCC->CFGR |= RCC_CFGR_SW_1;
    /* Wait for IO to complete */
    __DSB();

    response = (
            ((RCC->CFGR & RCC_CFGR_SWS) >> RCC_CFGR_SWS_OFFSET)
            == RCC_CFGR_SW_1
        );
    if(TRUE == response)
    {
        SystemCoreClock = 168000000;
    }

    return response;
}

static void init_extmem(
    void
)
{
    GPIO_InitTypeDef gpio;


    /* Enable FSMC */
    RCC->AHB3ENR |= RCC_AHB3ENR_FSMCEN;
    __DSB();

    /*  FSMC driver settings   */
    #define FSMC_BCR1 0
    #define FSMC_BTR1 1

    /* First disable FSMC Bank1 */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_MBKEN;

    /* SETup FSMC timing */
    #define FSMC_BTR1_BUSTURN_OFFSET    16
    #define FSMC_BTR1_DATAST_OFFSET     8
    #define FSMC_BTR1_ADDHLD_OFFSET     4
    #define FSMC_BTR1_ADDSET_OFFSET     0

    /* Bus Turn (#CE cahnge) */
    FSMC_Bank1->BTCR[FSMC_BTR1] &= ~FSMC_BTR1_BUSTURN;
    FSMC_Bank1->BTCR[FSMC_BTR1] |=
            ((INIT_EXTMEM_BUSTURN << FSMC_BTR1_BUSTURN_OFFSET)
                & FSMC_BTR1_BUSTURN);

    /* Data Set */
    FSMC_Bank1->BTCR[FSMC_BTR1] &= ~FSMC_BTR1_DATAST;
    FSMC_Bank1->BTCR[FSMC_BTR1] |=
            ((INIT_EXTMEM_DATASET << FSMC_BTR1_DATAST_OFFSET)
                & FSMC_BTR1_DATAST);

    /* Address Hold */
    FSMC_Bank1->BTCR[FSMC_BTR1] &= ~FSMC_BTR1_ADDHLD;
    FSMC_Bank1->BTCR[FSMC_BTR1] |=
            ((INIT_EXTMEM_ADDRHOLD << FSMC_BTR1_ADDHLD_OFFSET)
                & FSMC_BTR1_ADDHLD);

    /* Address Setup */
    FSMC_Bank1->BTCR[FSMC_BTR1] &= ~FSMC_BTR1_ADDSET;
    FSMC_Bank1->BTCR[FSMC_BTR1] |=
            ((INIT_EXTMEM_ADDRSETUP << FSMC_BTR1_ADDSET_OFFSET)
                & FSMC_BTR1_ADDSET);


    /* Disable write bursts */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_CBURSTRW;
    /* Disable asynchronous write */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_ASYNCWAIT;
    /* Not usig the extended mode */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_EXTMOD;
    /* Disable NWAIT */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_WAITEN;
    /* Enable write operations */
    FSMC_Bank1->BTCR[FSMC_BCR1] |= FSMC_BCR1_WREN;
    /*  Set WAITCFG, WRAPMOD, WAITPOL a BURSTEN  bits to their default values */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~(FSMC_BCR1_WAITCFG | FSMC_BCR1_WRAPMOD |
            FSMC_BCR1_WAITPOL | FSMC_BCR1_BURSTEN);
    /* Disable NOR flash */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_FACCEN;
    /* 16 bit Data bus */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_MWID;
    FSMC_Bank1->BTCR[FSMC_BCR1] |= FSMC_BCR1_MWID_0;
    /* The memory type is SRAM */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_MTYP;
    /* Disable address and data bus multiplexing */
    FSMC_Bank1->BTCR[FSMC_BCR1] &= ~FSMC_BCR1_MUXEN;

    /* Wait for IO to finish */
    __DSB();

    /* GPIO settings for FSMC */
    GPIO_StructInit(&gpio);

    gpio.GPIO_Mode  = GPIO_Mode_AF;      /* < Alternate function */
    gpio.GPIO_OType = GPIO_OType_PP;     /* < Push-Pull */
    gpio.GPIO_PuPd  = GPIO_PuPd_NOPULL;  /* < No pull-up, pull-down */
    gpio.GPIO_Speed = GPIO_Speed_100MHz; /* < Output speed */

    /* Enable GPIOs */
    RCC->AHB1ENR |= (RCC_AHB1ENR_GPIODEN | RCC_AHB1ENR_GPIOEEN |
            RCC_AHB1ENR_GPIOFEN | RCC_AHB1ENR_GPIOGEN);
    __DSB();

    /* GPIOD */
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource0, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource1, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource4, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource5, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource7, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource8, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource9, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource10, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource11, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource15, GPIO_AF_FSMC);
    gpio.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4 | GPIO_Pin_5 |
        GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11 |
        GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
    GPIO_Init(GPIOD, &gpio);


    /* GPIOE */
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource0, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource1, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource7, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource8, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource9, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource10, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource11, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource12, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource13, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource14, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOE, GPIO_PinSource15, GPIO_AF_FSMC);
    gpio.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_7 | GPIO_Pin_8 |
        GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12 | GPIO_Pin_13 |
        GPIO_Pin_14 | GPIO_Pin_15;
    GPIO_Init(GPIOE, &gpio);


    /* GPIOF */
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource0, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource1, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource2, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource3, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource4, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource5, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource12, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource13, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource14, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOF, GPIO_PinSource15, GPIO_AF_FSMC);
    gpio.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 |
        GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 |
        GPIO_Pin_15;
    GPIO_Init(GPIOF, &gpio);


    /* GPIOG */
    GPIO_PinAFConfig(GPIOG, GPIO_PinSource0, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOG, GPIO_PinSource1, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOG, GPIO_PinSource2, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOG, GPIO_PinSource3, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOG, GPIO_PinSource4, GPIO_AF_FSMC);
    GPIO_PinAFConfig(GPIOG, GPIO_PinSource5, GPIO_AF_FSMC);
    gpio.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 |
        GPIO_Pin_4 | GPIO_Pin_5;
    GPIO_Init(GPIOG, &gpio);


    /* Enable Bank 1 */
    FSMC_Bank1->BTCR[FSMC_BCR1] |= FSMC_BCR1_MBKEN;
    __DSB();
    __ISB();
}

/*#define EXTMEM_SIZE_HWORD   0x80000

bool_t extmem_test(
    void
)
{
    uint16_t *mem = (uint16_t*)0x60000000;
    uint32_t pos;
    uint16_t data;

    data = 0;
    for(pos = 0; pos < EXTMEM_SIZE_HWORD; pos ++)
    {
        mem[pos] = data;
        data = ((data + 1) % 33257);
    }

    data = 0;
    for(pos = 0; pos < EXTMEM_SIZE_HWORD; pos ++)
    {
        if(mem[pos] != data)
        {
            break;
        }
        data = ((data + 1) % 33257);
    }

    return (pos == EXTMEM_SIZE_HWORD);
}*/

/* Public functions ----------------------------------------------------------*/
void SystemInit()
{
    uint32_t pc;

    /* Enable CCM */
    RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN;
    __DSB();
    __ISB();

    /* Enable SYSCFG */
    RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
    /* Wait until all memory transactions complete before remapping */
    __DSB();

    /* Remap */
    SYSCFG->MEMRMP = CODE_PLACE;

    /* Wait until remapping is done */
    __DMB();
    /* Flush instruction pipe */
    __ISB();
    /* Enable FPU */
    SCB->CPACR |= 0xF00000;

    /* Configure NVIC 4 preemption bits */
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);

    /* Flash setup */
    FLASH_PrefetchBufferCmd(ENABLE);
    FLASH_DataCacheCmd(ENABLE);
    FLASH_InstructionCacheCmd(ENABLE);

    /* Configure clock */
    (void)init_clock();

    /* Configure external Memory */
    /* Get Program counter value */
    __asm volatile("mov %0, pc" : "=r" (pc));
    /* Configure EXTMEM only if code is not executed from it */
    if(pc < 0x60000000)
    {
        init_extmem();
    }

}
