#include "pll.h"
#include "mcom02.h"

/********MCOM-02 REGMAP DEFINE*******************************************/
/***************************System Registers*****************************/
#define CMCTR_BASE              0x38094000
#define GAT_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 GAT_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

#define GPIOA29_I2C0_SDA				(1 << 29)
#define GPIOA29_I2C0_SCL				(1 << 30)
#define GPIOD22_I2C1_SDA				(1 << 22)
#define GPIOD23_I2C1_SCL				(1 << 23)
#define GPIOD24_I2C2_SDA				(1 << 24)
#define GPIOD25_I2C2_SCL				(1 << 25)

/***FAN53555**************************************************************/
/***5 A, 2.4MHz, Digitally Programmable TinyBuck Regulator****************/

#define FAN53555_I2C_NUM                                0
#define FAN53555_I2C_SLAVE_ADDR                         0x60
#define FAN53555_VSEL0_REG                              0
#define FAN53555_OPTION4_VOLT_TO_SEL(mv)                (((mv) - 603)/12.826f + 1)

/***CPU FREQ TABLE********************************************************/
#define CPU_FREQ_VOLTAGE_SIZE                           3
const unsigned int CPU_FREQ_VOLTAGE[CPU_FREQ_VOLTAGE_SIZE][2] __attribute__ ((aligned(8))) = {
//   MHz,   mV
    {816, 1040},
    {912, 1103},
    {1008, 1205}
};

/***DSP FREQ TABLE********************************************************/
#define DSP_FREQ_VOLTAGE_SIZE                           3
const unsigned int DSP_FREQ_VOLTAGE[CPU_FREQ_VOLTAGE_SIZE][2] __attribute__ ((aligned(8))) = {
//   MHz,   mV
    {696, 1040},
    {768, 1103},
    {888, 1205}
};

/*I2C Settings*/
#define BASE_I2C0                                       0x3802c000
#define BASE_I2C1                                       0x3802d000
#define BASE_I2C2                                       0x3802e000

#define I2C_CMD_READ                                    (1 << 8)
#define I2C_CMD_WRITE                                   (0 << 8)
#define I2C_STATUS_TFE                                  (1 << 2)
#define I2C_STATUS_RFNE                                 (1 << 3)

const unsigned int I2C_GPIO[3][3] __attribute__ ((aligned(8))) = {
    { SWPORTA_CTL, GPIOA29_I2C0_SDA, GPIOA29_I2C0_SCL},
    { SWPORTD_CTL, GPIOD22_I2C1_SDA, GPIOD23_I2C1_SCL},
    { SWPORTD_CTL, GPIOD24_I2C2_SDA, GPIOD25_I2C2_SCL},
};

typedef struct {
    volatile unsigned int IC_CON;
    volatile unsigned int IC_TAR;
    volatile unsigned int IC_SAR;
    volatile unsigned int IC_HS_MADDR;
    volatile unsigned int IC_DATA_CMD;
    volatile unsigned int IC_SS_SCL_HCNT;
    volatile unsigned int IC_SS_SCL_LCNT;
    volatile unsigned int IC_FS_SCL_HCNT;
    volatile unsigned int IC_FS_SCL_LCNT;
    volatile unsigned int IC_HS_SCL_HCNT;
    volatile unsigned int IC_HS_SCL_LCNT;
    volatile unsigned int IC_INTR_STAT;
    volatile unsigned int IC_INTR_MASK;
    volatile unsigned int IC_RAW_INTR_STAT;
    volatile unsigned int IC_RX_TL;
    volatile unsigned int IC_TX_TL;
    volatile unsigned int IC_CLR_INTR;
    volatile unsigned int IC_CLR_RX_UNDER;
    volatile unsigned int IC_CLR_RX_OVER;
    volatile unsigned int IC_CLR_TX_OVER;
    volatile unsigned int IC_CLR_RD_REQ;
    volatile unsigned int IC_CLR_TX_ABRT;
    volatile unsigned int IC_CLR_RX_DONE;
    volatile unsigned int IC_CLR_ACTIVITY;
    volatile unsigned int IC_CLR_STOP_DET;
    volatile unsigned int IC_CLR_START_DET;
    volatile unsigned int IC_CLR_GEN_CALL;
    volatile unsigned int IC_ENABLE;
    volatile unsigned int IC_STATUS;
    volatile unsigned int IC_TXFLR;
    volatile unsigned int IC_RXFLR;
    volatile unsigned int IC_SDA_HOLD;
    volatile unsigned int IC_TX_ABRT_SOURCE;
    volatile unsigned int IC_SLV_DATA_NACK_ONLY;
    volatile unsigned int IC_DMA_CR;
    volatile unsigned int IC_DMA_TDLR;
    volatile unsigned int IC_DMA_RDLR;
    volatile unsigned int IC_SDA_SETUP;
    volatile unsigned int IC_ACK_GENERAL_CALL;
    volatile unsigned int IC_ENABLE_STATUS;
    volatile unsigned int IC_FS_SPKLEN;
    volatile unsigned int IC_HS_SPKLEN;
    volatile unsigned char RESERVED0[76];
    volatile unsigned int IC_COMP_PARAM_1;
    volatile unsigned int IC_COMP_VERSION;
    volatile unsigned int IC_COMP_TYPE;
} i2c_reg_t;

