/*
 * This file sets the system clock to its full glory of 80Mhz
 */

#include "clock.h"
#include <stdint.h>
#include "flash.h"
#include "gpio.h"
#include "spin.h"

#define TIMEOUT 10000

int pll_off()
{
  uint32_t c;

  RCC.c.pllon = false;
  for (c = 0; c < TIMEOUT && RCC.c.pllrdy; ++c)
    ; /* Wait for OFF. */

  if (c == TIMEOUT) {
    return E_TIMEOUT;
  }

  return 0;
}

int pll_on()
{
  uint32_t c;

  RCC.c.pllon = true;
  for (c = 0; c < TIMEOUT && !RCC.c.pllrdy; ++c)
    ; /* Wait for RDY. */

  if (c == TIMEOUT) {
    return E_TIMEOUT;
  }

  return 0;
}

int configure_pll(
    uint8_t pllp_div_factor, pll_divisor_t pllr, /* System clock divisor. */
    pll_divisor_t pllq,  /* Divison factor for PLL48M1CLK. */
    pllp_divisor_t pllp, /* Divison factor for PLLSAI2CLK. */
    uint8_t plln,        /* PLL numerator. */
    pllm_divisor_t pllm, /* PLL denominator. */
    pll_src_t pllsrc /* PLL source */)
{
  if (RCC.c.pllrdy) {
    /* PLL must be off to configure it. */
    return E_NOT_OFF;
  }

  /* Make sure inputs are valid. */
  if (pllp_div_factor == 1 || pllp_div_factor > 31) {
    return E_BADPLLP_DIV;
  }
  if (plln < 8 || plln > 86) {
    return E_BADPLLN;
  }

  union RCC_PLLCFGR tmp;

  tmp.pllpdiv = pllp_div_factor;
  tmp.pllr = pllr >> 1;
  tmp.pllren = pllr & 1;
  tmp.pllp = pllp >> 1;
  tmp.pllpen = pllp & 1;
  tmp.pllq = pllq >> 1;
  tmp.pllqen = pllq & 1;
  tmp.plln = plln;
  tmp.pllm = pllm;

  tmp.pllsrc = pllsrc;

  RCC.pllcfg = tmp;

  return 0;
}

int set_system_clock_MHz(uint8_t mhz)
{
  /* Set the source of the system colck to MSI temporarily. */
  set_system_clock_src(SYSTEM_CLOCK_SRC_MSI);

  if (mhz <= 8 || mhz > 80) {
    return E_BAD_ARG;
  }

  pll_off();

  configure_pll(
      0,
      PLL_DIVISOR_4,
      PLL_DIVISOR_4,
      PLLP_DIVISOR_7,
      mhz,
      PLLM_DIVISOR_1,
      PLL_SRC_MSI);

  pll_on();

  /* Configure the flash to have 4 wait states. This is required at
   * 80 MHz. */
  FLASH.ac_r &= ~0x07;
  FLASH.ac_r |= 0x04;

  /* Set the source of the system colck to PLL. */
  set_system_clock_src(SYSTEM_CLOCK_SRC_PLL);
  return 0;
}

int set_system_clock_src(system_clock_src_t src)
{
  uint8_t value = RCC.cfg.r & ~0x03;
  RCC.cfg.r = value | src;
}

int enable_hsi(__IO rcc_t* rcc, bool enable)
{
  uint32_t c;
  rcc->c.hsion = !!enable;
  for(c = 0; c < TIMEOUT && !rcc->c.hsirdy; ++ c)
    ;
  if (c == TIMEOUT) {
    return E_TIMEOUT;
  }
  return 0;
}