#include "flash.h"
#include "peripherals.h"
#include "pin_names.h"
#include "pin.h"
#include "register.h"
#include "ticker.h"
#include "scb.h"
#include "rcc.h"
#include "assertions.h"

DEFINE_THIS_FILE;

const uint32_t SYSCLOCK_Frequency	= 160000000UL;
const uint32_t HCLOCK_Frequency		=  80000000UL;
const uint32_t PCLOCK_1_Frequency	=  20000000UL;
const uint32_t PCLOCK_2_Frequency	=  40000000UL;

void ControlAllPeripheralResets(BitOperation op)
{
	RCC&    resetController(*new RCC);

	// Operate on AHB1 Peripherals
	resetController.ResetPeripherals( GPIOA_RST  | GPIOB_RST | GPIOC_RST | GPIOD_RST |
			                          GPIOE_RST  | GPIOF_RST | GPIOG_RST | GPIOH_RST |
			                          GPIOI_RST  | CRC_RST   | DMA1_RST	 | DMA2_RST	 |
			                          ETHMAC_RST | OTG_H_RST, op);

	// Operate on AHB2 Peripherals
	resetController.ResetPeripherals( DCMI_RST | CRYP_RST | HSAH_RST | RNG_RST | OTG_F_RST, op );

	// Operate on AHB3 Peripherals
	resetController.ResetPeripherals( FSMC_RST, op );

	// Operate on APB1 Peripherals
	resetController.ResetPeripherals( TIM2_RST   | TIM3_RST   | TIM4_RST    | TIM5_RST    |
			                          TIM6_RST   | TIM7_RST   | TIM12_RST   | TIM13_RST   |
			                          TIM14_RST  | WWDG_RST   | SPI2_RST    | SPI3_RST    |
			                          USART2_RST | USART3_RST | UART4_RST   | UART5_RST   |
			                          I2C1_RST   | I2C2_RST   | I2C3_RST    | CAN1_RST    |
			                          CAN2_RST   | PWR_RST    | DAC_RST, op );

	// Operate on APB2 Peripherals
	resetController.ResetPeripherals( TIM1_RST   | TIM8_RST   | USART1_RST  | USART6_RST  |
					                  ADC_RST    | SDIO_RST   | SPI1_RST    | SYSCFG_RST  |
					                  TIM9_RST   | TIM10_RST  | TIM11_RST, op);
}

void ControlAllPeripheralClocks(BitOperation op)
{
	RCC&    resetController(*new RCC);
	// Enable AHB1 Peripheral Clocks
	resetController.EnableClocks( GPIOA_EN     | GPIOB_EN      | GPIOC_EN     | GPIOD_EN      |
			                      GPIOE_EN     | GPIOF_EN      | GPIOG_EN     | GPIOH_EN      |
			                      GPIOI_EN     | CRC_EN        | DMA1_EN      | DMA2_EN       |
			                      ETHMAC_EN    | ETHMAC_TX_EN  | ETHMAC_RX_EN | ETHMAC_PTP_EN |
			                      OTG_HS_EN    | OTG_HS_ULPI_EN, op);

	// Enable AHB2 Peripheral Clocks
	resetController.EnableClocks( DCMI_EN | CRYP_EN | HASH_EN | RNG_EN | OTG_FS_EN, op);

	// Enable AHB3 Peripheral Clocks
	resetController.EnableClocks( FSMC_EN, op);

	// Enable APB1 Peripheral Clocks
	resetController.EnableClocks( TIM2_EN      | TIM3_EN       | TIM4_EN     | TIM5_EN       |
		            	          TIM6_EN      | TIM7_EN       | TIM12_EN    | TIM13_EN      |
		            	          TIM14_EN     | WWDG_EN       | SPI2_EN     | SPI3_EN       |
		            	          USART2_EN    | USART3_EN     | UART4_EN    | UART5_EN      |
		            	          I2C1_EN      | I2C2_EN       | I2C3_EN     | CAN1_EN       |
		            	          CAN2_EN      | PWR_EN        | DAC_EN, op);

	// Enable APB2 Peripheral Clocks
	resetController.EnableClocks( TIM1_EN      | TIM8_EN       | USART1_EN   | USART6_EN     |
		                          ADC1_EN      | ADC2_EN       | ADC3_EN     | SDIO_EN       |
		                          SPI1_EN      | SYSCFG_EN     | TIM11_EN    | TIM10_EN      |
		                          TIM9_EN, op);
}