typedef union
{
    struct
    {
        volatile unsigned int NSEL0 : 6;
        volatile unsigned int MODE0 : 1;
        volatile unsigned int BUCK_EN0 : 1;
        volatile unsigned int reserved : 24;
    };
    volatile unsigned int regval;
}vsel0_reg_t;

enum dw_i2c_speed_mode {
    I2C_SPEED_STANDART  = 0x1,          // standard speed (100 kbps)
    I2C_SPEED_FAST      = 0x2,          // fast speed (400 kbps)
    I2C_SPEED_HIGH      = 0x3           // high speed (3400 kbps)
};
enum dw_i2c_address_mode {
    I2C_7BIT_ADDRESS    = 0x0,          // 7-bit address mode.  Only the 7 LSBs
                                        // of the slave and/or target address
                                        // are relevant.
    I2C_10BIT_ADDRESS   = 0x1           // 10-bit address mode.  The 10 LSBs of
                                        // the slave and/or target address are
                                        // relevant.
};
enum dw_i2c_tx_mode {
    I2C_TX_TARGET       = 0x0,          // normal transfer using target address
    I2C_TX_GEN_CALL     = 0x1,          // issue a general call
    I2C_TX_START_BYTE   = 0x3           // issue a start byte I2C command
};

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

i2c_reg_t* getI2CHandler(unsigned int i2cNum)
{
    i2cNum &= 0x3;
    return (i2c_reg_t*) (BASE_I2C0 + 0x1000 * i2cNum);
}
void initI2CTx(unsigned int i2cNum, unsigned int slave) {
    volatile unsigned int temp;
    i2c_reg_t* i2c;
    i2cNum &= 0x3;

    //Enable clock
    GATE_SYS_CTR |= CLK_I2C0_EN << i2cNum;
    //Enable GPIO
    GPIO0(I2C_GPIO[i2cNum][0]) |= I2C_GPIO[i2cNum][1] | I2C_GPIO[i2cNum][2];

    i2c = getI2CHandler(i2cNum);

    i2c->IC_ENABLE = 0;                     //Disable i2c;
    i2c->IC_INTR_MASK = 0;                  //Mask interrupts
    temp = i2c->IC_CLR_INTR;                //Clear interrupts
    (void) temp;
    i2c->IC_CON = 1                         //Enable master mode
            | (I2C_SPEED_STANDART << 1)     //Setup speed
            | (I2C_7BIT_ADDRESS << 3)       //Set address bit-mode slave
            | (1 << 5)                      //Restart enable
            | (1 << 6);                     //Disable slave mode
    i2c->IC_TAR = (slave & 0x3FF)           //Set slave address
            | (I2C_TX_TARGET << 10)         //Set TX mode
            | (I2C_7BIT_ADDRESS << 12);     //Set address bit-mode master
    //i2c->IC_INTR_MASK = 0xFFF;              //Unmask interrupts
    i2c->IC_ENABLE = 1;                     //Enable i2c;
}
unsigned int readI2CReg(unsigned int i2cNum, unsigned int regNum)
{
    i2cNum &= 0x3;
    regNum &= 0xFF;
    i2c_reg_t* i2c = getI2CHandler(i2cNum);

    //Send register number
    i2c->IC_DATA_CMD = regNum | I2C_CMD_WRITE;
    //Wait sending
    while(!(i2c->IC_STATUS & I2C_STATUS_TFE));

    //Read answer
    i2c->IC_DATA_CMD = I2C_CMD_READ;
    //Wait data
    while(!(i2c->IC_STATUS & I2C_STATUS_RFNE));

    return (i2c->IC_DATA_CMD & 0xFF);
}

