#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

#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);
}
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))
        ;
}