#include "omw_clkcal.h"

// omw_clkcal_delayus
void omw_clkcal_delayus(int d){
    unsigned int ts, td, tl;
    tl = d/32; // 32us
    if(tl < 2)
        tl = 2;
    // ts, td
    ts = SYS_CTRL->AON_RTC;
    td = ts + tl;
    // delay
    while(1) {
        tl = SYS_CTRL->AON_RTC;
        if(tl >= td)
            break;
    }
}

// omw_clkcal_delayms
void omw_clkcal_delayms(int d){
    unsigned int ts, td, tl;
    tl = d*32; // 32us
    if(tl < 2)
        tl = 2;
    // ts, td
    ts = SYS_CTRL->AON_RTC;
    td = ts + tl;
    // delay
    while(1) {
        tl = SYS_CTRL->AON_RTC;
        if(tl >= td)
            break;
    }
}

// omw_clkcal_reset
void omw_clkcal_reset(void){
    int t;
    // reset = 1
    t = SYS_CTRL->CLKCAL_CTRL;
    t = t | (1<<1);
    SYS_CTRL->CLKCAL_CTRL = t;
    // delay 100us
    omw_clkcal_delayus(100);
    // reset = 0
    t = SYS_CTRL->CLKCAL_CTRL;
    t = t & (~(1<<1));
    SYS_CTRL->CLKCAL_CTRL = t;
    // delay 100us
    omw_clkcal_delayus(100);
}

// omw_clkcal_init
void omw_clkcal_init(int refclk, int calclk, int cyc, int ie) {
    int t;
    // *** set ie
    t = SYS_CTRL->CLKCAL_CTRL;
    t = t & (~1); // en = 0
    t = t | (ie << 2);
    SYS_CTRL->CLKCAL_CTRL = t;
    // *** set clkcal
    // clk1: calclk, set window; clk0: refclk, count
    if((refclk == CLKCAL_CLK_XT24M) || (refclk == CLKCAL_CLK_RC24M))
        t = ((1<<calclk) << 8) | (1<<refclk);
    // clk1: refclk, set window; clk0: calclk, count
    else
        t = ((1<<refclk) << 8) | (1<<calclk);
    // set reg
    SYS_CTRL->CLKCAL_CLKSEL = t;
    SYS_CTRL->CLKCAL_CNT0 = cyc;
}

// omw_clkcal_deinit
void omw_clkcal_deinit(void) {
    // reset reg
    SYS_CTRL->CLKCAL_CTRL = 0;
    SYS_CTRL->CLKCAL_CLKSEL = 0;
    // reset
    omw_clkcal_reset();
}
// omw_clkcal_enable
void omw_clkcal_enable(int d){
    int t;
    // *** set ie
    t = SYS_CTRL->CLKCAL_CTRL;
    if (d == 0) t &= (~0x01);
    else        t |= ( 0x01);
    SYS_CTRL->CLKCAL_CTRL = t;
}

// omw_clkcal_calpol
int omw_clkcal_calpol(int refclk, int calclk, int cyc) {
    int t;
    // init, ie=0
    omw_clkcal_init(refclk, calclk, cyc, 0);
    omw_clkcal_reset();
    omw_clkcal_enable(1);
    // setp 4: wait
    while(1){
        t = SYS_CTRL->CLKCAL_CTRL;
        t = t & (1<<4);
        if(t) break;
    }
    // return
    t = SYS_CTRL->CLKCAL_CNT1;
    return t;
}

// omw_clkcal_calint
void omw_clkcal_calint(int refclk, int calclk, int cyc) {
    // init, ie=0
    omw_clkcal_init(refclk, calclk, cyc, 1);
    omw_clkcal_reset();
    omw_clkcal_enable(1);
}

