#include "project.h"

CONST u8 HSIDivFactor[4] = { 1, 2, 4, 8 }; /*!< Holds the different HSI Divider factors */
CONST u8 CLKPrescTable[8] = { 1, 2, 4, 8, 10, 16, 20, 40 }; /*!< Holds the different CLK prescaler values */

u32
CLK_GetClockFreq (void)
{
  u32 clockfrequency = 0;
  CLK_Source_TypeDef clocksource = CLK_SOURCE_HSI;
  u8 tmp = 0, presc = 0;

  /* Get CLK source. */
  clocksource = (CLK_Source_TypeDef) CLK->CMSR;

  if (clocksource == CLK_SOURCE_HSI) {
    tmp = (u8) (CLK->CKDIVR & CLK_CKDIVR_HSIDIV);
    tmp = (u8) (tmp >> 3);
    presc = HSIDivFactor[tmp];
    clockfrequency = HSI_VALUE / presc;
  } else if (clocksource == CLK_SOURCE_LSI)
    clockfrequency = LSI_VALUE;
  else
    clockfrequency = HSE_VALUE;

  return ((u32) clockfrequency);
}


void
clock_init (void)
{
  /* Clear High speed internal clock prescaler */
  CLK->CKDIVR &= (u8) (~CLK_CKDIVR_HSIDIV);
  /* Set High speed internal clock prescaler */
  CLK->CKDIVR |= (u8) CLK_PRESCALER_HSIDIV1;
}

void
CLK_PeripheralClockConfig (CLK_Peripheral_TypeDef CLK_Peripheral,
                           FunctionalState NewState)
{
  if (((u8) CLK_Peripheral & (u8) 0x10) == 0x00) {
    if (NewState != DISABLE) {
      CLK->PCKENR1 |= (u8) ((u8) 1 << ((u8) CLK_Peripheral & (u8) 0x0F)); /* Enable the peripheral Clock */
    } else {
      CLK->PCKENR1 &= (u8) (~ (u8) (((u8) 1 << ((u8) CLK_Peripheral & (u8) 0x0F)))); /* Disable the peripheral Clock */
    }
  } else {
    if (NewState != DISABLE) {
      CLK->PCKENR2 |= (u8) ((u8) 1 << ((u8) CLK_Peripheral & (u8) 0x0F)); /* Enable the peripheral Clock */
    } else {
      CLK->PCKENR2 &= (u8) (~ (u8) (((u8) 1 << ((u8) CLK_Peripheral & (u8) 0x0F)))); /* Disable the peripheral Clock */
    }
  }
}