#include "pll.h"

/********MCOM-02 REGMAP DEFINE*******************************************/
/***************************System Registers*****************************/
#define CMCTR_BASE 0x38094000
#define GATE_SYS_CTR (*(volatile unsigned int *)(CMCTR_BASE + 0x04c))
#define CLK_I2C2_EN (1 << 18)
#define CLK_I2C1_EN (1 << 17)
#define CLK_I2C0_EN (1 << 16)
#define GATE_DSP_CTR (*(volatile unsigned int *)(CMCTR_BASE + 0x068))
#define DSPENC_EN (1 << 3)
#define DSPEXT_EN (1 << 2)
#define DSP1_EN (1 << 1)
#define DSP0_EN (1 << 0)
#define SEL_APLL (*(volatile unsigned int *)(CMCTR_BASE + 0x100))
#define SEL_CPLL (*(volatile unsigned int *)(CMCTR_BASE + 0x104))
#define SEL_DPLL (*(volatile unsigned int *)(CMCTR_BASE + 0x108))
#define SEL_SPLL (*(volatile unsigned int *)(CMCTR_BASE + 0x10c))
#define SEL_VPLL (*(volatile unsigned int *)(CMCTR_BASE + 0x110))
#define PLL_LOCK_BIT (1 << 31)

/***************************GPIO******************************************/
#define GPIO0_BASE 0x38034000
#define GPIO0(a) (*(volatile unsigned int *)(GPIO0_BASE + (a)))
#define SWPORTA_DR 0x00
#define SWPORTA_DDR 0x04
#define SWPORTA_CTL 0x08
#define SWPORTB_DR 0x0c
#define SWPORTB_DDR 0x10
#define SWPORTB_CTL 0x14
#define SWPORTC_DR 0x18
#define SWPORTC_DDR 0x1c
#define SWPORTC_CTL 0x20
#define SWPORTD_DR 0x24
#define SWPORTD_DDR 0x28
#define SWPORTD_CTL 0x2c

void setCPUPLLFreq(unsigned int MHz);
void setDSPPLLFreq(unsigned int MHz);

int setCPUFreq(unsigned int MHz)
{
    if (MHz < MIN_CPU_FREQ_MHZ || MHz > MAX_CPU_FREQ_MHZ ) {
        return 1;
    }

    // Set CPU freq
    setCPUPLLFreq(MHz);

    return 0;
}
int setDSPFreq(unsigned int MHz)
{
    if (MHz < MIN_DSP_FREQ_MHZ || MHz > MAX_DSP_FREQ_MHZ) {
        return 1;
    }
    // Enable DSP_CLK
    GATE_DSP_CTR |= DSP0_EN | DSP1_EN | DSPEXT_EN | DSPENC_EN;
    // Set default DSP freq
    setDSPPLLFreq(DEFAULT_XTI_CLOCK);

    // Set DSP freq
    setDSPPLLFreq(MHz);

    return 0;
}
unsigned int getCurrentCPUFreq()
{
    int sel = (SEL_APLL & 0xFF);
    if (sel > 0x3D)
        sel = 0x3D;
    return (sel + 1) * DEFAULT_XTI_CLOCK;
}
unsigned int getCurrentDSPFreq()
{
    int sel = (SEL_DPLL & 0xFF);
    if (sel > 0x3D)
        sel = 0x3D;
    return (sel + 1) * DEFAULT_XTI_CLOCK;
}
void setCPUPLLFreq(unsigned int MHz)
{
    int sel = (MHz / DEFAULT_XTI_CLOCK) - 1;
    if (sel < 0)
        sel = 0;
    SEL_APLL = sel;
    while (!(SEL_APLL & PLL_LOCK_BIT))
        ;
}
void setDSPPLLFreq(unsigned int MHz)
{
    int sel = (MHz / DEFAULT_XTI_CLOCK) - 1;
    if (sel < 0)
        sel = 0;
    SEL_DPLL = sel;
    while (!(SEL_DPLL & PLL_LOCK_BIT))
        ;
}