#include "board.h"

void PeriClk0MutEnable(uint32_t ClkBitMsk)
{
    rt_base_t level;

    level = rt_hw_interrupt_disable();
    M0P_SYSCTRL->PERI_CLKEN0 |= ClkBitMsk;
    rt_hw_interrupt_enable(level);
}

uint32_t M0P_SYSCTRL_PERI_CLKEN1_BACKUP = 0;
void PeriClk1MutEnable(uint32_t ClkBitMsk)
{
    rt_base_t level;

    level = rt_hw_interrupt_disable();
    M0P_SYSCTRL_PERI_CLKEN1_BACKUP |= ClkBitMsk;
    M0P_SYSCTRL->PERI_CLKEN1 = M0P_SYSCTRL_PERI_CLKEN1_BACKUP;
    rt_hw_interrupt_enable(level);
}

void PeriClk0MutDisable(uint32_t ClkBitMsk_n)
{
    rt_base_t level;

    level = rt_hw_interrupt_disable();
    M0P_SYSCTRL->PERI_CLKEN0 &= ClkBitMsk_n;
    rt_hw_interrupt_enable(level);
}

void PeriClk1MutDisable(uint32_t ClkBitMsk_n)
{
    rt_base_t level;

    level = rt_hw_interrupt_disable();
    M0P_SYSCTRL_PERI_CLKEN1_BACKUP &= ClkBitMsk_n;
    M0P_SYSCTRL->PERI_CLKEN1 = M0P_SYSCTRL_PERI_CLKEN1_BACKUP;
    rt_hw_interrupt_enable(level);
}

void MutResetPeri(peri_reset_t PeriRst)
{
    rt_base_t level;
    uint32_t PeriRstMsk;
    
    PeriRstMsk = ~(0x1u<<(PeriRst&0x1Fu));
    level = rt_hw_interrupt_disable();
    if (PeriRst<32){
        M0P_RESET->PERI_RESET0 = 0x7F7F6FFF & PeriRstMsk;
        M0P_RESET->PERI_RESET0 = 0x7F7F6FFF;
    } else {
        M0P_RESET->PERI_RESET1 = 0x00000318 & PeriRstMsk;
        M0P_RESET->PERI_RESET1 = 0x00000318;
    }
    rt_hw_interrupt_enable(level);
}

#define CLKTRIM_RUN_TIME    40

void clktrim_init(void)
{
    PeriClk_MutEnable(PeriClk_Trim);
    M0P_CLK_TRIM->CR = (0<<8) |     // calclk_sel[2], 
                       (0<<7) |     // IE, interrupt 0:off; 1:on;
                       (0<<6) |     // mon_en, monitor 0:off; 1:on;
//                       (0<<4) |     // calclk_sel[1:0], 0:RCH, 1:XTH; 2:RCL; 3:XTL; 4:PLL;
                       (1<<4) |     // calclk_sel[1:0], 0:RCH, 1:XTH; 2:RCL; 3:XTL; 4:PLL;
                       (3<<1) |     // refclk_sel[1:0], 0:RCH, 1:XTH; 2:RCL; 3:XTL;
                       (0<<0);      // trim_start, 0:stop; 1:start;

    M0P_CLK_TRIM->REFCON = 32768*CLKTRIM_RUN_TIME;

    M0P_CLK_TRIM->CR_f.TRIM_START = 1;
}

clkoffset_t clktrim_check(void)
{
    clkoffset_t ofst;

    if (M0P_CLK_TRIM->IFR_f.STOP){
        ofst.valid = 1;
//        ofst.ppm = (1000000.0f/(4000000*CLKTRIM_RUN_TIME))*((float)M0P_CLK_TRIM->CALCNT - 4000000*CLKTRIM_RUN_TIME);
        ofst.ppm = (1000000.0f/(7372800*CLKTRIM_RUN_TIME))*((float)M0P_CLK_TRIM->CALCNT - 7372800*CLKTRIM_RUN_TIME);
        M0P_CLK_TRIM->CR_f.TRIM_START = 0;
        M0P_CLK_TRIM->CR_f.TRIM_START = 1;
    } else {
        ofst.valid = 0;
    }

    return(ofst);
}