// 使用二分法查找最佳频率校准值
void omw_clkcal_rc24m_calibration(void){
    int t, cyc, tgt, meas, diff, diff_min, diff_max, cc, cc_min, cc_max;
    int rco24m_cc, rco24m_cc_xo_su;
    // init
    t = AON_CTRL->RCO24M_CFG;
    cc = (t>>8) & 0xff;
    cc_min = 0;
    cc_max = 255;
    diff_min = 0x0fffffff;
    diff_max = 0x0fffffff;
    cyc = CLKCAL_RC24M_CAL_MS*CLKCAL_RC24M_TGT_FREQ/1000; // counter value under RC24M clk
    tgt = CLKCAL_RC24M_CAL_MS*CLKCAL_XTAL_FREQ/1000; // counter value under XTAL clk
    //---------------------------------------
    // CALIBRATION
    //---------------------------------------
    while(1){
        // measure
        meas = omw_clkcal_calpol(CLKCAL_CLK_XT24M, CLKCAL_CLK_RC24M, cyc);
        diff = meas - tgt;
        // modify
        if(diff > 0){ // dec
            diff_max = diff;
            cc_max = cc;
            cc = cc - (cc - cc_min)/2;
        } else { // inc
            diff_min = diff;
            cc_min = cc;
            cc = cc + (cc_max - cc_min)/2;
        }
        if((cc == cc_min) || (cc == cc_max))
            break;
        // update
        t = AON_CTRL->RCO24M_CFG;
        t &= (~(0xff<<8));
        t |= (cc<<8);
        AON_CTRL->RCO24M_CFG = t;
    }
    // finally check
    if(abs(diff_min) > abs(diff_max)) cc = cc_max;
    else cc = cc_min;
    //---------------------------------------
    // WRITE AON REG
    //---------------------------------------
    // rco24m_cc
    rco24m_cc = cc;
    // rco24m_cc_xo_su, (170 + rco24m_cc - 0.94*170/1.04)/(0.94/1.04)
    // rco24m_cc_xo_su = (int)((176.8 + ((float)cc)*1.04)/RCO_FREQ_DEV_XO_SU - 170);
    rco24m_cc_xo_su = (17680 + cc*104)/RCO_FREQ_DEV_XO_SU_INT - 170;
    // range check
    if(rco24m_cc < 0) rco24m_cc = 0;
    if(rco24m_cc > 255) rco24m_cc = 255;
    if(rco24m_cc_xo_su < 0) rco24m_cc_xo_su = 0;
    if(rco24m_cc_xo_su > 255) rco24m_cc_xo_su = 255;
    t = AON_CTRL->RCO24M_CFG;
    t &= (~(0xffff<<8));
    t |= ((rco24m_cc<<8) | (rco24m_cc_xo_su<<16));
    AON_CTRL->RCO24M_CFG = t;
    //---------------------------------------
    // CLKCAL DE-INIT
    //---------------------------------------
    omw_clkcal_enable(0);
    omw_clkcal_reset();
}

void omw_clkcal_rc32k_calibration(void){
    int t, cyc, tgt, meas, diff, diff_min, diff_max; // meas may overflow when 32-bit int ???
    int cc, cc_min, cc_max;
    // init
    t = AON_CTRL->RCO32K_CFG;
    cc = (t>>7) & 0xff;
    cc_min = 0;
    cc_max = 255;
    diff_min = 0x0fffffff;
    diff_max = 0x0fffffff;
    cyc = CLKCAL_RC32K_CAL_MS*CLKCAL_RC32K_TGT_FREQ/1000; // counter value under RC32K clk
    tgt = CLKCAL_RC32K_CAL_MS*CLKCAL_XTAL_FREQ/1000; // counter value under XTAL clk
    // main
    while(1){
        // meas
        meas = omw_clkcal_calpol(CLKCAL_CLK_XT24M, CLKCAL_CLK_RC32K, cyc);
        diff = meas - tgt;
        // modify
        if(diff > 0){ // dec
            diff_max = diff;
            cc_max = cc;
            cc = cc - (cc - cc_min)/2;
        } else { // inc
            diff_min = diff;
            cc_min = cc;
            cc = cc + (cc_max - cc_min)/2;
        }
        if((cc == cc_min) || (cc == cc_max))
            break;
        // update
        t = AON_CTRL->RCO32K_CFG;
        t &= (~(0xff<<7));
        t |= (cc<<7);
        AON_CTRL->RCO32K_CFG = t;
    }
    if(abs(diff_min) > abs(diff_max)) cc = cc_max;
    else cc = cc_min;
    t = AON_CTRL->RCO32K_CFG;
    t &= (~(0xff<<7));
    t |= (cc<<7);
    AON_CTRL->RCO32K_CFG = t;
    //---------------------------------------
    // CLKCAL DE-INIT
    //---------------------------------------
    omw_clkcal_enable(0);
    omw_clkcal_reset();
}