unsigned int get_l3comm_freq()
{
       return (((SET_SPLL&0xff) + 1)*XTI_FREQ >> (DIV_SYS0_CTR&0x1)) >> (DIV_SYS1_CTR&0x1);
}
void writeI2CReg(unsigned int i2cNum, unsigned int regNum, unsigned int regVal)
{
    i2cNum &= 0x3;
    regNum &= 0xFF;
    regVal &= 0xFF;
    i2c_reg_t* i2c = getI2CHandler(i2cNum);

    //Send register number
    i2c->IC_DATA_CMD = regNum | I2C_CMD_WRITE;
    //Send register value
    i2c->IC_DATA_CMD = regVal | I2C_CMD_WRITE;
    //Wait sending
    while(!(i2c->IC_STATUS & I2C_STATUS_TFE));
}
void i2c_delay()
{
    volatile int i;
    for(i = 0; i < 10000; i++)
    {
        asm volatile("nop");
    }
}
int setCPUFreq(unsigned int MHz)
{
    unsigned int dsp_voltage, last_freq, voltage;

    if(MHz < MIN_CPU_FREQ_MHZ || MHz > MAX_CPU_FREQ_MHZ
            || (voltage = getCPUVoltage(MHz)) == 0)
    {
        return 1;
    }
    //Check DSP freq and voltage
    dsp_voltage = getDSPVoltage(getCurrentDSPFreq());

    if(dsp_voltage > voltage)
        voltage = dsp_voltage;  //Set max voltage

    //Save current CPU freq
    last_freq = getCurrentCPUFreq();
    //Set default CPU freq
    setCPUPLLFreq(DEFAULT_XTI_CLOCK);

    //Set Core Voltage
    if(setCoreVoltage(voltage))
    {
        //Restore CPU freq
        setCPUPLLFreq(last_freq);
        return 2;
    };
    //Set CPU freq
    setCPUPLLFreq(MHz);

    return 0;
}
int setDSPFreq(unsigned int MHz)
{
    unsigned int cpu_voltage, last_freq, voltage;

    if(MHz < MIN_DSP_FREQ_MHZ || MHz > MAX_DSP_FREQ_MHZ
            || (voltage = getDSPVoltage(MHz)) == 0)
    {
        return 1;
    }
    //Enable DSP_CLK
    GATE_DSP_CTR |=
              DSP0_EN
            | DSP1_EN
            | DSPEXT_EN
            | DSPENC_EN
            ;
    //Check CPU freq and voltage
    cpu_voltage = getCPUVoltage(getCurrentCPUFreq());

    if(cpu_voltage > voltage)
        voltage = cpu_voltage;  //Set max voltage

    //Save current DSP freq
    last_freq = getCurrentDSPFreq();
    //Set default DSP freq
    setDSPPLLFreq(DEFAULT_XTI_CLOCK);

    //Set Core Voltage
    if(setCoreVoltage(voltage))
    {
        //Restore DSP freq
        setDSPPLLFreq(last_freq);
        return 2;
    };
    //Set DSP freq
    setDSPPLLFreq(MHz);

    return 0;
}
int setCoreVoltage(unsigned int voltage)
{
    vsel0_reg_t sel0_reg;

    sel0_reg.regval = 0;
    sel0_reg.NSEL0 = FAN53555_OPTION4_VOLT_TO_SEL(voltage); //Set voltage
    sel0_reg.MODE0 = 0;                                     //Allow Auto-PFM mode
    sel0_reg.BUCK_EN0 = 1;                                  //Software buck enable

    initI2CTx(FAN53555_I2C_NUM, FAN53555_I2C_SLAVE_ADDR);
    writeI2CReg(FAN53555_I2C_NUM, FAN53555_VSEL0_REG, sel0_reg.regval);
    i2c_delay();
    if(readI2CReg(FAN53555_I2C_NUM, FAN53555_VSEL0_REG) != sel0_reg.regval)
    {
        return 1;
    }
    return 0;
}
unsigned int getCPUVoltage(unsigned int MHz)
{
    int i;
    for(i = 0; i < CPU_FREQ_VOLTAGE_SIZE; i++)
    {
        if(MHz <= CPU_FREQ_VOLTAGE[i][0])
        {
            return CPU_FREQ_VOLTAGE[i][1];
        }
    }
    return 0;
}
unsigned int getDSPVoltage(unsigned int MHz)
{
    int i;
    for(i = 0; i < DSP_FREQ_VOLTAGE_SIZE; i++)
    {
        if(MHz <= DSP_FREQ_VOLTAGE[i][0])
        {
            return DSP_FREQ_VOLTAGE[i][1];
        }
    }
    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));
}