#include "clocks.h"
#include "registers.h"
#include <stm32h747xx.h>

void clocks_pll_configure(clock_pll_t pll, uint8_t divm, pll_source_t source,
                          uint16_t divn, uint8_t divp, uint8_t divq,
                          uint8_t divr) {
  clocks_pll_disable(pll);

  reg_write_bits_pos(&RCC->PLLCKSELR, divm, RCC_PLLCKSELR_DIVM1_Pos + ((pll << 1) << 3), 0x3F);
  reg_write_bits_pos(&RCC->PLLCKSELR, source, RCC_PLLCKSELR_PLLSRC_Pos, 0x3);

  reg_write_bits(&RCC->PLL1DIVR + (pll << 1),
                 (((divr - 1) & 0x7F) << RCC_PLL1DIVR_R1_Pos) |
                 (((divq - 1) & 0x7F) << RCC_PLL1DIVR_Q1_Pos) |
                 (((divp - 1) & 0x7F) << RCC_PLL1DIVR_P1_Pos) |
                 (((divn - 1) & 0x1FF) << RCC_PLL1DIVR_N1_Pos),
                 0x7F7FFFFF);
}
void clocks_pll_enable(clock_pll_t pll) {
  reg_set_bits(&RCC->CR, RCC_CR_PLL1ON << (uint8_t)(pll << 1));
}
void clocks_pll_disable(clock_pll_t pll) {
  reg_clear_bits(&RCC->CR, RCC_CR_PLL1ON << ((uint8_t)(pll << 1)));
}
void clocks_pll_wait_ready(clock_pll_t pll, uint16_t timeout_us) {
  while (reg_read_bits_pos(&RCC->CR, RCC_CR_PLL1ON_Pos + (uint8_t)(pll << 1) + 1, 1) == 0);
}

void clocks_system_clock_source(sysclock_source_t source, uint8_t d1cpre, uint8_t d1ppre, uint8_t hpre, uint16_t timeout_us) {
  reg_write_bits(&RCC->D1CFGR,
                 ((d1cpre & 0xF) << RCC_D1CFGR_D1CPRE_Pos) |
                 ((d1ppre & 0x7) << RCC_D1CFGR_D1PPRE_Pos) |
                 ((hpre & 0xF) << RCC_D1CFGR_HPRE_Pos),
                 RCC_D1CFGR_D1CPRE | RCC_D1CFGR_D1PPRE | RCC_D1CFGR_HPRE);
  reg_write_bits(&RCC->CFGR, source, RCC_CFGR_SW);
  while (reg_read_bits_pos(&RCC->CFGR, RCC_CFGR_SWS_Pos, 0x7) != source);
}