void SetupClocks(void)
{
	FLASH&      flashController(*new FLASH);
	RCC&    	resetController(*new RCC);
	uint32_t	sysclk;
	uint32_t	hclk;
	uint32_t	pclk_1;
	uint32_t	pclk_2;

	// Enable clocks to internal memory
	resetController.EnableClocks( BKPSRAM_EN | CCMDATARAM_EN, ASSERT);

	// Make sure that we're running from the high speed internal clock
	// Turn on the high speed internal clock (nominally 16 MHz)
	resetController.Configure_HS_InternalOscillator(RCC::ENABLED);
	// Wait until the high speed internal oscillator is stable
	while (!resetController.Is_HS_InternalOscillatorReady(1000)) ;

	// Enable all peripherals clocks, except internal memory
	ControlAllPeripheralClocks(ASSERT);

	// Assert and hold RESET to all peripherals
	ControlAllPeripheralResets(ASSERT);

	// Enable the high speed external oscillator (25 MHz)
	resetController.Configure_HS_ExternalOscillator(RCC::HSE_ON);
	// Wait for the oscillator to be stable
	while (!resetController.Is_HS_ExternalOscillatorReady(1000)) ;

	// Release RESET to all peripherals
	ControlAllPeripheralResets(NEGATE);

	// Disable clocks to all peripherals, except internal memory
	ControlAllPeripheralClocks(NEGATE);

	// Set the Flash Memory Latency to 5 wait states
	flashController.SetLatency(5);

	// Configure PLL, SYSCLOCK = 160 MHz, AHBCLOCK = 80 MHz,
	//                PCLOCK1  =  20 MHz, PCLOCK2  = 40 MHz
	resetController.Initialize_PLL(RCC::HS_ExternalOscillator, 25, 320, RCC::PLL_DividedByTwo, 8);
	resetController.Select_AHB_ClockDivider(RCC::SYSCLK_DividedBy2);
	resetController.Select_APB1_ClockDivider(RCC::AHBCLOCK_DividedBy4);
	resetController.Select_APB2_ClockDivider(RCC::AHBCLOCK_DividedBy2);

	// Enable the PLL
	resetController.Configure_PLL(RCC::PLL_ON);
	// Wait for the PLL to stabilize
	while (!resetController.Is_PLL_Ready(1000)) ;

	// Select the PLL as the SYSCLOCK clock source
	resetController.SetSystemClockSource(RCC::PhaseLockedLoop);
	// Wait until the PLL is the SYSCLOCK source
	while (!resetController.GetSystemClockSource() == RCC::PhaseLockedLoop) ;

	resetController.GetClocks(sysclk, hclk, pclk_1, pclk_2);

	// Check to see that the clocks are correctly configured
	REQUIRE(sysclk == SYSCLOCK_Frequency);
	REQUIRE(hclk   == HCLOCK_Frequency);
	REQUIRE(pclk_1 == PCLOCK_1_Frequency);
	REQUIRE(pclk_2 == PCLOCK_2_Frequency);
}

void SetupPins(void)
{
	RCC&    resetController(*new RCC);

	// Don't forget to turn the clocks on, dumb shit....
	resetController.EnableClocks( GPIOA_EN     | GPIOB_EN      | GPIOC_EN     | GPIOD_EN      |
			                      GPIOE_EN     | GPIOF_EN      | GPIOG_EN     | GPIOH_EN      |
			                      GPIOI_EN,  ASSERT);
	Out<PF_6> foo;
	foo = 1;
}

void SystemInit(void)
{
	SetupClocks();
	SetupPins();

	Ticker		theSystickTimer;
	SCB& 		scb(*new SCB);

	// set Priority for Cortex-M4 System Interrupts
	scb.SetSystemHandlerPriority (SCB::SysTick_IRQn, (1 << NVIC_PRIO_BITS) - 1);
	theSystickTimer.Configure(9999);
}