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

void clocks_enable(clock_t clock) {
  uint32_t mask = 0;
  switch (clock) {
  case CLOCK_HSI:
    mask = RCC_CR_HSION_Msk;
    break;
  case CLOCK_HSE:
    mask = RCC_CR_HSEON_Msk;
    break;
  case CLOCK_HSI48:
    mask = RCC_CR_HSI48ON_Msk;
    break;
  }

  RCC->CR |= mask;
}

bool clocks_ready(clock_t clock) {
  uint32_t mask = 0;
  switch (clock) {
  case CLOCK_HSI:
    mask = RCC_CR_HSIRDY_Msk;
    break;
  case CLOCK_HSE:
    mask = RCC_CR_HSERDY_Msk;
    break;
  case CLOCK_HSI48:
    mask = RCC_CR_HSI48RDY_Msk;
    break;
  }

  return (RCC->CR & mask) != 0;
}

void clocks_wait_ready(clock_t clock) {
  while (!clocks_ready(clock));
}

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


  bool enable_p = divp > 0;
  bool enable_q = divq > 0;
  bool enable_r = divr > 0;

  reg_write_bits_pos(&RCC->PLLCFGR,
                     (enable_p << 0) | (enable_q << 1) | (enable_r << 2),
                     3*pll + RCC_PLLCFGR_DIVP1EN_Pos, 0x7);

  reg_write_bits_pos(&RCC->PLLCFGR, vcosel, (4*pll + RCC_PLLCFGR_PLL1VCOSEL_Pos), 1);


  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_Msk | RCC_D1CFGR_D1PPRE_Msk | RCC_D1CFGR_HPRE_Msk);
  reg_write_bits(&RCC->CFGR, source, RCC_CFGR_SW);
  while (reg_read_bits_pos(&RCC->CFGR, RCC_CFGR_SWS_Pos, 0x7) != source);
}