#include "system.h"
void system_clk_div(uint8_t clk_div)
{
    SYSCON &= 0x3F;
    SYSCON |= (clk_div << 6);
}

void clk_enable(uint8_t clk_source)
{
    if(clk_source == SUBCLK)
        SYSCON |= 0x02;
    else if(clk_source == IHRC)
        SYSCON |= 0x01;
    else
        while(1);
}

void clk_disable(uint8_t clk_source)
{
    if(clk_source == SUBCLK)
        SYSCON &= 0xFD;
    else if(clk_source == IHRC)
        SYSCON &= 0xFE;
    else
        while(1);
}

void main_clk_sel(uint8_t clk_source)
{
    if(clk_source == SUBCLK)
        CLKCFG |= 0x80;
    else if(clk_source == IHRC)
        SYSCON &= 0x7F;
    else
        while(1);    
}

void sub_clk_sel(uint8_t subclk_source)
{
    if(subclk_source == XTAL)
        CLKCFG |= 0x40;
    else if(subclk_source == ILRC)
        SYSCON &= 0xBF;
    else
        while(1);     
}

void cpu_idle(void)
{
    PCON |= 0x01;
}

void cpu_stop(void)
{
    NOP;
    NOP;
    NOP;
    NOP;    
    PCON |= 0x02;
    NOP;
    NOP;
    NOP;
    NOP;
}

void clko_output(uint8_t clko_sel)
{
    P2CR |= 0x08;//P23 OUTPUT
    ALFSEL1 |= 0x04;//P23 is buz or clko
    REMAP = 0x02;//P23 choose clko
    CLKCFG &= 0xF8;
    CLKCFG |= clko_sel;
}