#include "System.h"

// PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
#define PLL_M 8
#ifndef EnableOverclocking
#define PLL_N 336
#else
#define PLL_N 352
#endif

// SYSCLK = PLL_VCO / PLL_P
#define PLL_P 2

// USB OTG FS, SDIO and RNG Clock =  PLL_VCO / PLLQ
#define PLL_Q 7

// PLLI2S_VCO = (HSE_VALUE Or HSI_VALUE / PLL_M) * PLLI2S_N
// I2SCLK = PLLI2S_VCO / PLLI2S_R
#define PLLI2S_N 192
#define PLLI2S_R 5

static void InitializeClocks();

void InitializeSystem()
{
	// Reset the RCC clock configuration to the default reset state
	RCC->CR|=0x00000001; // Set HSION bit
	RCC->CFGR=0x00000000;	// Reset CFGR register
	RCC->CR&=0xFEF6FFFF; // Reset HSEON, CSSON and PLLON bits
	RCC->PLLCFGR=0x24003010; // Reset PLLCFGR register
	RCC->CR&=0xFFFBFFFF; // Reset HSEBYP bit
	RCC->CIR=0x00000000; // Disable all interrupts

	// Configure the System clock source, PLL Multiplier and Divider factors, 
	// AHB/APBx prescalers and Flash settings
	InitializeClocks();

	// Set vector table offset to flash memory start.
	SCB->VTOR=FLASH_BASE;

	// Set up interrupts to 4 bits preemption priority.
	SCB->AIRCR=0x05FA0000|0x300;
}

static void InitializeClocks()
{
	// PLL (clocked by HSE) used as System clock source.

	// Enable HSE and wait until it is read.
	RCC->CR|=RCC_CR_HSEON;
	while(!(RCC->CR&RCC_CR_HSERDY));

	// Enable high performance mode, System frequency up to 168 MHz.
	RCC->APB1ENR|=RCC_APB1ENR_PWREN;
	PWR->CR|=PWR_CR_PMODE;  

	RCC->CFGR|=RCC_CFGR_HPRE_DIV1; // HCLK = SYSCLK / 1
	RCC->CFGR|=RCC_CFGR_PPRE2_DIV2; // PCLK2 = HCLK / 2
	RCC->CFGR|=RCC_CFGR_PPRE1_DIV4; // PCLK1 = HCLK / 4

	// Configure the main PLL.
	RCC->PLLCFGR=PLL_M|(PLL_N<<6)|(((PLL_P>>1)-1)<<16)|(RCC_PLLCFGR_PLLSRC_HSE)|(PLL_Q<<24);

	// Enable the main PLL and wait until it is ready.
	RCC->CR|=RCC_CR_PLLON;
	while(!(RCC->CR&RCC_CR_PLLRDY));
   
	// Configure Flash prefetch, Instruction cache, Data cache and wait state
	FLASH->ACR=FLASH_ACR_ICEN|FLASH_ACR_DCEN|FLASH_ACR_LATENCY_5WS;

	// Select the main PLL as system clock source.
	RCC->CFGR&=~RCC_CFGR_SW;
	RCC->CFGR|=RCC_CFGR_SW_PLL;

	// Wait until the main PLL is used as system clock source.
	while((RCC->CFGR&RCC_CFGR_SWS)!=RCC_CFGR_SWS_PLL);
}



// Interrupt handlers.

static InterruptHandler **WritableInterruptTable();
void Default_Handler();

extern InterruptHandler *__isr_vector_sram[];

void InstallInterruptHandler(IRQn_Type interrupt,InterruptHandler handler)
{
	// TODO: Disable interrupts.
	InterruptHandler **currenttable=WritableInterruptTable();
	currenttable[interrupt+16]=handler;
}

void RemoveInterruptHandler(IRQn_Type interrupt,InterruptHandler handler)
{
	// TODO: Disable interrupts.
	InterruptHandler **currenttable=WritableInterruptTable();
	currenttable[interrupt+16]=Default_Handler;
}

static InterruptHandler **WritableInterruptTable()
{
	InterruptHandler **currenttable=(InterruptHandler **)SCB->VTOR;
	if((uint32_t)currenttable==FLASH_BASE)
	{
		InterruptHandler **flashtable=(InterruptHandler **)FLASH_BASE;
		currenttable=__isr_vector_sram;
		for(int i=0;i<98;i++) currenttable[i]=flashtable[i];

		SCB->VTOR=(uint32_t)currenttable;
	}

	return currenttable;
}