/********************************************************************************
 * Marvell GPL License Option
 *
 * If you received this File from Marvell, you may opt to use, redistribute and/or
 * modify this File in accordance with the terms and conditions of the General
 * Public License Version 2, June 1991 (the "GPL License"), a copy of which is
 * available along with the File in the license.txt file or by writing to the Free
 * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 or
 * on the worldwide web at http://www.gnu.org/licenses/gpl.txt.
 *
 * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY
 * DISCLAIMED.  The GPL License provides additional details about this warranty
 * disclaimer.
 ******************************************************************************/

#if (BERLIN_CHIP_VERSION == BERLIN_BG2CDP)
#pragma GCC optimize ("O0")
#include <string.h>
#include "galois_io.h"
#include "pllDiag.h"
#include "global.h"
//#include "GaloisTime.h"
#ifdef DIAG_PRT
#include "lgpl_printf.h"
#endif

volatile SIE_avPll regSIE_avPll;
int vcoFreqIndex;
unsigned int avPllChRegOffset[8]=
{
	RA_avPll_C1,
	RA_avPll_C2,
	RA_avPll_C3,
	RA_avPll_C4,
	RA_avPll_C5,
	RA_avPll_C6,
	RA_avPll_C7,
	RA_avPll_C8
};

typedef struct
{
	int vco_freq_index;
	float clkout_freq; //in MHz
	int freq_offset; //signed number
	unsigned int p_sync2;
	unsigned int p_sync1;
	unsigned int post_div;
	unsigned int post_div_0p5;
	unsigned int invalid; //this indicate it is a valid setting or not: 0 is valid, 1 is not
} CLK_FREQ;

//for channel 1 to 7, index starts at 1, [0][0] and [1][0] are not used
float current_freq[2][8];

enum
{
	HDMI_8BIT_MODE=0,
	HDMI_10BIT_MODE,
	HDMI_12BIT_MODE
};

enum
{
	FRAME_RATE_59P94=0,
	FRAME_RATE_60
};

#define	OVERSAMPLE_HDMI			0
#define OVERSAMPLE_1X			1
#define OVERSAMPLE_2X			2
#define OVERSAMPLE_4X			4
#define OVERSAMPLE_8X			8

double diag_videoFreqs[] =
{
	//video frequencies, pixel freq
	25.17482517,
	27,
	54,
	74.17582418,
	108,
	148.3516484
};

//all the VCO freq reqired for video and audio in MHz
double diag_vcoFreqs[]=
{
	1512,	//8 bit HDMI and 12 bit HMDI
	1620,	//12 bit HDMI
	1856.25,//10 bit HDMI
	2160,	//8 bit HDMI
	2227.5,	//12 bit HDMI
	2520,	//10 bit HDMI
	2700,	//10 bit HDMI
	2970,   //8 bit HDMI
};

//from Section 7 table
unsigned char diag_avpllRegFBDIV[]=
{
	60,	//VCO_FREQ_1_512G,
	65, //VCO_FREQ_1_620G,
	74, //VCO_FREQ_1_856G,
	86, //VCO_FREQ_2_160G,
	89, //VCO_FREQ_2_227G,
	101, //VCO_FREQ_2_520G,
	108, //VCO_FREQ_2_700G,
	119, //VCO_FREQ_2_970G,
};

//from Section 7 table, bit18 is sign bit
//Note that sign bit=0 for negative
//sign bit=1 for positive
unsigned int diag_avpllRegFREQ_OFFSET_C8[]=
{
		 (33554),	//VCO_FREQ_1_512G,
((1<<18)|(12905)), 	//VCO_FREQ_1_620G,
		 (14169), 	//VCO_FREQ_1_856G,
		 (19508), 	//VCO_FREQ_2_160G,
		 (4712),	//VCO_FREQ_2_227G
((1<<18)|(8305)), 	//VCO_FREQ_2_520G,
		 (00000), 	//VCO_FREQ_2_700G,
((1<<18)|(7049)), 	//VCO_FREQ_2_970G,
};

unsigned int diag_avpllRegSPEED[]=
{
	0x2,	//1.5G<F<=1.7G , for 1.512G
	0x2,	//1.5G<F<=1.7G , for 1.62G
	0x3,	//1.7G<F<=1.9G , for 1.856G
//	0x4,	//1.9G<F<=2.1G
	0x5,	//2.1G<F<=2.3G , for 2.16G
	0x5,	//2.1G<F<=2.3G , for 2.227G
//	0x6,	//2.3G<F<=2.45G
	0x7,	//2.45G<F<=2.6G, for 2.52G
	0x8, 	//2.6G<F<= 2.75G, for 2.7G
//	0x9, 	//2.75G<F<= 2.9G
	0xa,	//2.9G<F<=3.0G, for 2.97G
};


/*Interpolator current setting for different frequency
 VCO Frequency '  INTPI
*/
unsigned int diag_avpllRegINTPI[]=
{
	0x3,	//for 1.512G
	0x3,	//for 1.62G
	0x5,	//for 1.856G
	0x7,	//for 2.16G
	0x7,	//for 2.227G
	0x9, 	//for 2.52G
	0xa, 	//for 2.7G
	0xb,	//for 2.97G
};

unsigned int diag_avpllRegINTPR[]=
{
	0x4,	//for 1.512G
	0x4,	//for 1.62G
	0x4,	//for 1.856G
	0x3,	//for 2.16G
	0x3,	//for 2.227G
	0x3, 	//for 2.52G
	0x1, 	//for 2.7G
	0x1,	//for 2.97G
};

//vco is determined by video frequence and HDMI mode
static int vcoSelectionTable[3][6] =
{
	//8 bit table
	{VCO_FREQ_1_512G, VCO_FREQ_2_160G, VCO_FREQ_2_160G, VCO_FREQ_2_970G, VCO_FREQ_2_160G, VCO_FREQ_2_970G},
	//10 bit table
	{VCO_FREQ_2_520G, VCO_FREQ_2_700G, VCO_FREQ_2_700G, VCO_FREQ_1_856G, VCO_FREQ_2_700G, VCO_FREQ_1_856G},
	//12 bit table
	{VCO_FREQ_1_512G, VCO_FREQ_1_620G, VCO_FREQ_1_620G, VCO_FREQ_2_227G, VCO_FREQ_1_620G, VCO_FREQ_2_227G},
};

//1KPPM is determined by video frequence and 59Hz/60Hz mode
static int ppm1kSelectionTable[2][6] =
{
	{-1, 0, 0, -1, 0, -1}, //59.94Hz 1KPPM flag
	{ 0, 1, 1,  0, 1,  0}, //60Hz 1KPPM flag
};

static CLK_FREQ clk_freqs_computed[VCO_FREQ_2_970G+1];

int diag_pll_A_VCO_Setting = VCO_FREQ_2_227G;
int diag_pll_B_VCO_Setting = VCO_FREQ_1_620G;

unsigned int gAVPLLA_Channel_OutputClock[8] = {0};
unsigned int gAVPLLB_Channel_OutputClock[8] = {0};

//this is good for all VCOs except 2520GHz
unsigned int audio_freq1[] =
{
    8467200  ,
    11289600 ,
    90316800 ,
    16934400 ,
    22579200 ,
    33868800 ,
    6144000 ,
    8192000  ,
    65536000 ,
    12288000 ,
    16384000 ,
    24576000 ,
    9216000  ,
    12288000  ,
    98304000 ,
    18432000 ,
    24576000 ,
    36864000 ,
};

//this is for 2520GHz
unsigned int audio_freq2[] =
{
    705600     ,
    1411200    ,
    2822400    ,
    5644800    ,
    8467200    ,
    11289600   ,
    16934400   ,
    22579200   ,
    33868800   ,
    45158400   ,
    90316800   ,
    512000     ,
    1024000    ,
    2048000    ,
    4096000    ,
    6144000    ,
    8192000    ,
    12288000   ,
    16384000   ,
    24576000   ,
    32768000   ,
    768000     ,
    1536000    ,
    3072000    ,
    6144000    ,
    9216000    ,
    12288000   ,
    18432000   ,
    24576000   ,
    36864000   ,
    49152000   ,
    98304000   ,
};

void diag_videoFreq(SIE_avPll *avPllBase, int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId);
static void diag_assertPllReset(SIE_avPll *avPllBase);
static void diag_deassertPllReset(SIE_avPll *avPllBase);
//assert reset for channel1 to 7
static void diag_assertCxReset(SIE_avPll *avPllBase);
static void diag_deassertCxReset(SIE_avPll *avPllBase);
static void diag_enableAvPll(SIE_avPll *avPllBase, unsigned int init_vco);
static void diag_dump_CLK_FREQ(CLK_FREQ* pCLK_FREQ);

void diag_setDPll(SIE_avPll *avPllBase, int enable, int p_sync1, int p_sync2, int chId)
{
	//disable DPll first
	//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_EN_DPLL = 0;
	PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
        ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_EN_DPLL = 0;
	PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);

	if(!p_sync1)
	{
		if(enable) 
		{
			dbg_printf(PRN_RES, "Warning p_sync1 is 0\n");
		}
	}

	//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_P_SYNC1 = p_sync1;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl2, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl2);
        ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_P_SYNC1 = p_sync1;
        PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl2, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl2);

	//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_P_SYNC2 = p_sync2;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl3);
        ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_P_SYNC2 = p_sync2;
        PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl3);

	if(enable)
	{
		//enable DPLL
		//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_EN_DPLL = 1;
	        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
                ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_EN_DPLL = 1;
	        PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
	}
}

void diag_setVDDL_VCOref(SIE_avPll *avPllBase)
{
        //T32avPll_ctrlPLL ctrlPLL;
        //ctrlPLL.u32 = avPllBase->u32avPll_ctrlPLL;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);

        //ctrlPLL.uctrlPLL_VCO_REF1P45_SEL = 0x3;
        //ctrlPLL.uctrlPLL_VDDL = 0xF;// set VDDL to 1.16v
        regSIE_avPll.uctrlPLL_VCO_REF1P45_SEL = 0x3;
        regSIE_avPll.uctrlPLL_VDDL = 0xF;// set VDDL to 1.16v

        //* (volatile unsigned int *)(&(avPllBase->u32avPll_ctrlPLL)) = ctrlPLL.u32;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
}
static void diag_powerDownChannel(SIE_avPll *avPllBase, int chId);

void diag_setVCO(SIE_avPll *avPllBase, int vco_freq_index)
{
	dbg_printf(PRN_INFO, "PLLA %d VCO freq change to %f\n", vco_freq_index, diag_vcoFreqs[vco_freq_index]);

	//first power done PLL and Channels
	diag_powerDown(avPllBase);

	//assert resets
	diag_assertPllReset(avPllBase);
	diag_assertCxReset(avPllBase);

        //change VDDL and VCO_ref to meet duty cycle
        diag_setVDDL_VCOref(avPllBase);

	//power up PLL and channels
	diag_powerUp(avPllBase);

    // @yeliu: power down these channels by hardcoded, only for bg2cdp
    diag_powerDownChannel(avPllBase, 2);
    diag_powerDownChannel(avPllBase, 5);
    diag_powerDownChannel(avPllBase, 6);

	//following settings are done under reset to improve long term reliability
	//avPllBase->uctrlPLL_FBDIV = diag_avpllRegFBDIV[vco_freq_index];
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        regSIE_avPll.uctrlPLL_FBDIV = diag_avpllRegFBDIV[vco_freq_index];
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);

	//avPllBase->uctrlINTP_INTPI = diag_avpllRegINTPI[vco_freq_index];
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlINTP, regSIE_avPll.u32avPll_ctrlINTP);
        regSIE_avPll.uctrlINTP_INTPI = diag_avpllRegINTPI[vco_freq_index];
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlINTP, regSIE_avPll.u32avPll_ctrlINTP);

	//avPllBase->uctrlINTP_INTPR = diag_avpllRegINTPR[vco_freq_index];
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlINTP, regSIE_avPll.u32avPll_ctrlINTP);
        regSIE_avPll.uctrlINTP_INTPR = diag_avpllRegINTPR[vco_freq_index];
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlINTP, regSIE_avPll.u32avPll_ctrlINTP);

	//avPllBase->uctrlPLL_EXT_SPEED = diag_avpllRegSPEED[vco_freq_index];
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL1, regSIE_avPll.u32avPll_ctrlPLL1);
        regSIE_avPll.uctrlPLL_EXT_SPEED = diag_avpllRegSPEED[vco_freq_index];
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL1, regSIE_avPll.u32avPll_ctrlPLL1);


	//(avPllBase->ie_C8).uctrl_FREQ_OFFSET = diag_avpllRegFREQ_OFFSET_C8[vco_freq_index];
        PHY_HOST_Bus_Read32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
        (regSIE_avPll.ie_C8).uctrl_FREQ_OFFSET = diag_avpllRegFREQ_OFFSET_C8[vco_freq_index];
        PHY_HOST_Bus_Write32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);

	if(avPllBase == (SIE_avPll *)AVPLL_A)
		diag_pll_A_VCO_Setting = vco_freq_index;
	else
		diag_pll_B_VCO_Setting = vco_freq_index;

	//deassert resets
	diag_deassertCxReset(avPllBase);

	diag_deassertPllReset(avPllBase);


	//toggle the offset_rdy bit
	{
		//(avPllBase->ie_C8).uctrl_FREQ_OFFSET_READY = 1;
                PHY_HOST_Bus_Read32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
                (regSIE_avPll.ie_C8).uctrl_FREQ_OFFSET_READY = 1;
                PHY_HOST_Bus_Write32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);

		//add some delay for Fvco=3GHz pulse>172ns, For Fvco=1.5GHz pulse>344ns
//		for(i=0; i<10000*10; i++);
	cpu_cycle_count_delay(550);

		//(avPllBase->ie_C8).uctrl_FREQ_OFFSET_READY = 0;
                PHY_HOST_Bus_Read32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
                (regSIE_avPll.ie_C8).uctrl_FREQ_OFFSET_READY = 0;
                PHY_HOST_Bus_Write32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
	}

	//do calibration
	diag_calibrate(avPllBase, diag_vcoFreqs[vco_freq_index]);

}

void diag_setChanOffset(SIE_avPll *avPllBase, int offset, int chId)
{
        unsigned int reg_offset = 0;

        dbg_printf(PRN_RES, "avPllBase(0x%X), offset(%d),  chId(%d)\n", avPllBase, offset, chId);
	if(offset>0)
	{
		reg_offset = (1<<18) | (offset) ;
	}
	else
	{
		reg_offset = -offset;
	}

	dbg_printf(PRN_INFO, "set 0x%X offset to 0x%x for C%d\n", avPllBase, reg_offset, chId);

	//set offset register
	//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_FREQ_OFFSET = reg_offset;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
        ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_FREQ_OFFSET = reg_offset;
        PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);

	//toggle the offset_rdy bit
	{
		//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_FREQ_OFFSET_READY = 1;
                PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
                ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_FREQ_OFFSET_READY = 1;
                PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);

		//add some delay because need to be asserted by 30ns
//		for(i=0; i<10000; i++);
	cpu_cycle_count_delay(200);

		//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_FREQ_OFFSET_READY = 0;
                PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
                ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_FREQ_OFFSET_READY = 0;
                PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
	}
}

void diag_set_Post_Div(SIE_avPll *avPllBase, int div, int chId)
{
	//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_POSTDIV = div;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
        ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_POSTDIV = div;
        PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
}

void diag_set_Post_0P5_Div(SIE_avPll *avPllBase, int enable, int chId)
{
	//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_POSTDIV_0P5 = enable;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
        ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_POSTDIV_0P5 = enable;
        PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
}

#define OFFSET_1KPPM_INC	(4190)
#define OFFSET_1KPPM_DEC	(-4194)

//this is done by using offset and dpll
//inc=1 means increase freq to 1001/1000
//inc=0 means disable 1KPPM (by setting offset to 0 and disable DPLL)
//inc=-1 means decrease freq to 1000/1001
void diag_set1KPPM(SIE_avPll *avPllBase, int inc, int chId)
{
	dbg_printf(PRN_RES, "avPllBase=0x%X, inc=%i, chId=%i\n", avPllBase, inc, chId);
	if(inc)
	{
		if(inc>0)
		{
			//increase by 1KPPM
			diag_setChanOffset(avPllBase, OFFSET_1KPPM_INC, chId);
			diag_setDPll(avPllBase, 1, 1000, 1001, chId);
		}
		else
		{
			//decrease by 1KPPM
			diag_setChanOffset(avPllBase, OFFSET_1KPPM_DEC, chId);
			diag_setDPll(avPllBase, 1, 1001, 1000, chId);
		}
	}
	else
	{
		//set offset to 0 and disable DPLL
		diag_setChanOffset(avPllBase, 0, chId);
		diag_setDPll(avPllBase, 0, 0, 0, chId);
	}
}

/* new reference in MHz, chan : chan_A 0, Chan_B 1 */
void diag_changeVcoFreq(int vco_freq_index, int grp)
{
	if(grp ==0)
	{
		diag_setVCO((SIE_avPll *)AVPLL_A, vco_freq_index);
	}
#if HAS_AVPLL_B
	else
	{
		diag_setVCO(AVPLL_B, vco_freq_index);
	}
#endif
}

void diag_assertPllReset(SIE_avPll *avPllBase)
{
	//assert reset
	//avPllBase->uctrlPLL_RESET=1;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        regSIE_avPll.uctrlPLL_RESET = 1;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
}

//assert reset for channel1 to 7
void diag_assertCxReset(SIE_avPll *avPllBase)
{
	int chId;

	for(chId=1; chId<=7; chId++)
        {
		//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_RESET = 1;
                PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl3);
                ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_RESET = 1;
                PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl3);
        }
}

void diag_deassertPllReset(SIE_avPll *avPllBase)
{
	volatile int i;

	for(i=0; i<10000*10; i++);

	//deassert reset
	//avPllBase->uctrlPLL_RESET=0;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        regSIE_avPll.uctrlPLL_RESET = 0;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
}

//assert reset for channel1 to 7
void diag_deassertCxReset(SIE_avPll *avPllBase)
{
	int chId;

	for(chId=1; chId<=7; chId++)
        {
		//((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_RESET = 0;
                PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl3);
                ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_RESET = 0;
                PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl3);
        }
}

static void diag_powerDownChannel(SIE_avPll *avPllBase, int chId)
{
    PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
    ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_PU = 0;
    PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
}

static void diag_powerUpChannel(SIE_avPll *avPllBase, int chId)
{
    PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
    ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_PU = 1;
    PHY_HOST_Bus_Write32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
}

void diag_powerDown(SIE_avPll *avPllBase)
{
	int chId;

	//avPllBase->uctrlPLL_PU = 0;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        regSIE_avPll.uctrlPLL_PU = 0;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);


    for(chId=1; chId<=7; chId++)
    {
        diag_powerDownChannel(avPllBase, chId);
    }

	//avPllBase->ie_C8.uctrl_PU = 0;
        PHY_HOST_Bus_Read32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
        (regSIE_avPll.ie_C8).uctrl_PU = 0;
        PHY_HOST_Bus_Write32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
}

void diag_powerUp(SIE_avPll *avPllBase)
{
	int chId;

    for(chId=1; chId<=7; chId++)
    {
        diag_powerUpChannel(avPllBase, chId);
    }
	//avPllBase->ie_C8.uctrl_PU = 1;
        PHY_HOST_Bus_Read32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
        (regSIE_avPll.ie_C8).uctrl_PU = 1;
        PHY_HOST_Bus_Write32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);

	//avPllBase->uctrlPLL_PU = 1;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        regSIE_avPll.uctrlPLL_PU = 1;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
}

//this function will enable AVPLL A and B and turn on caliberation
void diag_initAvPll()
{
	int i;
	T32Gbl_PadSelect  regPadSelect;
	T32avioGbl_AVPLLA_CLK_EN avplla_clk_en_data;


	diag_enableAvPll((SIE_avPll *)AVPLL_A, VCO_FREQ_2_227G);
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	diag_enableAvPll((SIE_avPll *)AVPLL_B, VCO_FREQ_1_620G);
#endif

	BFM_HOST_Bus_Read32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), &avplla_clk_en_data.u32);
	avplla_clk_en_data.uAVPLLA_CLK_EN_ctrl=0x3f;
	BFM_HOST_Bus_Write32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), avplla_clk_en_data.u32);


	dbg_printf(PRN_RES, "Enable DVIO Pad\n");
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_PadSelect), &regPadSelect.u32);
	regPadSelect.uPadSelect_DVIO_OEN = 1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_PadSelect), regPadSelect.u32);


    for (i=0; i<8; i++) {
        gAVPLLA_Channel_OutputClock[i] = 0;
        gAVPLLB_Channel_OutputClock[i] = 0;
	}
    // 525P settings for CPCB
    diag_videoFreq((SIE_avPll *)AVPLL_A, 1, 0, 0, 4, 1);
    //for HDMI
    diag_videoFreq((SIE_avPll *)AVPLL_A, 1, 0, 0, 10.0, 7);
}
/*
// This function is from Diag's 1/16/2014 version, it is not called by any other functions.
//this function only reintialize the AVIO_GBL's AVPLL. It should be called after AVIO gets reset
void diag_reInitAvPll_A()
{
        int i;
        int chId;
        T32Gbl_PadSelect  regPadSelect;
        T32avioGbl_AVPLLA_CLK_EN avplla_clk_en_data;

        diag_enableAvPll(AVPLL_A, VCO_FREQ_2_227G);


        BFM_HOST_Bus_Read32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), &avplla_clk_en_data.u32);
        avplla_clk_en_data.uAVPLLA_CLK_EN_ctrl=0x3f;
        BFM_HOST_Bus_Write32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), avplla_clk_en_data.u32);

    for (i=0; i<8; i++) {
        gAVPLLA_Channel_OutputClock[i] = 0;
        }
}
*/
void diag_enableAvPll(SIE_avPll *avPllBase, unsigned int init_vco)
{
	//set up VCO frequency, calibration done inside
	diag_setVCO(avPllBase, init_vco);
	dbg_printf(PRN_RES, "Enabled AvPLL instance %x\n", avPllBase);
}


void diag_calibrate(SIE_avPll *avPllBase, double fvco)
{
	volatile int i=100000;
	//MV_TimeSpec_t Time_Start, Time_End;

	//set the speed_thresh for current Fvco
	//avPllBase->uctrlCAL_SPEED_THRESH = (int)(fvco/100+0.5);
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlCAL, regSIE_avPll.u32avPll_ctrlCAL);
        regSIE_avPll.uctrlCAL_SPEED_THRESH = (int)(fvco/100+0.5);
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlCAL, regSIE_avPll.u32avPll_ctrlCAL);


	//assert PLL resebratet
	//avPllBase->uctrlPLL_RESET=1;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        regSIE_avPll.uctrlPLL_RESET = 1;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);

	cpu_cycle_count_delay(20000);
        //MV_Time_GetSysTime(&Time_Start);
	//avPllBase->uctrlPLL_RESET=0;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        regSIE_avPll.uctrlPLL_RESET = 0;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);

	//>20us and <50us must start calibration
	cpu_cycle_count_delay(35000);

	//avPllBase->uctrlCAL_PLL_CAL_START = 1;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlCAL, regSIE_avPll.u32avPll_ctrlCAL);
        regSIE_avPll.uctrlCAL_PLL_CAL_START = 1;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlCAL, regSIE_avPll.u32avPll_ctrlCAL);

        //MV_Time_GetSysTime(&Time_End);
#if 1 // PLATFORM==ASIC
	//poll PLL CAL done bit
	//while(!avPllBase->ustatus_PLL_CAL_DONE)
	while(1)
	{
                PHY_HOST_Bus_Read32(avPllBase->u32avPll_status, regSIE_avPll.u32avPll_status);
                if(regSIE_avPll.ustatus_PLL_CAL_DONE)
                     break;

		if(--i<0)
		{
			dbg_printf(PRN_RES, "PLL calibration Failed\n");
			break;
		}
	}
#endif
	cpu_cycle_count_delay(20000);

	//avPllBase->uctrlCAL_PLL_CAL_START = 0;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlCAL, regSIE_avPll.u32avPll_ctrlCAL);
        regSIE_avPll.uctrlCAL_PLL_CAL_START = 0;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlCAL, regSIE_avPll.u32avPll_ctrlCAL);

}

int divHDMICode[] =
{
	1, 2, 4, 6
};

int divAV1Code[] =
{
	1, 2, 5, 5
};

void diag_avpllFreq(SIE_avPll *avPllBase, int chId)
{
	double vco_freq;
	double freq_from_dpll, freq_from_offset;
	int offset, sync1, sync2, post_div, post_0p5_div;
	int enDPLL;

	dbg_printf(PRN_RES, "avPllBase=0x%x, diag_pll_A_VCO_Setting=%i, diag_pll_B_VCO_Setting=%i\n", \
	           avPllBase, diag_pll_A_VCO_Setting, diag_pll_B_VCO_Setting);
	dbg_printf(PRN_RES, "diag_vcoFreqs[diag_pll_A_VCO_Setting]=%f, diag_vcoFreqs[diag_pll_B_VCO_Setting]=%f\n", \
	           diag_vcoFreqs[diag_pll_A_VCO_Setting], diag_vcoFreqs[diag_pll_B_VCO_Setting]);
	if(avPllBase == (SIE_avPll *)AVPLL_A)
		vco_freq=diag_vcoFreqs[diag_pll_A_VCO_Setting];
	else
		vco_freq=diag_vcoFreqs[diag_pll_B_VCO_Setting];

	//find offset, sync1, sync2, divHDMI, divAV1, divAV2, divAV3 for this channel
	//offset = 		((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_FREQ_OFFSET;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl1);
        offset = ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_FREQ_OFFSET;

	//sync1 =  		((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_P_SYNC1;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl2, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl2);
        sync1 = ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_P_SYNC1;

	//sync2 =  		((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_P_SYNC2;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl3);
        sync2 = ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_P_SYNC2;

	//post_div = 		((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_POSTDIV;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
        post_div = ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_POSTDIV;

	//post_0p5_div = 	((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_POSTDIV_0P5;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
        post_0p5_div = ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_POSTDIV_0P5;

	//enDPLL = 		((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].uctrl_EN_DPLL;
        PHY_HOST_Bus_Read32(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl, ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].u32avpllCh_ctrl);
        enDPLL = ((SIE_avpllCh *)&(regSIE_avPll.ie_C1))[chId-1].uctrl_EN_DPLL;


	dbg_printf(PRN_RES, "offset=%i, sync1=%i, sync2=%i\n", offset, sync1, sync2);
	dbg_printf(PRN_RES, "post_div=%i, post_0p5_div=%i, enDPLL=%i\n", \
	           post_div, post_0p5_div, enDPLL);

	if(enDPLL != 0)
		freq_from_dpll = vco_freq*sync2/sync1;
	else
		freq_from_dpll = vco_freq;

	if(offset & (1<<18))
	{
		offset = (offset & ((1<<18)-1));
	}
	else
	{
		offset = -offset;
	}
	freq_from_offset = 1/(1-(double)offset/4194304)*vco_freq;

	if(post_div)
	{
		freq_from_offset = freq_from_offset/(post_div+post_0p5_div*.5);
		freq_from_dpll = freq_from_dpll/(post_div+post_0p5_div*.5);
	}

	dbg_printf(PRN_RES, "PLLA C%d Freq is: %fMHz(Offset) %fMHz(DPLL)\n", chId, freq_from_offset, freq_from_dpll);

	if(avPllBase == (SIE_avPll *)AVPLL_A)
		current_freq[0][chId]=freq_from_dpll;
	else
		current_freq[1][chId]=freq_from_dpll;

}

int diag_getVCOFreq(int hdmiMode, int freqIndex)
{
    return (vcoSelectionTable[hdmiMode][freqIndex]);
}

void diag_vcoFreq(SIE_avPll *avPllBase)
{
	double ref_freq=25.0;
	double freq_from_offset;
	int offset, fbDiv;
	(void)freq_from_offset;

	//offset = (avPllBase->ie_C8).uctrl_FREQ_OFFSET;
        PHY_HOST_Bus_Read32((avPllBase->ie_C8).u32avpllCh8_ctrl1, (regSIE_avPll.ie_C8).u32avpllCh8_ctrl1);
        offset = (regSIE_avPll.ie_C8).uctrl_FREQ_OFFSET;

	//fbDiv = avPllBase->uctrlPLL_FBDIV;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlPLL, regSIE_avPll.u32avPll_ctrlPLL);
        fbDiv = regSIE_avPll.uctrlPLL_FBDIV;

	if(offset& (1<<18))
	{
		offset = (offset & ((1<<18)-1));
	}
	else
	{
		offset = -offset;
	}

	freq_from_offset = ref_freq*fbDiv*(1 - (double)offset/4194304);

	dbg_printf(PRN_RES, "PLLA VCO Freq is: %f\n", freq_from_offset);
}


//freq_index, 0 to 5, the 6 VCLKs
//hdmiMode, HDMI_8BIT_MODE, HDMI_10BIT_MODE, HDMI_12BIT_MODE
//frameRate, FRAME_RATE_59P94 FRAME_RATE_60
//overSampleRate, 1, 2, 4, 8, 10, 12.5, 15
void diag_videoFreq(SIE_avPll *avPllBase, int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId)
{
	double vcoFreq, videoFreq;
	int ppm1K=ppm1kSelectionTable[frameRate][freqIndex];
	double divider;

	vcoFreqIndex=vcoSelectionTable[hdmiMode][freqIndex];
	//set VCO freq here
	diag_setVCO(avPllBase, vcoFreqIndex);
	dbg_printf(PRN_RES, "VCO freq=%f\n", diag_vcoFreqs[vcoFreqIndex]);

	//check to see if ppm1K is need
	vcoFreq = diag_vcoFreqs[vcoFreqIndex];
	if(ppm1K<0)
	{
		vcoFreq = vcoFreq*1000/1001;
	}

	if(ppm1K>0)
	{
		vcoFreq = vcoFreq*1001/1000;
	}
	diag_set1KPPM(avPllBase, ppm1K, chId);

	//get the video freq
	videoFreq = diag_videoFreqs[freqIndex];
	if(frameRate)
	{
		//60Hz vclk is 1001/1000 times 59.94Hz vclk
		videoFreq = videoFreq*1001/1000;
	}
	dbg_printf(PRN_RES, "Video freq=%f\n", videoFreq);

	divider = vcoFreq*2/videoFreq/overSampleRate;
	dbg_printf(PRN_RES, "AV post divider is %f %d\n", divider/2, (int)(divider+0.5)/2);

	//disable dividers
	diag_set_Post_Div(avPllBase, 0 , chId);
	diag_set_Post_0P5_Div(avPllBase, 0 , chId);

	//this enables post divider
	if(divider>2)
	{
		diag_set_Post_Div(avPllBase, ((int)(divider+0.5)/2) , chId);
	}

	if(((int)(divider+0.5))&1)
	{
		//fractional divider, use POSTDIV_0P5_CX
		diag_set_Post_0P5_Div(avPllBase, 1 , chId);
	}

	dbg_printf(PRN_RES, "PLL(%x) C%d=%f\n", avPllBase, chId, videoFreq*overSampleRate);

	if(((unsigned int)avPllBase&0xffff0000)==MEMMAP_AVIO_GBL_BASE)
		gAVPLLA_Channel_OutputClock[chId] = (unsigned int)(videoFreq*overSampleRate*1000000);
	else
		gAVPLLB_Channel_OutputClock[chId] = (unsigned int)(videoFreq*overSampleRate*1000000);

	diag_dump_CLK_FREQ(&(clk_freqs_computed[freqIndex]));
}

//freqIndex is the vco freq index into clk_freqs_computed[] array
int diag_clockFreq_computed(SIE_avPll *avPllBase, int freqIndex, int chId)
{
	int diag_pll_VCO_Setting;

	if(avPllBase == (SIE_avPll *)AVPLL_A)
	{
		diag_pll_VCO_Setting=diag_pll_A_VCO_Setting;
		dbg_printf(PRN_RES, "set A%d to %fMHz\n", chId, clk_freqs_computed[freqIndex].clkout_freq);
	}
	else
	{
		diag_pll_VCO_Setting=diag_pll_B_VCO_Setting;
		dbg_printf(PRN_RES, "set B%d to %fMHz\n", chId, clk_freqs_computed[freqIndex].clkout_freq);
	}

	if(freqIndex > (int)(sizeof(clk_freqs_computed) / sizeof(CLK_FREQ)))
	{
		dbg_printf(PRN_RES, "VCO Freq Index not found\n");
		return 1;
	}

	if(clk_freqs_computed[freqIndex].invalid)
	{
		dbg_printf(PRN_RES, "Frequency entry is invalid!\n");
		return 1;
	}

	if(diag_pll_VCO_Setting != clk_freqs_computed[freqIndex].vco_freq_index)
	{
		dbg_printf(PRN_RES, "VCO frequency is changed to %f!\n",
			diag_vcoFreqs[clk_freqs_computed[freqIndex].vco_freq_index]);

		diag_setVCO(avPllBase, clk_freqs_computed[freqIndex].vco_freq_index);
	}

	//change offset
	diag_setChanOffset(avPllBase, clk_freqs_computed[freqIndex].freq_offset, chId);

	//change p_sync
	diag_setDPll(avPllBase, (clk_freqs_computed[freqIndex].p_sync1!=0),
			clk_freqs_computed[freqIndex].p_sync1,
			clk_freqs_computed[freqIndex].p_sync2, chId);

	//update now div
	diag_set_Post_Div(avPllBase, clk_freqs_computed[freqIndex].post_div, chId);
	diag_set_Post_0P5_Div(avPllBase, clk_freqs_computed[freqIndex].post_div_0p5, chId);

        dbg_printf(PRN_RES, "-------------clk_freqs_computed[%i]----------------------------\n", freqIndex);
	diag_dump_CLK_FREQ(&(clk_freqs_computed[freqIndex]));
	return 0;

}

/*this function is introduced by software team*/
int diag_getChannelOutputFreq(int AVPLLIndex,int chID)
{
    if (chID<0 || chID>7)
        return 0;

    if (AVPLLIndex == 0) //AVPLL-A
        return (gAVPLLA_Channel_OutputClock[chID]);
    else
        return (gAVPLLB_Channel_OutputClock[chID]);
}

int diag_en_ATP(SIE_avPll *avPllBase)
{
	int i;
	char character;

	dbg_printf(PRN_RES, "enabling PLL_A Analog Test Point one-by-one\n");

	for(i=1; i<=0x1f; i++)
	{
		//avPllBase->uctrlTest_TEST_MON = (0x20 | i);
                PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);
                regSIE_avPll.uctrlTest_TEST_MON = (0x20 | i);
                PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);

		dbg_printf(PRN_RES, "enabled ATP %d, hit n for next\n", i);
		while(1)
		{
			//character = diag_uart_master_read_1_char(0);
			if (character == 'n') break;
		}
	}

	//avPllBase->uctrlTest_TEST_MON = 0;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);
        regSIE_avPll.uctrlTest_TEST_MON = 0;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);

	return 0;
}

int diag_en_clkout_tst(SIE_avPll *avPllBase, int test_mon)
{
	char character = '\0';

	//avPllBase->uctrlTest_CLKOUT_TST_EN = 0;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);
        regSIE_avPll.uctrlTest_CLKOUT_TST_EN = 0;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);

	//avPllBase->uctrlTest_TEST_MON = test_mon;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);
        regSIE_avPll.uctrlTest_TEST_MON = test_mon;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);

	//avPllBase->uctrlTest_CLKOUT_TST_EN = 1;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);
        regSIE_avPll.uctrlTest_TEST_MON = 1;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);

	dbg_printf(PRN_RES, "enabled CLKOUT TST %d, hit q to quit\n", test_mon);
	while(1)
	{
		//character = diag_uart_master_read_1_char(0);
		if (character == 'q') break;
	}

	//avPllBase->uctrlTest_CLKOUT_TST_EN = 0;
        PHY_HOST_Bus_Read32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);
        regSIE_avPll.uctrlTest_CLKOUT_TST_EN = 0;
        PHY_HOST_Bus_Write32(avPllBase->u32avPll_ctrlTest, regSIE_avPll.u32avPll_ctrlTest);

	return 0;
}

int diag_avPllRegDump(SIE_avPll *avPllBase)
{
	int chId;
	(void)avPllBase;

	dbg_printf(PRN_RES, "AVPLL control Regs:\n");

	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_ctrlPLL), avPllBase->u32avPll_ctrlPLL);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_ctrlPLL1), avPllBase->u32avPll_ctrlPLL1);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_ctrlCAL), avPllBase->u32avPll_ctrlCAL);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_ctrlCAL1), avPllBase->u32avPll_ctrlCAL1);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_ctrlSlowLoop), avPllBase->u32avPll_ctrlSlowLoop);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_ctrlINTP), avPllBase->u32avPll_ctrlINTP);

	for(chId=1; chId<=7; chId++)
	{
		dbg_printf(PRN_RES, "AVPLL Channel control Regs, chanId=%d:\n", chId);

		dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl);
		dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1);
		dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl2), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl2);
		dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3);

	}

	chId=8;
	dbg_printf(PRN_RES, "AVPLL Channel control Regs, chanId=%d:\n", chId);

	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl1);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl2), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl2);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3), ((SIE_avpllCh *)(&(avPllBase->ie_C1)))[chId-1].u32avpllCh_ctrl3);

	dbg_printf(PRN_RES, "AVPLL status:\n");
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_status), avPllBase->u32avPll_status);
	dbg_printf(PRN_RES, "0x%08X: 0x%08X\n", &(avPllBase->u32avPll_status1), avPllBase->u32avPll_status1);
	return 0;
}

int gcd(unsigned int m, unsigned int n)
{
	dbg_printf(PRN_INFO, "R=%d, Q=%d\n", m, n);

	int rem;
	while(n!=0)
	{
		rem=m%n;
		m=n;
		n=rem;
	}

	dbg_printf(PRN_INFO, "S=%d\n", m);
	return(m);
}

void diag_print_freqs_computed(int i)
{
	if(clk_freqs_computed[i].invalid)
	{
		dbg_printf(PRN_RES, "%d: %4.2fGHz\t%4.3fMHz\tN\A\n", i,
			diag_vcoFreqs[i],
			clk_freqs_computed[i].clkout_freq);
	}
	else
	{
		dbg_printf(PRN_RES, "%d: %4.2fGHz\t%4.3fMHz\t%6d\t%6d\t%6d\t%d\t%d\t%1.3f\n", i,
			diag_vcoFreqs[i],
			clk_freqs_computed[i].clkout_freq,
			clk_freqs_computed[i].freq_offset,
			clk_freqs_computed[i].p_sync2,
			clk_freqs_computed[i].p_sync1,
			clk_freqs_computed[i].post_div,
			clk_freqs_computed[i].post_div_0p5,
			(((double)clk_freqs_computed[i].p_sync2-(double)clk_freqs_computed[i].p_sync1))/clk_freqs_computed[i].p_sync1);
	}
}

//target_freq is in Hz
//compute channel post div, freq_offset for target freq
int diag_compute_freq_setting(unsigned int vco_freq_index, unsigned int target_freq)
{
	double offset, offset_percent;
	double divider;
	double ratio;
	int int_divider;

	memset(&(clk_freqs_computed[vco_freq_index]), 0, sizeof(CLK_FREQ));

	clk_freqs_computed[vco_freq_index].vco_freq_index=vco_freq_index;
	clk_freqs_computed[vco_freq_index].clkout_freq=(float)target_freq/1000000;

	//.5 divider is only used when divider is less than 24
	//This matches the settings in audio freq table in the IP doc

	ratio = diag_vcoFreqs[vco_freq_index]*1000000/target_freq;

	if(ratio < 24)
	{
		//allow 0.5 divider, round to closest 0.5
		int_divider = (int)(ratio*2+0.5);
	}
	else
	{
		//round to closest int
		int_divider = ((int)(ratio+0.5))*2;
	}
	divider = ((double)int_divider)/2;

	dbg_printf(PRN_INFO, "int_div=%d, divider=%f\n", int_divider, divider);

	clk_freqs_computed[vco_freq_index].post_div_0p5= (int_divider&1);
	clk_freqs_computed[vco_freq_index].post_div=((int)(divider));

	//now figure out the offset
	offset_percent = (target_freq*divider/1000000 - diag_vcoFreqs[vco_freq_index])/diag_vcoFreqs[vco_freq_index];

	dbg_printf(PRN_INFO, "offset_percent=%f\n", offset_percent);

	offset = 4194304*offset_percent/(1+offset_percent);

	dbg_printf(PRN_INFO, "offset=%f\n", offset);


	if(offset<=-262144 || offset>= 262144)
	{
		//offset cannot be achieved
		clk_freqs_computed[vco_freq_index].invalid = 1;
		return 1;
	}
	else
	{
		unsigned int vco_ratio, freq_ratio, gcd_val;

		//for rounding
		if(offset>0) offset+=0.5;
		else offset-=0.5;

		clk_freqs_computed[vco_freq_index].freq_offset = (int)(offset);
		dbg_printf(PRN_INFO, "target=%d, divider=%f, target_freq*divider=%f\n", target_freq, divider, (target_freq*divider));
		gcd_val= gcd(diag_vcoFreqs[vco_freq_index]*1000000, target_freq*divider);
		vco_ratio = (int)(diag_vcoFreqs[vco_freq_index]*1000000/gcd_val);
		freq_ratio = (int)(target_freq*divider/gcd_val);
		dbg_printf(PRN_INFO, "T=%d, U=%d\n", freq_ratio, vco_ratio);

		if((gcd_val/1000)<1000)
		{
			clk_freqs_computed[vco_freq_index].p_sync2=freq_ratio;
			clk_freqs_computed[vco_freq_index].p_sync1=vco_ratio;
		}
		else
		{
			clk_freqs_computed[vco_freq_index].p_sync2=freq_ratio*((int)(gcd_val/1000000+0.5));
			clk_freqs_computed[vco_freq_index].p_sync1=vco_ratio*((int)(gcd_val/1000000+0.5));
		}

	}

	return 0;
}

int diag_clockFreq(SIE_avPll *avPllBase, int vco_freq_index, unsigned int target_freq, int chId)
{
	dbg_printf(PRN_RES, "avPllBase(0x%X), vco_freq_index(%d), target_freq(%d), chId(%d)\n", avPllBase, vco_freq_index, target_freq, chId);

	if(diag_compute_freq_setting(vco_freq_index, target_freq))
	{
		dbg_printf(PRN_RES, "freq %fHz cannot be achieved with VCO=%fMHz\n", (float)target_freq/1000000, diag_vcoFreqs[vco_freq_index]);
		return 1;
	}
	else
	{
		//frequency ok, set it
		diag_clockFreq_computed(avPllBase, vco_freq_index, chId);
	}

	return 0;
}

void diag_check_audioFreq()
{
	size_t i;

	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_2_227G]);
	for(i=0; i<sizeof(audio_freq1)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_2_227G, audio_freq1[i]);
		diag_print_freqs_computed(VCO_FREQ_2_227G);
	}


	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_1_620G]);
	for(i=0; i<sizeof(audio_freq1)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_1_620G, audio_freq1[i]);
		diag_print_freqs_computed(VCO_FREQ_1_620G);
	}

	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_1_512G]);
	for(i=0; i<sizeof(audio_freq1)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_1_512G, audio_freq1[i]);
		diag_print_freqs_computed(VCO_FREQ_1_512G);
	}

	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_2_700G]);
	for(i=0; i<sizeof(audio_freq1)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_2_700G, audio_freq1[i]);
		diag_print_freqs_computed(VCO_FREQ_2_700G);
	}

	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_1_856G]);
	for(i=0; i<sizeof(audio_freq1)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_1_856G, audio_freq1[i]);
		diag_print_freqs_computed(VCO_FREQ_1_856G);
	}

	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_2_520G]);
	for(i=0; i<sizeof(audio_freq2)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_2_520G, audio_freq2[i]);
		diag_print_freqs_computed(VCO_FREQ_2_520G);
	}

	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_2_160G]);
	for(i=0; i<sizeof(audio_freq1)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_2_160G, audio_freq1[i]);
		diag_print_freqs_computed(VCO_FREQ_2_160G);
	}

	dbg_printf(PRN_RES, "VCO=%f:\n", diag_vcoFreqs[VCO_FREQ_2_970G]);
	for(i=0; i<sizeof(audio_freq1)/sizeof(int); i++)
	{
		diag_compute_freq_setting(VCO_FREQ_2_970G, audio_freq1[i]);
		diag_print_freqs_computed(VCO_FREQ_2_970G);
	}
}

/*
// This function is from Diag's 1/16/2014 version, it is not called by any other functions.
//check if the current avPLL is power down or not
//0=power down
//1=power up
int diag_powerStatus(SIE_avPll *avPllBase)
{
      return avPllBase->uctrlPLL_PU;
}
*/

void diag_dump_CLK_FREQ(CLK_FREQ* pCLK_FREQ)
{
    (void)pCLK_FREQ;
    dbg_printf(PRN_RES, "vco_freq_index=%i\n", pCLK_FREQ->vco_freq_index);
    dbg_printf(PRN_RES, "clkout_freq=%f\n", pCLK_FREQ->clkout_freq);
    dbg_printf(PRN_RES, "freq_offset=%i\n", pCLK_FREQ->freq_offset);
    dbg_printf(PRN_RES, "p_sync2=%i\n", pCLK_FREQ->p_sync2);
    dbg_printf(PRN_RES, "p_sync1=%i\n", pCLK_FREQ->p_sync1);
    dbg_printf(PRN_RES, "post_div=%i\n", pCLK_FREQ->post_div);
    dbg_printf(PRN_RES, "post_div_0p5=%i\n", pCLK_FREQ->post_div_0p5);
    dbg_printf(PRN_RES, "invalid=%i\n", pCLK_FREQ->invalid);
}

void cpu_cycle_count_delay(unsigned int ns)
{
    unsigned int start,diff, end;

    GA_REG_WORD32_READ(0xF7E82C04, &start);

    do
    {
        GA_REG_WORD32_READ(0xF7E82C04, &end);
        if(start >= end)
                diff = start - end;
        else
                diff = 1000*1000 + start - end;

    } while(diff < ns/10);
}

int AVPLL_Set(int groupId, int chanId, unsigned int avFreq)
{
    if (groupId == 0)
        diag_clockFreq((SIE_avPll *)AVPLL_A, vcoFreqIndex, avFreq, chanId);
#if HAS_AVPLL_B
    else if (groupId == 1)
        diag_clockFreq(AVPLL_B, vcoFreqIndex, avFreq, chanId);
#endif
    return 0;
}

void diag_videoFreq_A(int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId)
{
    (void)freqIndex;
    (void)hdmiMode;
    (void)frameRate;
    (void)overSampleRate;
    (void)chId;
}

void diag_videoFreq_B(int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId)
{
    (void)freqIndex;
    (void)hdmiMode;
    (void)frameRate;
    (void)overSampleRate;
    (void)chId;
}

void dump_AVPLL_regs(int nRegs)
{
	int i;

	dbg_printf(PRN_RES, "\n");
	for (i=0; i<=nRegs && i<0xD0; i +=0x10)
		dbg_printf(PRN_RES, "0x%08X:\t0x%08X\t0x%08X\t0x%08X\t0x%08X\n", 0xf7e20000 + i, *((unsigned int*)((char*)AVPLL_A + i)), *((unsigned int*)((char*)AVPLL_A + i + 4)), *((unsigned int*)((char*)AVPLL_A + i + 8)), *((unsigned int*)((char*)AVPLL_A + i +12)));
	if(i == 0xD0 && i < nRegs) {
		dbg_printf(PRN_RES, "0x%08X:\t0x%08X\t0x%08X\n", 0xf7e200d0, *((unsigned int*)((char*)AVPLL_A + 0xd0)), *((unsigned int*)((char*)AVPLL_A + 0xd4)));
	}
	dbg_printf(PRN_RES, "\n");
}

void AVPLL_Enable(void)
{
	diag_initAvPll();
}
#else	//BG2CDP
#include "com_type.h"
#include "Galois_memmap.h"
#include "galois_io.h"
#include "global.h"
#include "avpll.h"
#include "mv_logger.h"

#if (BERLIN_CHIP_VERSION >= BERLIN_BG2_Q)
#include "avioGbl.h"
#endif
#ifdef BERLIN

#define BFM_HOST_Bus_Write32 	GA_REG_WORD32_WRITE
#define BFM_HOST_Bus_Read32	GA_REG_WORD32_READ

static char dbg_buf[256];

enum PRINT_LEVEL
{
	PRN_NONE=0,
	PRN_ERR,
	PRN_RES,
	PRN_INFO,
	PRN_DBG
};
void dbg_printf(SIGN32 logLevel,const char* szFormat, ...)
{
#if 1
	va_list args;
	va_start(args, szFormat);
	vsnprintf(dbg_buf, 255, szFormat, args);
	printf("%s\n", dbg_buf);
	va_end(args);
#endif
}

typedef struct
{
	unsigned int vco_freq_index;
	unsigned int av_freq_index;
	int freq_offset; //signed number
	unsigned int p_sync2;
	unsigned int p_sync1;
	unsigned int divAV1;
	unsigned int divAV2;
	unsigned int divAV3;
	//unsigned int invalid; //this indicate it is a valid setting or not: 0 is valid, 1 is not

} CLK_FREQ;

double current_freq[2][7];

enum
{
	HDMI_8BIT_MODE=0,
	HDMI_10BIT_MODE,
	HDMI_12BIT_MODE,
	HDMI_MAX_MODE
};

enum
{
	FRAME_RATE_59P94=0,
	FRAME_RATE_60
};

#define OVERSAMPLE_HDMI		0
#define OVERSAMPLE_1X			1
#define OVERSAMPLE_2X			2
#define OVERSAMPLE_4X			4
#define OVERSAMPLE_8X			8

double diag_hdmiOverSample[] =
{
	10,
	12.5,
	15
};

double diag_videoFreqs[] =
{
	//video frequencies, pixel freq
	25.17482517,
	27,
	54,
	74.17582418,
	108,
	148.3516484,
	135.30
};

//all the VCO freq reqired for video and audio in MHz
double diag_vcoFreqs[]=
{
	1080, 	//8 bit HDMI
	1260,	//10 bit HDMI
	1350,	//10 bit HDMI
	1485,	//8 bit HDMI
	1512, 	//8 bit HDMI	and 12 bit HDMI
	1620,	//12 bit HDMI
	1856.25,//10 bit HDMI
	2227.5,	//12 bit HDMI
	1353,
};

//from Section 7 table
unsigned char diag_avpllRegFBDIV[]=
{
	43, //VCO_FREQ_1_080G,
	50, //VCO_FREQ_1_260G,
	54, //VCO_FREQ_1_350G,
	59, //VCO_FREQ_1_485G,
	60, //VCO_FREQ_1_512G,
	65, //VCO_FREQ_1_620G,
	74, //VCO_FREQ_1_856G,
	89, //VCO_FREQ_2_227G
	54, //VCO_FREQ_1_353G,
};

//from Section 7 table, bit18 is signe bit
#if (BERLIN_CHIP_VERSION >= BERLIN_BG2_Q)
unsigned int diag_avpllRegFREQ_OFFSET_C8[]=
{
	(19508),	//VCO_FREQ_1_080G,
	(33554), 	//VCO_FREQ_1_260G,
	(00000), 	//VCO_FREQ_1_350G,
	(28435), 	//VCO_FREQ_1_485G,
	(33554), 	//VCO_FREQ_1_512G,
	((1<<18)|(12906)), 	//VCO_FREQ_1_620G,
	(14170), 	//VCO_FREQ_1_856G,
	(4713), 	//VCO_FREQ_2_227G
	(00000), 	//VCO_FREQ_1_353G,
};
#else
unsigned int diag_avpllRegFREQ_OFFSET_C8[]=
{
	(19508),	//VCO_FREQ_1_080G,
	(33554), 	//VCO_FREQ_1_260G,
	(00000), 	//VCO_FREQ_1_350G,
	(28435), 	//VCO_FREQ_1_485G,
	(33554), 	//VCO_FREQ_1_512G,
	((1<<18)|(12905)), 	//VCO_FREQ_1_620G,
	(14169), 	//VCO_FREQ_1_856G,
	(4712), 	//VCO_FREQ_2_227G
	(00000), 	//VCO_FREQ_1_353G,
};
#endif

#if (BERLIN_CHIP_VERSION >= BERLIN_BG2_Q)
unsigned int diag_avpllRegSPEED[]=
{
	0x0,	//1.08G<F<= 1.21G, for 1.08G
	0x2,	//1.21G<F<=1.4G	   for 1.26G
	0x2,	//1.21G<F<=1.4G	   for 1.35G
	0x3,	//1.4G<F<=1.61G    for 1.48G
	0x3,	//1.4G<F<=1.61G    for 1.51G
	0x4,	//1.61G<F<= 1.86G  for 1.62G
	0x5, 	//1.61G<F<= 1.86G  for 1.85G
//	0x4,	//1.86G<F<= 2G	   not used
//	0x5,	//2G<F<=2.22G      not used
	0x6,	//F> 2.22G         for 2.22G
	0x2,	//1.21G<F<=1.4G	   for 1.35G
};
#else
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
unsigned int diag_avpllRegSPEED[]=
{
	0x0,	//1.08G<F<= 1.21G, for 1.08G
	0x0,	//1.21G<F<=1.4G	   for 1.26G
	0x1,	//1.21G<F<=1.4G	   for 1.35G
	0x2,	//1.4G<F<=1.61G    for 1.48G
	0x2,	//1.4G<F<=1.61G    for 1.51G
	0x3,	//1.61G<F<= 1.86G  for 1.62G
	0x4,	//1.61G<F<= 1.86G  for 1.85G
//	0x4,	//1.86G<F<= 2G	   not used
//	0x5,	//2G<F<=2.22G      not used
	0x6,	//F> 2.22G         for 2.22G
	0x1,	//		for 1.35G
};
#else /* (BERLIN_CHIP_VERSION == BERLIN_BG2_CD) */
unsigned int diag_avpllRegSPEED[]=
{
	0x0,    //0.95G<F<=1.08G   for 1.08G
	0x2,    //1.25G<F<=1.4G    for 1.26G
	0x2,    //1.21G<F<=1.4G    for 1.35G
	0x3,    //1.42G<F<=1.61G   for 1.48G
	0x3,    //1.42G<F<=1.61G   for 1.51G
	0x4,    //1.60G<F<= 1.86G  for 1.62G
	0x5,    //1.80G<F<= 2G     for 1.85G
	0x6,    //2.0G<F<=2.3G     for 2.22G
	0x2,	//1.21G<F<=1.4G	   for 1.35G
};
#endif /* (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) */
#endif /* (BERLIN_CHIP_VERSION >= BERLIN_BG2_Q)  */

#if (BERLIN_CHIP_VERSION >= BERLIN_BG2_Q)
unsigned int diag_avpllRegINTPI[]=
{
	0x3,	//1G~1.4		for 1.08G
	0x3,	//1G~1.4		for 1.26G
	0x3,	//1G~1.4		for 1.35G
	0x4,	//1.4G~1.8G		for 1.48G
	0x4,	//1.4G~1.8G		for 1.51G
	0x4,	//1.4G~1.8G		for 1.62G
	0x5,	//1.8G~2.25G		for 1.85G
	0x5,	//1.8G~2.25G    	for 2.22G
	0x3,	//			for 1.35G
};
#else
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
unsigned int diag_avpllRegINTPI[]=
{
	0x2,	//1G~1.4		for 1.08G
	0x2,	//1G~1.4		for 1.26G
	0x2,	//1G~1.4		for 1.35G
	0x3,	//1.4G~1.8G		for 1.48G
	0x3,	//1.4G~1.8G		for 1.51G
	0x3, 	//1.4G~1.8G		for 1.62G
	0x4, 	//1.8G~2.25G		for 1.85G
	0x4,	//1.8G~2.25G    	for 2.22G
	0x2,	//			for 1.35G
};
#else /* (BERLIN_CHIP_VERSION == BERLIN_BG2_CD) */
unsigned int diag_avpllRegINTPI[]=
{
	0x3,    //1G~1.4                for 1.08G
	0x3,    //1G~1.4                for 1.26G
	0x3,    //1G~1.4                for 1.35G
	0x4,    //1.4G~1.8G             for 1.48G
	0x4,    //1.4G~1.8G             for 1.51G
	0x4,    //1.4G~1.8G             for 1.62G
	0x5,    //1.8G~2.25G    	for 1.85G
	0x5,    //1.8G~2.25G    	for 2.22G
	0x3,	//			for 1.35G
};
#endif /* (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) */
#endif /* (BERLIN_CHIP_VERSION >= BERLIN_BG2_Q)  */

//vco is determined by video frequence and HDMI mode
static int vcoSelectionTable[3][7] =
{
	{AVPLL_VCO_FREQ_1_512G, AVPLL_VCO_FREQ_1_080G, AVPLL_VCO_FREQ_1_080G, AVPLL_VCO_FREQ_1_485G, AVPLL_VCO_FREQ_1_080G, AVPLL_VCO_FREQ_1_485G, AVPLL_VCO_FREQ_1_353G},
	{AVPLL_VCO_FREQ_1_260G, AVPLL_VCO_FREQ_1_350G, AVPLL_VCO_FREQ_1_350G, AVPLL_VCO_FREQ_1_856G, AVPLL_VCO_FREQ_1_350G, AVPLL_VCO_FREQ_1_856G, AVPLL_VCO_FREQ_1_353G},
	{AVPLL_VCO_FREQ_1_512G, AVPLL_VCO_FREQ_1_620G, AVPLL_VCO_FREQ_1_620G, AVPLL_VCO_FREQ_2_227G, AVPLL_VCO_FREQ_1_620G, AVPLL_VCO_FREQ_2_227G, AVPLL_VCO_FREQ_1_353G},
};

//1KPPM is determined by video frequence and 59Hz/60Hz mode
static int ppm1kSelectionTable[2][7] =
{
	{-1, 0, 0, -1, 0, -1, 0}, //59.94Hz 1KPPM flag
	{ 0, 1, 1,  0, 1,  0, 0}, //60Hz 1KPPM flag
};


CLK_FREQ clk_freqs_computed[AVPLL_VCO_FREQ_MAX];

// Just for Audio channel configuration. Video channel configuration uses other tables.
static CLK_FREQ clk_freqs[] =
{
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_90P3168_MHZ,	-56508,	12544, 12375, 6, 0x5},	// 44.1KHz
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_65P536_MHZ, 	-121161,114688,111375,6, 0x7},	// 32KHz
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_98P304_MHZ,	-62130,	188416,185625,4, 0x17},	// 48KHz

	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_90P3168_MHZ,	-14712,	3136,	3125, 5, 0x9},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_65P536_MHZ,	-47104,	2048,	2025, 6, 0x5},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_98P304_MHZ,	125696,	16384,	16875,5, 0x8},

	{AVPLL_VCO_FREQ_1_512G,	AUDIO_FREQ_90P3168_MHZ,	-63883, 1904,	1875, 4, 0x11},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_65P536_MHZ,	13000,	23552,	23625,4, 0x17},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_98P304_MHZ,	106496, 1536,	1575, 4, 0xF},

	{AVPLL_VCO_FREQ_1_350G,	AUDIO_FREQ_90P3168_MHZ,	-14712,	3136,	3125, 4, 0xF},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_65P536_MHZ,	-80018,	28672,	28125,4, 0x15},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_98P304_MHZ,	-80018,	28672,	28125,5, 0x7},

	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_90P3168_MHZ,	-89347,	175616,	171875,4,0x15},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_65P536_MHZ,	48553,	917504,	928125,5,0xE},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_98P304_MHZ,	-25882,	311296,	309375,4,0x13},

	{AVPLL_VCO_FREQ_1_260G,	AUDIO_FREQ_90P3168_MHZ,	-14712,	3136,	3125,  5,0x7},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_65P536_MHZ,	49906,	38912,	39375, 4,0x13},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_98P304_MHZ,	-58919,	13312,	13125, 4,0xD},

	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_90P3168_MHZ,	-14712,	3136,	3125,  5,0x6},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_65P536_MHZ,	125696,	16384,	16875, 5,0x8},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_98P304_MHZ, 	-5213,	5632,	5625,  4,0xB},

	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_90P3168_MHZ,	115900,	100352,	103125,5,0x8},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_65P536_MHZ, 	-62130,	188416,	185625,4,0x17},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_98P304_MHZ, 	29696,	4096,	4125,  4,0xF},

	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_22P5792_MHZ, -56508, 12544, 12375, 6, 0x14},	// 44.1KHz
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_16P384_MHZ,  29696,  4096,  4125,  6, 0x1B},	// 32KHz
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_24P576_MHZ,  29696,  4096,  4125,  6, 0x12},  // 48KHz

	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_22P5792_MHZ, 104704, 5488,  5625,  6, 0xE},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_16P384_MHZ,  -47104, 2048,  2025,  6, 0x14},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_24P576_MHZ,  59234,  3328,  3375,  6, 0xD},

	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_22P5792_MHZ, 126750, 1456,  1500,  6, 0xD},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_16P384_MHZ,  106496, 1536,  1575,  6, 0x12},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_24P576_MHZ,  106496, 1536,  1575,  6, 0xC},

	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_22P5792_MHZ, -14712, 3136,  3125,  6, 0xC},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_16P384_MHZ,  125696, 16384, 16875, 6, 0x10},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_24P576_MHZ,  -5213,  5632,  5625,  6, 0xB},

	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_22P5792_MHZ, 115900, 100352,103125,6, 0x10},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_16P384_MHZ,  125696, 16384, 16875, 4, 0x6E},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_24P576_MHZ,  29696,  4096,  4125,  6, 0xF},

	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_22P5792_MHZ, 61280,  1232,	 1250,  6, 0xB},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_16P384_MHZ,  106496, 1024,	 1050,	6, 0xF},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_24P576_MHZ,  106496, 1024,	 1050,	6, 0xA},

	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_22P5792_MHZ, -181895,1176,	 1125,  6, 0xA},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_16P384_MHZ,  59234,  3328,	 3375,	6, 0xD},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_24P576_MHZ,  -98304, 1152,	 1125,	6, 0x9},

	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_22P5792_MHZ, 49589,  20384, 20625,	6, 0xD},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_16P384_MHZ,  29696,  4096,	 4125,	6, 0x12},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_24P576_MHZ,  29696,  4096,	 4125,  6, 0xC},

	// new entries for APLL deviation
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_22P636_MHZ,  -66890,  22636,  22275,  6, 0x14},	//
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_16P424_MHZ,  19408,  4106,  4125,  6, 0x1B},
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_24P636_MHZ,  19408,  4106,  4125,  6, 0x12},

	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_22P636_MHZ,  93917,  39613,  40500,  6, 0xE}, //
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_16P424_MHZ,  -57204,  2053,  2025,  6, 0x14},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_24P636_MHZ,  48875,  26689,  27000,  6, 0xD},

	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_22P636_MHZ,  115908, 73567,  75600,  6, 0xD},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_16P424_MHZ,  96021, 2053,  2100,  6, 0x12},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_24P636_MHZ,  96021, 2053,  2100,  6, 0xC},

	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_22P636_MHZ,  -25199,  5659,  5625,  6, 0xC},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_16P424_MHZ,  115174,  16424,  16875,  6, 0x10},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_24P636_MHZ,  -15415,  22583,  22500,  6, 0xB},

	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_22P636_MHZ,  105084, 181088,  185625,  6, 0x10},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_16P424_MHZ,  -72193, 188876,  185625,  6, 0x17},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_24P636_MHZ,  19408,  4106,  4125,  6, 0xF},

	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_22P636_MHZ,  50601, 62249,	 63000,	6, 0xB},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_16P424_MHZ,  96021, 2053,	 2100,	6, 0xF},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_24P636_MHZ,  96021, 2053,	 2100,	6, 0xA},

	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_22P636_MHZ,  -191964, 5659,	 5400,	6, 0xA},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_16P424_MHZ,  48875, 26689,	 27000,	6, 0xD},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_24P636_MHZ,  -108279, 2053,	 2000,	6, 0x9},

	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_22P636_MHZ,  38940,  73567,	 74250,  6, 0xD},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_16P424_MHZ,  19408,  4106,	 4125,  6, 0x12},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_24P636_MHZ,  19408,  4106,	 4125,  6, 0xC},

	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_90P544_MHZ,  -66890,  22636, 22275, 6, 0x5},
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_65P696_MHZ,  -131081, 114968 , 111375, 6, 0x7},
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_98P544_MHZ,  115174, 16424, 16875, 5, 0xB},

	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_90P544_MHZ,  -25199,  5659, 5625, 5, 0x9},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_65P696_MHZ,  -57204,  2053, 2025, 6, 0x5},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_98P544_MHZ,  115174, 16424, 16875, 5, 0x8},

	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_90P544_MHZ,  -74248,  96203, 94500, 4, 0x11},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_65P696_MHZ,  2753,  47219, 47250, 4, 0x17},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_98P544_MHZ,  -172123, 8212, 7875, 5, 0x8},

	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_90P544_MHZ,  -25199,  5659, 5625, 4, 0xF},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_65P696_MHZ,  -90038,  28742, 28125, 4, 0x15},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_98P544_MHZ,  -90038,  28742, 28125, 5, 0x7},

	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_90P544_MHZ,  105084,  181088, 185625, 6, 0x4},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_65P696_MHZ,  -243948,  65696, 61875, 6, 0x6},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_98P544_MHZ,  194979,  32848, 34375, 5, 0x9},

	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_90P544_MHZ,  -25199,  5659, 5625, 5, 0x7},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_65P696_MHZ,  -172123,  8212,	7875, 6, 0x4},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_98P544_MHZ,  -68990,  26689, 26250, 4, 0xD},

	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_90P544_MHZ,  -25199,  5659, 5625, 5, 0x6},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_65P696_MHZ,  115174,  16424,	 16875,  5, 0x8},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_98P544_MHZ,  -15415,  22583, 22500,  4, 0xB},

	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_90P544_MHZ,  105084,  181088, 185625, 5, 0x8},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_65P696_MHZ,  115174,  16424,	 16875,  5, 0xB},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_98P544_MHZ,  -243948,  65686, 61875,  5, 0x8},

	//
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_22P522_MHZ,  -45999,  22522,  22275,  6, 0x14},	//
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_16P342_MHZ,  40551,  8171,  8250,  6, 0x1B},
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_24P514_MHZ,  40375,  12257,  12375,  6, 0x12},

	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_22P522_MHZ,  115623,  78827,  81000,  6, 0xE}, //
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_16P342_MHZ,  -36445,  8171,  8100,  6, 0x14},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_24P514_MHZ,  5381,  134827,  135000,  5, 0x21},

	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_22P522_MHZ,  137725, 146393,  151200,  6, 0xD},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_16P342_MHZ,  -109390, 155249,  151200,  6, 0x13},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_24P514_MHZ,  -21712, 54281,  54000,  5, 0x1F},

	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_22P522_MHZ,  -4097,  11261,  11250,  6, 0xC},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_16P342_MHZ,  -117972,  138907,  135000,  6, 0x11},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_24P514_MHZ,  5381,  134827,  135000,  6, 0xB},

	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_22P522_MHZ,  126846, 180176,  185625,  6, 0x10},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_16P342_MHZ,  -51510, 187933,  185625,  6, 0x17},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_24P514_MHZ,  40379,  12257,  12375,  6, 0xF},

	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_22P522_MHZ,  72088, 123871,	 126000,6, 0xB},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_16P342_MHZ,  117549, 8171,	 8400,	6, 0xF},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_24P514_MHZ,  117373, 1751,	 1800,	6, 0xA},

	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_22P522_MHZ,  -171705, 11261,	 10800,	6, 0xA},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_16P342_MHZ,  70166,   106223, 108000,6, 0xD},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_24P514_MHZ,  -87944, 12257,	 12000,	6, 0x9},

	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_22P522_MHZ,  60367,  146393,	 148500,  6, 0xD},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_16P342_MHZ,  40551,  8171,	 8250,  6, 0x12},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_24P514_MHZ,  40379,  12257,	 12375,  6, 0xC},

	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_90P088_MHZ,  -45999,  22522, 22275, 6, 0x5},
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_65P368_MHZ,  -110692, 114394 , 111375, 6, 0x7},
	{AVPLL_VCO_FREQ_2_227G, AUDIO_FREQ_98P056_MHZ,  136622, 49028, 50625, 5, 0xB},

	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_90P088_MHZ,  -4097,  11261, 11250, 5, 0x9},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_65P368_MHZ,  -36445,  8171, 8100, 6, 0x5},
	{AVPLL_VCO_FREQ_1_620G, AUDIO_FREQ_98P056_MHZ,  136622, 49028, 50625, 5, 0x8},

	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_90P088_MHZ,  -53393,  191437, 189000, 4, 0x11},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_65P368_MHZ,  23813,  187933, 189000, 4, 0x17},
	{AVPLL_VCO_FREQ_1_512G, AUDIO_FREQ_98P056_MHZ,  -152106, 3502, 3375, 5, 0x8},

	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_90P088_MHZ,  -4097,  11261, 11250, 4, 0xF},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_65P368_MHZ,  -69444,  57197, 56250, 4, 0x15},
	{AVPLL_VCO_FREQ_1_350G, AUDIO_FREQ_98P056_MHZ,  -69612,  85799, 84375, 5, 0x7},

	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_90P088_MHZ,  126846,  180176, 185625, 6, 0x4},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_65P368_MHZ,  -224126,  65368, 61875, 6, 0x6},
	{AVPLL_VCO_FREQ_1_856G, AUDIO_FREQ_98P056_MHZ,  216824,  98056, 103125, 5, 0x9},

	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_90P088_MHZ,  -4097,  11261, 11250, 5, 0x7},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_65P368_MHZ,  -151941,  8171,	7875, 6, 0x4},
	{AVPLL_VCO_FREQ_1_260G, AUDIO_FREQ_98P056_MHZ,  -48460,  22763, 22500, 4, 0xD},

	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_90P088_MHZ,  -4097,  11261, 11250, 5, 0x6},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_65P368_MHZ,  136798,  16342,	 16875,  5, 0x8},
	{AVPLL_VCO_FREQ_1_080G, AUDIO_FREQ_98P056_MHZ,  5381,  134827, 135000,  4, 0xB},

	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_90P088_MHZ,  126846,  180176, 185625, 5, 0x8},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_65P368_MHZ,  136798,  16342,	 16875,  5, 0xB},
	{AVPLL_VCO_FREQ_1_485G, AUDIO_FREQ_98P056_MHZ,  -224288,  196112, 185625,  5, 0x8},
};

int diag_pll_A_VCO_Setting = AVPLL_VCO_FREQ_2_227G;
int diag_pll_B_VCO_Setting = AVPLL_VCO_FREQ_1_620G;

unsigned int gAVPLLA_Channel_OutputClock[8] = {0};
unsigned int gAVPLLB_Channel_OutputClock[8] = {0};

#define OFFSET_1KPPM_INC	(-4194)
#define OFFSET_1KPPM_DEC	(4194)

int divHDMICode[] =
{
	1, 2, 4, 6
};

int divAV1Code[] =
{
	1, 2, 5, 5
};


//Yongchun Move from lower here to handle the resolution change could change the VCO.
static int avpll_inited[2] = {0, 0};
static double org_ppm[2] = {0.0, 0.0};
static double cur_ppm[2] = {0.0, 0.0};

// This formular covert the actual base ref frequency (f0) to the nominal resolution reference frequency Fr
// The VCO out and in is reversed as the PLL nornal channels. so. Fr/f0 = (1+ Offset0/P)  p= 4194304
// so: f0/Fr= 1+ ppm0 = 1/(1+ Offset/P); then ppm0 = -(Offset)/(P+offset)
static double offset_2_ppm(int offset) {
        int v = offset & 0x3ffff;

        if(offset & 0x40000) v = -v;

        return -((1000000.0 * v)/(4194304.0 + v));
}

// This is used to cover the real_ppm to nominal reference frequency to real base ref frequency.
// Fx/Fr = (1+ ppm_real) = Fx/f0*f0/Fr = (1+Offset_X/P)(1+ppm0) then Offset_X = P*(ppm_real-ppm0)/(1+ppm0)
// however the input parameter ppm = ppm0+ ppm_real(delta) so offset_X = P*(ppm -2*ppm0)/(1+ppm0)

static int ppm_2_offset(double ppm, double ppm0) {
        double c = 4194304.0 * (ppm - 2*ppm0) / (1000000.0 + ppm0);

        int offset = (int)c;

        if(offset > 524287) {
                offset = 524287;
        }else if(offset < -524287) {
                offset = -524287;
        }

        return (offset < 0) ? ((-offset) | 0x40000) : offset;
}


#if (BERLIN_CHIP_VERSION < BERLIN_BG2_Q)
void diag_setDPll_A(int enable, int p_sync1, int p_sync2, int chId)
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl30 regData30;

	double ppm_from_dpll;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), &regData30.u32);


	//disable DPll first
	regData30.uavPllCtl_avPllEnDpllC_A &= ~(1<<(chId-1));
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), regData30.u32);

	if(!p_sync1)
	{
		if(enable) dbg_printf(PRN_RES, "Warning p_sync1 is 0\n");
	}
	else
	{
		ppm_from_dpll = (double)p_sync2/p_sync1;
		dbg_printf(PRN_RES, "  DPLL PPM is %f\n", ppm_from_dpll);
	}

	switch(chId)
	{
		//set values
		case 1:
		{
			T32Gbl_avPllCtl15 regData15;
			T32Gbl_avPllCtl23 regData23;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl15), &regData15.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl23), &regData23.u32);
			regData15.uavPllCtl_avPllPSync1C1_A = p_sync1;
			regData23.uavPllCtl_avPllPSync2C1_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl15), regData15.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl23), regData23.u32);
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl16 regData16;
			T32Gbl_avPllCtl24 regData24;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl16), &regData16.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl24), &regData24.u32);
			regData16.uavPllCtl_avPllPSync1C2_A = p_sync1;
			regData24.uavPllCtl_avPllPSync2C2_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl16), regData16.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl24), regData24.u32);
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl17 regData17;
			T32Gbl_avPllCtl25 regData25;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl17), &regData17.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl25), &regData25.u32);
			regData17.uavPllCtl_avPllPSync1C3_A = p_sync1;
			regData25.uavPllCtl_avPllPSync2C3_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl17), regData17.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl25), regData25.u32);
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl18 regData18;
			T32Gbl_avPllCtl26 regData26;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl18), &regData18.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl26), &regData26.u32);
			regData18.uavPllCtl_avPllPSync1C4_A = p_sync1;
			regData26.uavPllCtl_avPllPSync2C4_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl18), regData18.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl26), regData26.u32);
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl19 regData19;
			T32Gbl_avPllCtl27 regData27;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl19), &regData19.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl27), &regData27.u32);
			regData19.uavPllCtl_avPllPSync1C5_A = p_sync1;
			regData27.uavPllCtl_avPllPSync2C5_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl19), regData19.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl27), regData27.u32);
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl20 regData20;
			T32Gbl_avPllCtl28 regData28;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl20), &regData20.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl28), &regData28.u32);
			regData20.uavPllCtl_avPllPSync1C6_A = p_sync1;
			regData28.uavPllCtl_avPllPSync2C6_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl20), regData20.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl28), regData28.u32);
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl21 regData21;
			T32Gbl_avPllCtl29 regData29;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl21), &regData21.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl29), &regData29.u32);
			regData21.uavPllCtl_avPllPSync1C7_A = p_sync1;
			regData29.uavPllCtl_avPllPSync2C7_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl21), regData21.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl29), regData29.u32);
		}
		break;
		case 8:
		{
			T32Gbl_avPllCtl22 regData22;
			T32Gbl_avPllCtl30 regData30;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl22), &regData22.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), &regData30.u32);
			regData22.uavPllCtl_avPllPSync1C8_A = p_sync1;
			regData30.uavPllCtl_avPllPSync2C8_A = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl22), regData22.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), regData30.u32);
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid channel\n");
		break;
	}

	if(enable)
	{
		//enable DPLL
		regData30.uavPllCtl_avPllEnDpllC_A |= (1<<(chId-1));

		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), regData30.u32);
	}
#endif
}

void diag_setDPll_B(int enable, int p_sync1, int p_sync2, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	T32Gbl_avPllCtl61 regData61;
	double ppm_from_dpll;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), &regData61.u32);

	//disable DPLL first
	regData61.uavPllCtl_avPllEnDpllC_B &= ~(1<<(chId-1));
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), regData61.u32);

	if(!p_sync1)
	{
		if(enable) dbg_printf(PRN_RES, "Warning p_sync1 is 0\n");
	}
	else
	{
		ppm_from_dpll = (double)p_sync2/p_sync1;
		dbg_printf(PRN_RES, "  DPLL PPM is %f\n", ppm_from_dpll);
	}

	switch(chId)
	{
		//set values
		case 1:
		{
			T32Gbl_avPllCtl46 regData46;
			T32Gbl_avPllCtl54 regData54;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl46), &regData46.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl54), &regData54.u32);
			regData46.uavPllCtl_avPllPSync1C1_B = p_sync1;
			regData54.uavPllCtl_avPllPSync2C1_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl46), regData46.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl54), regData54.u32);
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl47 regData47;
			T32Gbl_avPllCtl55 regData55;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl47), &regData47.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl55), &regData55.u32);
			regData47.uavPllCtl_avPllPSync1C2_B = p_sync1;
			regData55.uavPllCtl_avPllPSync2C2_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl47), regData47.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl55), regData55.u32);
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl48 regData48;
			T32Gbl_avPllCtl56 regData56;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl48), &regData48.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl56), &regData56.u32);
			regData48.uavPllCtl_avPllPSync1C3_B = p_sync1;
			regData56.uavPllCtl_avPllPSync2C3_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl48), regData48.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl56), regData56.u32);
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl49 regData49;
			T32Gbl_avPllCtl57 regData57;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl49), &regData49.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl57), &regData57.u32);
			regData49.uavPllCtl_avPllPSync1C4_B = p_sync1;
			regData57.uavPllCtl_avPllPSync2C4_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl49), regData49.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl57), regData57.u32);
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl50 regData50;
			T32Gbl_avPllCtl58 regData58;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl50), &regData50.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl58), &regData58.u32);
			regData50.uavPllCtl_avPllPSync1C5_B = p_sync1;
			regData58.uavPllCtl_avPllPSync2C5_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl50), regData50.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl58), regData58.u32);
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl51 regData51;
			T32Gbl_avPllCtl59 regData59;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl51), &regData51.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl59), &regData59.u32);
			regData51.uavPllCtl_avPllPSync1C6_B = p_sync1;
			regData59.uavPllCtl_avPllPSync2C6_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl51), regData51.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl59), regData59.u32);
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl52 regData52;
			T32Gbl_avPllCtl60 regData60;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl52), &regData52.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl60), &regData60.u32);
			regData52.uavPllCtl_avPllPSync1C7_B = p_sync1;
			regData60.uavPllCtl_avPllPSync2C7_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl52), regData52.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl60), regData60.u32);
		}
		break;
		case 8:
		{
			T32Gbl_avPllCtl53 regData53;
			T32Gbl_avPllCtl61 regData61;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl53), &regData53.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), &regData61.u32);
			regData53.uavPllCtl_avPllPSync1C8_B = p_sync1;
			regData61.uavPllCtl_avPllPSync2C8_B = p_sync2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl53), regData53.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), regData61.u32);
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid channel\n");
		break;
	}

	if(enable)
	{
		//re-enable DPLL
		regData61.uavPllCtl_avPllEnDpllC_B |= (1<<(chId-1));
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), regData61.u32);
	}
#endif
}

void diag_setVCO_A(int vco_freq_index)
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl reg;
	T32Gbl_avPllCtl1 regData1;
	T32Gbl_avPllCtl9 regData9;

	dbg_printf(PRN_INFO, "PLLA %d VCO freq change to %f\n", diag_vcoFreqs[vco_freq_index]);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), &reg.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), &regData1.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData9.u32);

	//following three settings are done under reset to improve long term reliability
	diag_assertReset_A();
	regData1.uavPllCtl_avPllFbDiv_A = diag_avpllRegFBDIV[vco_freq_index];
	reg.uavPllCtl_avPllSpeed_A = diag_avpllRegSPEED[vco_freq_index];
	reg.uavPllCtl_avPllIntpi_A = diag_avpllRegINTPI[vco_freq_index];
	diag_deassertReset_A();

	regData9.uavPllCtl_avPllFreqOffsetC8_A = diag_avpllRegFREQ_OFFSET_C8[vco_freq_index];

	diag_pll_A_VCO_Setting = vco_freq_index;

    cur_ppm[0] = org_ppm[0] = offset_2_ppm(regData9.uavPllCtl_avPllFreqOffsetC8_A);
    avpll_inited[0] = 1;

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), reg.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), regData1.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData9.u32);

	//toggle the offset_rdy bit
	{
		volatile int i;

		T32Gbl_avPllCtl9 regData;
		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_A = (1<<7);
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData.u32);

		//add some delay because need to be asserted by 30ns
		for(i=0; i<10000; i++);

		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_A = 0;
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData.u32);
	}
#endif
}

void diag_setVCO_B(int vco_freq_index)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	T32Gbl_avPllCtl31 regData31;
	T32Gbl_avPllCtl32 regData32;
	T32Gbl_avPllCtl40 regData40;

	dbg_printf(PRN_INFO, "PLLB %d VCO freq change to %f\n", diag_vcoFreqs[vco_freq_index]);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), &regData31.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), &regData32.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData40.u32);

	//following three settings are done under reset to improve long term reliability
	diag_assertReset_B();
	regData32.uavPllCtl_avPllFbDiv_B = diag_avpllRegFBDIV[vco_freq_index];
	regData31.uavPllCtl_avPllSpeed_B = diag_avpllRegSPEED[vco_freq_index];
	regData31.uavPllCtl_avPllIntpi_B = diag_avpllRegINTPI[vco_freq_index];
	diag_deassertReset_B();

	regData40.uavPllCtl_avPllFreqOffsetC8_B = diag_avpllRegFREQ_OFFSET_C8[vco_freq_index];

	diag_pll_B_VCO_Setting = vco_freq_index;

    cur_ppm[1] = org_ppm[1] = offset_2_ppm(regData40.uavPllCtl_avPllFreqOffsetC8_B);
    avpll_inited[1] = 1;

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData40.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), regData32.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), regData31.u32);

	//toggle the offset_rdy bit
	{
		volatile int i;

		T32Gbl_avPllCtl40 regData;
		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_B = (1<<7);
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData.u32);

		//add some delay because need to be asserted by 30ns
		for(i=0; i<10000; i++);

		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_B = 0;
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData.u32);
	}
#endif
}

void diag_setChanOffset_A(int offset, int chId)
{
#ifndef FPGA_TAILORED_BOARDVER
	unsigned int reg_offset;
	double ppm_from_offset;

	if(offset<0)
	{
		reg_offset = (1<<18) | (-offset) ;
	}
	else
	{
		reg_offset = offset;
	}

	dbg_printf(PRN_INFO, "set A%d offset to 0x%x\n", chId, reg_offset);

	if(offset != 0)
	{
		ppm_from_offset = 1/((double)offset/4194304+1);
		dbg_printf(PRN_RES, "offset PPM is %f\n", ppm_from_offset);
	}

	//set offset register
	switch(chId)
	{
		case 1:
		{
			T32Gbl_avPllCtl2 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl2), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC1_A = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl2), regData.u32);
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl3 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl3), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC2_A = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl3), regData.u32);
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl4 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl4), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC3_A = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl4), regData.u32);
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl5 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl5), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC4_A = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl5), regData.u32);
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl6 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl6), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC5_A = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl6), regData.u32);
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl7 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl7), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC6_A = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl7), regData.u32);
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl8 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl8), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC7_A = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl8), regData.u32);
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invlid Channel ID (1 to 7 only)\n");
		break;
	}

	//toggle the offset_rdy bit
	{
		volatile int i;

		T32Gbl_avPllCtl9 regData;
		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_A = (1<<(chId-1));
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData.u32);

		//add some delay because need to be asserted by 30ns
		for(i=0; i<10000; i++);

		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_A = 0;
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData.u32);
	}
#endif
}

void diag_setChanOffset_B(int offset, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	unsigned int reg_offset;
	double ppm_from_offset;

	if(offset<0)
	{
		reg_offset = (1<<18) | (-offset) ;
	}
	else
	{
		reg_offset = offset;
	}

	dbg_printf(PRN_INFO, "set A%d offset to 0x%x\n", chId, reg_offset);

	if(offset != 0)
	{
		ppm_from_offset = 1/((double)offset/4194304+1);
		dbg_printf(PRN_RES, "offset PPM is %f\n", ppm_from_offset);
	}

	//set offset register
	switch(chId)
	{
		case 1:
		{
			T32Gbl_avPllCtl33 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl33), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC1_B = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl33), regData.u32);
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl34 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl34), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC2_B = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl34), regData.u32);
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl35 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl35), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC3_B = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl35), regData.u32);
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl36 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl36), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC4_B = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl36), regData.u32);
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl37 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl37), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC5_B = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl37), regData.u32);
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl38 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl38), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC6_B = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl38), regData.u32);
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl39 regData;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl39), &regData.u32);
			regData.uavPllCtl_avPllFreqOffsetC7_B = reg_offset;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl39), regData.u32);
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invlid Channel ID (1 to 7 only)\n");
		break;
	}

	//toggle the offset_rdy bit
	{
		volatile int i;

		T32Gbl_avPllCtl40 regData;
		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_B = (1<<(chId-1));
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData.u32);

		//add some delay because need to be asserted by 30ns
		for(i=0; i<10000; i++);

		BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData.u32);

		regData.uavPllCtl_avPllFreqOffsetReadyC_B = 0;
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData.u32);
	}
#endif
}

void diag_setHDMI_Div_A(int divHDMI, int chId)
{
#ifndef FPGA_TAILORED_BOARDVER
	unsigned int div_reg;
	T32Gbl_avPllCtl11 regData;

	switch(divHDMI)
	{
		case 0:
			div_reg = 0; //disable DIV_HDMI
			break;
		case 1:
			div_reg = 4; //div by 1, by pass
			break;
		case 2:
			div_reg = 5; //div by 2
			break;
		case 4:
			div_reg = 6; //div by 4
			break;
		case 6:
			div_reg = 7; //div by 6
			break;
		default:
			{
				dbg_printf(PRN_RES, "Invalid divider for HDMI\n");
				return ;
			}
	}

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl11), &regData.u32);

	switch(chId)
	{
		case 1:
			regData.uavPllCtl_avPllDivHdmiC1_A = div_reg;
		break;
		case 2:
			regData.uavPllCtl_avPllDivHdmiC2_A = div_reg;
		break;
		case 3:
			regData.uavPllCtl_avPllDivHdmiC3_A = div_reg;
		break;
		case 4:
			regData.uavPllCtl_avPllDivHdmiC4_A = div_reg;
		break;
		case 5:
			regData.uavPllCtl_avPllDivHdmiC5_A = div_reg;
		break;
		case 6:
			regData.uavPllCtl_avPllDivHdmiC6_A = div_reg;
		break;
		case 7:
			regData.uavPllCtl_avPllDivHdmiC7_A = div_reg;
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel Id\n");
		break;
	}

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl11), regData.u32);
#endif
}

void diag_setHDMI_Div_B(int divHDMI, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	unsigned int div_reg;
	T32Gbl_avPllCtl42 regData;

	switch(divHDMI)
	{
		case 0:
			div_reg = 0; //disable DIV_HDMI
			break;
		case 1:
			div_reg = 4; //div by 1, by pass
			break;
		case 2:
			div_reg = 5; //div by 2
			break;
		case 4:
			div_reg = 6; //div by 4
			break;
		case 6:
			div_reg = 7; //div by 6
			break;
		default:
			{
				dbg_printf(PRN_RES, "Invalid divider for HDMI\n");
				return ;
			}
	}

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl42), &regData.u32);

	switch(chId)
	{
		case 1:
			regData.uavPllCtl_avPllDivHdmiC1_B = div_reg;
		break;
		case 2:
			regData.uavPllCtl_avPllDivHdmiC2_B = div_reg;
		break;
		case 3:
			regData.uavPllCtl_avPllDivHdmiC3_B = div_reg;
		break;
		case 4:
			regData.uavPllCtl_avPllDivHdmiC4_B = div_reg;
		break;
		case 5:
			regData.uavPllCtl_avPllDivHdmiC5_B = div_reg;
		break;
		case 6:
			regData.uavPllCtl_avPllDivHdmiC6_B = div_reg;
		break;
		case 7:
			regData.uavPllCtl_avPllDivHdmiC7_B = div_reg;
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel Id\n");
		break;
	}

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl42), regData.u32);
#endif
}

//divAV1 is the register value, not the actual divider
//mapping is (binary):
//000 = bypass, the whole divAV channel will be bypassed
//100 = div by 1
//101 = div by 2
//110 = div by 5
//111 = div by 5
void diag_setAV_Div_A(int divAV1, int divAV2, int chId)
{
#ifndef FPGA_TAILORED_BOARDVER
	if(divAV2 !=0 && (divAV2<4 || divAV2>127))
	{
		dbg_printf(PRN_RES, "Invalid divider for AV1\n");
			return ;
	}


	//program DIV_AV1 and DIV_AV2
	switch(chId)
	{
		case 1:
		{
			T32Gbl_avPllCtl11 regData11;
			T32Gbl_avPllCtl12 regData12;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl11), &regData11.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
			regData11.uavPllCtl_avPllDivAv1C1_A = divAV1;
			regData12.uavPllCtl_avPllDivAv2C1_A = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl11), regData11.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl12 regData12;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
			regData12.uavPllCtl_avPllDivAv1C2_A = divAV1;
			regData12.uavPllCtl_avPllDivAv2C2_A = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl12 regData12;
			T32Gbl_avPllCtl13 regData13;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), &regData13.u32);
			regData12.uavPllCtl_avPllDivAv1C3_A = divAV1;
			regData13.uavPllCtl_avPllDivAv2C3_A = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), regData13.u32);
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl12 regData12;
			T32Gbl_avPllCtl13 regData13;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), &regData13.u32);
			regData12.uavPllCtl_avPllDivAv1C4_A = divAV1;
			regData13.uavPllCtl_avPllDivAv2C4_A = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), regData13.u32);
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl12 regData12;
			T32Gbl_avPllCtl13 regData13;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), &regData13.u32);
			regData12.uavPllCtl_avPllDivAv1C5_A = divAV1;
			regData13.uavPllCtl_avPllDivAv2C5_A = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), regData13.u32);
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl12 regData12;
			T32Gbl_avPllCtl13 regData13;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), &regData13.u32);
			regData12.uavPllCtl_avPllDivAv1C6_A = divAV1;
			regData13.uavPllCtl_avPllDivAv2C6_A = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), regData13.u32);
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl12 regData12;
			T32Gbl_avPllCtl14 regData14;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl14), &regData14.u32);
			regData12.uavPllCtl_avPllDivAv1C7_A = divAV1;
			regData14.uavPllCtl_avPllDivAv2C7_A = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl14), regData14.u32);
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel Id\n");
		break;
	}
#endif
}

//divAV1 is the register value, not the actual divider
//mapping is (binary):
//000 = bypass
//100 = div by 1
//101 = div by 2
//110 = div by 5
//111 = div by 5
void diag_setAV_Div_B(int divAV1, int divAV2, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	if(divAV2 !=0 && (divAV2<4 || divAV2>127))
	{
		dbg_printf(PRN_RES, "Invalid divider for AV1\n");
			return ;
	}


	//program DIV_AV1 and DIV_AV2
	switch(chId)
	{
		case 1:
		{
			T32Gbl_avPllCtl42 regData42;
			T32Gbl_avPllCtl43 regData43;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl42), &regData42.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
			regData42.uavPllCtl_avPllDivAv1C1_B = divAV1;
			regData43.uavPllCtl_avPllDivAv2C1_B = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl42), regData42.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl43 regData43;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
			regData43.uavPllCtl_avPllDivAv1C2_B = divAV1;
			regData43.uavPllCtl_avPllDivAv2C2_B = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl43 regData43;
			T32Gbl_avPllCtl44 regData44;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), &regData44.u32);
			regData43.uavPllCtl_avPllDivAv1C3_B = divAV1;
			regData44.uavPllCtl_avPllDivAv2C3_B = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), regData44.u32);
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl43 regData43;
			T32Gbl_avPllCtl44 regData44;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), &regData44.u32);
			regData43.uavPllCtl_avPllDivAv1C4_B = divAV1;
			regData44.uavPllCtl_avPllDivAv2C4_B = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), regData44.u32);
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl43 regData43;
			T32Gbl_avPllCtl44 regData44;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), &regData44.u32);
			regData43.uavPllCtl_avPllDivAv1C5_B = divAV1;
			regData44.uavPllCtl_avPllDivAv2C5_B = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), regData44.u32);
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl43 regData43;
			T32Gbl_avPllCtl44 regData44;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), &regData44.u32);
			regData43.uavPllCtl_avPllDivAv1C6_B = divAV1;
			regData44.uavPllCtl_avPllDivAv2C6_B = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), regData44.u32);
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl43 regData43;
			T32Gbl_avPllCtl45 regData45;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl45), &regData45.u32);
			regData43.uavPllCtl_avPllDivAv1C7_B = divAV1;
			regData45.uavPllCtl_avPllDivAv2C7_B = divAV2;
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
			BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl45), regData45.u32);
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel Id\n");
		break;
	}
#endif
}

//Input is the hex code for AV2 and AV3, see table III
void diag_setAV3_Div_A(int divAV2, int divAV3, int chId)
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl12 regData12;
	T32Gbl_avPllCtl13 regData13;
	T32Gbl_avPllCtl14 regData14;
	T32Gbl_avPllCtl15 regData15;
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), &regData13.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl14), &regData14.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl15), &regData15.u32);


	//program DIV_AV1 and DIV_AV2
	switch(chId)
	{
		case 1:
		{
			regData12.uavPllCtl_avPllDivAv2C1_A = divAV2;
			regData14.uavPllCtl_avPllDivAv3C1_A = divAV3;
		}
		break;
		case 2:
		{
			regData12.uavPllCtl_avPllDivAv2C2_A = divAV2;
			regData14.uavPllCtl_avPllDivAv3C2_A = divAV3;
		}
		break;
		case 3:
		{
			regData13.uavPllCtl_avPllDivAv2C3_A = divAV2;
			regData14.uavPllCtl_avPllDivAv3C3_A = divAV3;
		}
		break;
		case 4:
		{
			regData13.uavPllCtl_avPllDivAv2C4_A = divAV2;
			regData14.uavPllCtl_avPllDivAv3C4_A = divAV3;
		}
		break;
		case 5:
		{
			regData13.uavPllCtl_avPllDivAv2C5_A = divAV2;
			regData14.uavPllCtl_avPllDivAv3C5_A = divAV3;			}
		break;
		case 6:
		{
			regData13.uavPllCtl_avPllDivAv2C6_A = divAV2;
			regData14.uavPllCtl_avPllDivAv3C6_A = divAV3;			}
		break;
		case 7:
		{
			regData14.uavPllCtl_avPllDivAv2C7_A = divAV2;
			regData15.uavPllCtl_avPllDivAv3C7_A = divAV3;			}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel Id\n");
		break;
	}

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), regData12.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), regData13.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl14), regData14.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl15), regData15.u32);
#endif
}

void diag_setAV3_Div_B(int divAV2, int divAV3, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	T32Gbl_avPllCtl43 regData43;
	T32Gbl_avPllCtl44 regData44;
	T32Gbl_avPllCtl45 regData45;
	T32Gbl_avPllCtl46 regData46;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), &regData44.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl45), &regData45.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl46), &regData46.u32);

	switch(chId)
	{
		case 1:
		{
			regData43.uavPllCtl_avPllDivAv2C1_B = divAV2;
			regData45.uavPllCtl_avPllDivAv3C1_B = divAV3;
		}
		break;
		case 2:
		{
			regData43.uavPllCtl_avPllDivAv2C2_B = divAV2;
			regData45.uavPllCtl_avPllDivAv3C2_B = divAV3;
		}
		break;
		case 3:
		{
			regData44.uavPllCtl_avPllDivAv2C3_B = divAV2;
			regData45.uavPllCtl_avPllDivAv3C3_B = divAV3;
		}
		break;
		case 4:
		{
			regData44.uavPllCtl_avPllDivAv2C4_B = divAV2;
			regData45.uavPllCtl_avPllDivAv3C4_B = divAV3;
		}
		break;
		case 5:
		{
			regData44.uavPllCtl_avPllDivAv2C5_B = divAV2;
			regData45.uavPllCtl_avPllDivAv3C5_B = divAV3;
		}
		break;
		case 6:
		{
			regData44.uavPllCtl_avPllDivAv2C6_B = divAV2;
			regData45.uavPllCtl_avPllDivAv3C6_B = divAV3;
		}
		break;
		case 7:
		{
			regData45.uavPllCtl_avPllDivAv2C7_B = divAV2;
			regData46.uavPllCtl_avPllDivAv3C7_B = divAV3;
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel Id\n");
		break;
	}

	//write divAV2, divAV3 back
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), regData43.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), regData44.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl45), regData45.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl46), regData46.u32);
#endif
}


//this is done by using offset and dpll
//inc=1 means increase freq to 1001/1000
//inc=0 means disable 1KPPM (by setting offset to 0 and disable DPLL)
//inc=-1 means decrease freq to 1000/1001
void diag_set1KPPM_A(int inc, int chId)
{
	if(inc)
	{
		if(inc>0)
		{
			//increase by 1KPPM
			diag_setChanOffset_A(OFFSET_1KPPM_INC, chId);
			diag_setDPll_A(1, 1000, 1001, chId);
		}
		else
		{
			//decrease by 1KPPM
			diag_setChanOffset_A(OFFSET_1KPPM_DEC, chId);
			diag_setDPll_A(1, 1001, 1000, chId);
		}
	}
	else
	{
		//set offset to 0 and disable DPLL
		diag_setChanOffset_A(0, chId);
		diag_setDPll_A(0, 0, 0, chId);
	}
}

void diag_set1KPPM_B(int inc, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	if(inc)
	{
		if(inc>0)
		{
			//increase by 1KPPM
			diag_setChanOffset_B(OFFSET_1KPPM_INC, chId);
			diag_setDPll_B(1, 1000, 1001, chId);
		}
		else
		{
			//decrease by 1KPPM
			diag_setChanOffset_B(OFFSET_1KPPM_DEC, chId);
			diag_setDPll_B(1, 1001, 1000, chId);
		}
	}
	else
	{
		//set offset to 0 and disable DPLL
		diag_setChanOffset_B(0, chId);
		diag_setDPll_B(0, 0, 0, chId);
	}
#endif
}

/* new reference in MHz, chan : chan_A 0, Chan_B 1 */
void diag_changeRefFreq(int vco_freq_index, int grp)
{
	if(grp ==0)
	{
		diag_setVCO_A(vco_freq_index);
	}
	else
	{
		diag_setVCO_B(vco_freq_index);
	}
}

void diag_setMasterSlaveB(int MasterSlaveB_A, int MasterSlaveB_B)
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl30 regData30;
	T32Gbl_avPllCtl61 regData61;
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), &regData30.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), &regData61.u32);

	regData30.uavPllCtl_avPllMasterSlaveB_A = MasterSlaveB_A;
	regData61.uavPllCtl_avPllMasterSlaveB_B = MasterSlaveB_B;

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), regData30.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), regData61.u32);
#endif
}

void diag_assertReset_A()
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl reg;

	//assert reset
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), &reg.u32);
	reg.uavPllCtl_avPllResetPll_A=1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), reg.u32);
#endif
}

void diag_deassertReset_A()
{
#ifndef FPGA_TAILORED_BOARDVER
	volatile int i;

	T32Gbl_avPllCtl reg;

	for(i=0; i<10000; i++);

	//deassert reset
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), &reg.u32);
	reg.uavPllCtl_avPllResetPll_A=0;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), reg.u32);
#endif
}

void diag_assertReset_B()
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	T32Gbl_avPllCtl31 reg;

	//assert reset
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), &reg.u32);
	reg.uavPllCtl_avPllResetPll_B=1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), reg.u32);
#endif
}

void diag_deassertReset_B()
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	volatile int i;

	T32Gbl_avPllCtl31 reg;

	for(i=0; i<10000; i++);

	//deassert reset
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), &reg.u32);
	reg.uavPllCtl_avPllResetPll_B=0;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), reg.u32);
#endif
}

void diag_powerDown_A()
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl regData;
	T32Gbl_avPllCtl10 regData10;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), &regData.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl10), &regData10.u32);
	regData.uavPllCtl_avPllPu_A = 0;
	regData10.uavPllCtl_avPllPuC_A = 0;

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), regData.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl10), regData10.u32);
#endif
}

void diag_powerUp_A()
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl regData;
	T32Gbl_avPllCtl10 regData10;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), &regData.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl10), &regData10.u32);
	regData.uavPllCtl_avPllPu_A = 1;
	regData10.uavPllCtl_avPllPuC_A = 0x7f;

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), regData.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl10), regData10.u32);
#endif
}

void diag_powerDown_B()
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	T32Gbl_avPllCtl31 regData31;
	T32Gbl_avPllCtl41 regData41;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), &regData31.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl41), &regData41.u32);
	regData31.uavPllCtl_avPllPu_B = 0;
	regData41.uavPllCtl_avPllPuC_B = 0;

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), regData31.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl41), regData41.u32);
#endif
}

void diag_powerUp_B()
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	T32Gbl_avPllCtl31 regData31;
	T32Gbl_avPllCtl41 regData41;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), &regData31.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl41), &regData41.u32);
	regData31.uavPllCtl_avPllPu_B = 1;
	regData41.uavPllCtl_avPllPuC_B = 0x7f;

	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), regData31.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl41), regData41.u32);
#endif
}

//this function will enable AVPLL A and B and turn on caliberation
void AVPLL_Enable(void)
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl31 regData31;
	T32Gbl_avPllCtl32 regData32;

	T32Gbl_avPllCtl12 regData12;
	T32Gbl_avPllCtl1 regData1;
	T32Gbl_avPllCtl regData;
	T32Gbl_clkEnable  regClkEnable;
	T32Gbl_PadSelect  regPadSelect;
	T32Gbl_avPllCtl11 regData11;
	T32Gbl_avPllCtl42 regData42;
	T32Gbl_avPllCtl9 regData9;
	T32Gbl_avPllCtl10 regData10;
	T32Gbl_avPllCtl40 regData40;
	T32Gbl_avPllCtl41 regData41;

	volatile int i;
	int avpll_initialized=0;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), &regData31.u32);
 	//speed is not defaut value, then pll must has been initialized.
	if(regData31.uavPllCtl_avPllSpeed_B != diag_avpllRegSPEED[0])
		avpll_initialized=1;
	//turn on the reset_A and reset_B
	//these should be always on
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl11), &regData11.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl42), &regData42.u32);
	regData11.uavPllCtl_avPllResetC_A = 0;
	regData42.uavPllCtl_avPllResetC_B = 0;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl11), regData11.u32);
	if(!avpll_initialized)
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl42), regData42.u32);

	//turn on offset
	//these should be always on
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData9.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl10), &regData10.u32);
	regData9.uavPllCtl_avPllReserveC1_A = 1;
	regData9.uavPllCtl_avPllReserveC2_A = 1;
	regData10.uavPllCtl_avPllReserveC3_A = 1;
	regData10.uavPllCtl_avPllReserveC4_A = 1;
	regData10.uavPllCtl_avPllReserveC5_A = 1;
	regData10.uavPllCtl_avPllReserveC6_A = 1;
	regData10.uavPllCtl_avPllReserveC7_A = 1;
	regData10.uavPllCtl_avPllReserveC8_A = 1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData9.u32);
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl10), regData10.u32);

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData40.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl41), &regData41.u32);
	regData40.uavPllCtl_avPllReserveC1_B = 1;
	regData40.uavPllCtl_avPllReserveC2_B = 1;
	regData41.uavPllCtl_avPllReserveC3_B = 1;
	regData41.uavPllCtl_avPllReserveC4_B = 1;
	regData41.uavPllCtl_avPllReserveC5_B = 1;
	regData41.uavPllCtl_avPllReserveC6_B = 1;
	regData41.uavPllCtl_avPllReserveC7_B = 1;
	regData41.uavPllCtl_avPllReserveC8_B = 1;
	if(!avpll_initialized) {
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData40.u32);
		BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl41), regData41.u32);
	}

        //set p9v to correct value
        BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), &regData.u32);
        BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), &regData31.u32);
#if (BERLIN_CHIP_VERSION == BERLIN_BG2_CD) || (BERLIN_CHIP_VERSION == BERLIN_BG2CDP)
        regData.uavPllCtl_avPllVddr1p45V_A= 1;
#endif /* (BERLIN_CHIP_VERSION == BERLIN_BG2_CD) */
        regData.uavPllCtl_avPllVddr0p9V_A = 0xA;
        regData.uavPllCtl_avPllVthVcoCal_A = 0x2;
        regData31.uavPllCtl_avPllVddr0p9V_B = 0xA;
        regData31.uavPllCtl_avPllVthVcoCal_B = 0x2;
        BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl), regData.u32);
	if(!avpll_initialized)
        	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl31), regData31.u32);

        //set ICP to correct value
        BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), &regData1.u32);
        BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), &regData32.u32);
        regData1.uavPllCtl_avPllIcp_A = 0x5;
        regData32.uavPllCtl_avPllIcp_B = 0x5;
        BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), regData1.u32);
	if(!avpll_initialized)
        	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), regData32.u32);

	diag_assertReset_A();
	if(!avpll_initialized)
		diag_assertReset_B();

	diag_setVCO_A(AVPLL_VCO_FREQ_2_227G);
	if(!avpll_initialized)
		diag_setVCO_B(AVPLL_VCO_FREQ_1_620G);

	//power up
	diag_powerUp_A();
	if(!avpll_initialized)
		diag_powerUp_B();

	//reset, has delay inside function
	diag_deassertReset_A();
	if(!avpll_initialized)
		diag_deassertReset_B();

	//caliberate
	diag_calibrate_A();
	if(!avpll_initialized)
		diag_calibrate_B();

	//set up VCO frequency.
	diag_setVCO_A(AVPLL_VCO_FREQ_2_227G);
	if(!avpll_initialized)
		diag_setVCO_B(AVPLL_VCO_FREQ_1_620G);

#if (BERLIN_CHIP_VERSION == BERLIN_BG2_CD) || (BERLIN_CHIP_VERSION == BERLIN_BG2CDP)
        dbg_printf(PRN_RES, "Power down AVPLL B\n");
        diag_powerDown_B();
#endif /* (BERLIN_CHIP_VERSION == BERLIN_BG2_CD) */

	dbg_printf(PRN_RES, "Enabled AvPLL A and B Channels\n");

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkEnable), &regClkEnable.u32);
	regClkEnable.uclkEnable_audio0ClkEn = 1;
	regClkEnable.uclkEnable_audio1ClkEn = 1;
	regClkEnable.uclkEnable_audio2ClkEn = 1;
	regClkEnable.uclkEnable_audio3ClkEn = 1;
	regClkEnable.uclkEnable_audioHdClkEn = 1;
	regClkEnable.uclkEnable_video0ClkEn = 1;
	regClkEnable.uclkEnable_video1ClkEn = 1;
	regClkEnable.uclkEnable_video2ClkEn = 1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkEnable), regClkEnable.u32);

	dbg_printf(PRN_RES, "Enable DVIO Pad\n");
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_PadSelect), &regPadSelect.u32);
	regPadSelect.uPadSelect_DVIO_OEN = 1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_PadSelect), regPadSelect.u32);

        for (i=0; i<8; i++) {
            gAVPLLA_Channel_OutputClock[i] = 0;
            gAVPLLB_Channel_OutputClock[i] = 0;
        }

#if (BERLIN_CHIP_VERSION >= BERLIN_BG2_A0)
        diag_videoFreq_A(5, 0, 0, 1, 6); // set default video0clk to 148.5M
        diag_videoFreq_A(5, 0, 0, 1, 5); // set default video1clk to 148.5M
        diag_videoFreq_B(1, 2, 0, 1, 6); // set default video1clk to 148.5M
#else
        diag_videoFreq_A(5, 0, 0, 1, 1); // set default video0clk to 148.5M
        diag_videoFreq_A(5, 0, 0, 1, 2); // set default video1clk to 148.5M
        diag_videoFreq_B(1, 2, 0, 1, 1); // set default video1clk to 148.5M
#endif
#endif	// FPGA_TAILORED_BOARDVER
}

void diag_calibrate_A(void)
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_avPllCtl1 regData1;
	volatile int i;

	//turn off caliberation
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), &regData1.u32);
	regData1.uavPllCtl_avPllPllCaliStart_A=0;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), regData1.u32);

	//add some delay
	for(i=0; i<10000; i++);

	//enable caliberation
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), &regData1.u32);
	regData1.uavPllCtl_avPllPllCaliStart_A=1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), regData1.u32);
#endif
}

void diag_calibrate_B(void)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	T32Gbl_avPllCtl32 regData32;
	volatile int i;

	//turn off caliberation
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), &regData32.u32);
	regData32.uavPllCtl_avPllPllCaliStart_B=0;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), regData32.u32);

	//add some delay
	for(i=0; i<10000; i++);

	//enable caliberation
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), &regData32.u32);
	regData32.uavPllCtl_avPllPllCaliStart_B=1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), regData32.u32);
#endif
}

void diag_avpllFreq_A(int chId)
{
#ifndef FPGA_TAILORED_BOARDVER
	double vco_freq=diag_vcoFreqs[diag_pll_A_VCO_Setting];
	double freq_from_dpll, freq_from_offset;
	int offset, sync1, sync2, divHDMI, divAV1, divAV2, divAV3;
	int enDPLL;

	T32Gbl_avPllCtl11 regData11;
	T32Gbl_avPllCtl12 regData12;
	T32Gbl_avPllCtl13 regData13;
	T32Gbl_avPllCtl14 regData14;
	T32Gbl_avPllCtl15 regData15;
	T32Gbl_avPllCtl30 regData30;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl11), &regData11.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl12), &regData12.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl13), &regData13.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl14), &regData14.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl15), &regData15.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl30), &regData30.u32);

	enDPLL = regData30.uavPllCtl_avPllEnDpllC_A & (1<<(chId-1));

	//find offset, sync1, sync2, divHDMI, divAV1, divAV2, divAV3
	switch(chId)
	{
		case 1:
		{
			T32Gbl_avPllCtl2 regData2;
			T32Gbl_avPllCtl23 regData23;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl2), &regData2.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl23), &regData23.u32);

			offset = regData2.uavPllCtl_avPllFreqOffsetC1_A;
			sync1 =  regData15.uavPllCtl_avPllPSync1C1_A;
			sync2 =  regData23.uavPllCtl_avPllPSync2C1_A;
			divHDMI= regData11.uavPllCtl_avPllDivHdmiC1_A;
			divAV1 = regData11.uavPllCtl_avPllDivAv1C1_A;
			divAV2 = regData12.uavPllCtl_avPllDivAv2C1_A;
			divAV3 = regData14.uavPllCtl_avPllDivAv3C1_A;
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl3 regData3;
			T32Gbl_avPllCtl16 regData16;
			T32Gbl_avPllCtl24 regData24;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl3), &regData3.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl16), &regData16.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl24), &regData24.u32);

			offset = regData3.uavPllCtl_avPllFreqOffsetC2_A;
			sync1 =  regData16.uavPllCtl_avPllPSync1C2_A;
			sync2 =  regData24.uavPllCtl_avPllPSync2C2_A;
			divHDMI= regData11.uavPllCtl_avPllDivHdmiC2_A;
			divAV1 = regData12.uavPllCtl_avPllDivAv1C2_A;
			divAV2 = regData12.uavPllCtl_avPllDivAv2C2_A;
			divAV3 = regData14.uavPllCtl_avPllDivAv3C2_A;
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl4 regData4;
			T32Gbl_avPllCtl17 regData17;
			T32Gbl_avPllCtl25 regData25;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl4), &regData4.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl17), &regData17.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl25), &regData25.u32);
			offset = regData4.uavPllCtl_avPllFreqOffsetC3_A;
			sync1 =  regData17.uavPllCtl_avPllPSync1C3_A;
			sync2 =  regData25.uavPllCtl_avPllPSync2C3_A;
			divHDMI= regData11.uavPllCtl_avPllDivHdmiC3_A;
			divAV1 = regData12.uavPllCtl_avPllDivAv1C3_A;
			divAV2 = regData13.uavPllCtl_avPllDivAv2C3_A;
			divAV3 = regData14.uavPllCtl_avPllDivAv3C3_A;
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl5 regData5;
			T32Gbl_avPllCtl18 regData18;
			T32Gbl_avPllCtl26 regData26;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl5), &regData5.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl18), &regData18.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl26), &regData26.u32);
			offset = regData5.uavPllCtl_avPllFreqOffsetC4_A;
			sync1 =  regData18.uavPllCtl_avPllPSync1C4_A;
			sync2 =  regData26.uavPllCtl_avPllPSync2C4_A;
			divHDMI= regData11.uavPllCtl_avPllDivHdmiC4_A;
			divAV1 = regData12.uavPllCtl_avPllDivAv1C4_A;
			divAV2 = regData13.uavPllCtl_avPllDivAv2C4_A;
			divAV3 = regData14.uavPllCtl_avPllDivAv3C4_A;
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl6 regData6;
			T32Gbl_avPllCtl19 regData19;
			T32Gbl_avPllCtl27 regData27;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl6), &regData6.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl19), &regData19.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl27), &regData27.u32);
			offset = regData6.uavPllCtl_avPllFreqOffsetC5_A;
			sync1 =  regData19.uavPllCtl_avPllPSync1C5_A;
			sync2 =  regData27.uavPllCtl_avPllPSync2C5_A;
			divHDMI= regData11.uavPllCtl_avPllDivHdmiC5_A;
			divAV1 = regData12.uavPllCtl_avPllDivAv1C5_A;
			divAV2 = regData13.uavPllCtl_avPllDivAv2C5_A;
			divAV3 = regData14.uavPllCtl_avPllDivAv3C5_A;
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl7 regData7;
			T32Gbl_avPllCtl20 regData20;
			T32Gbl_avPllCtl28 regData28;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl7), &regData7.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl20), &regData20.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl28), &regData28.u32);
			offset = regData7.uavPllCtl_avPllFreqOffsetC6_A;
			sync1 =  regData20.uavPllCtl_avPllPSync1C6_A;
			sync2 =  regData28.uavPllCtl_avPllPSync2C6_A;
			divHDMI= regData11.uavPllCtl_avPllDivHdmiC6_A;
			divAV1 = regData12.uavPllCtl_avPllDivAv1C6_A;
			divAV2 = regData13.uavPllCtl_avPllDivAv2C6_A;
			divAV3 = regData14.uavPllCtl_avPllDivAv3C6_A;
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl8 regData8;
			T32Gbl_avPllCtl21 regData21;
			T32Gbl_avPllCtl29 regData29;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl8), &regData8.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl21), &regData21.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl29), &regData29.u32);
			offset = regData8.uavPllCtl_avPllFreqOffsetC7_A;
			sync1 =  regData21.uavPllCtl_avPllPSync1C7_A;
			sync2 =  regData29.uavPllCtl_avPllPSync2C7_A;
			divHDMI= regData11.uavPllCtl_avPllDivHdmiC7_A;
			divAV1 = regData12.uavPllCtl_avPllDivAv1C7_A;
			divAV2 = regData14.uavPllCtl_avPllDivAv2C7_A;
			divAV3 = regData15.uavPllCtl_avPllDivAv3C7_A;
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel\n");
			return;

	}

	if(enDPLL != 0)
		freq_from_dpll = vco_freq*sync2/sync1;
	else
		freq_from_dpll = vco_freq;

	if(offset& (1<<18))
	{
		offset = - (offset & ((1<<18)-1));
	}
	freq_from_offset = vco_freq/((double)offset/4194304+1);

	//figure out which divider used
	if(divHDMI != 0)
	{
		divHDMI &= 0x3;
		dbg_printf(PRN_RES, "divHDMI on: %d\n", divHDMI);

		freq_from_dpll = freq_from_dpll/divHDMICode[divHDMI];
		freq_from_offset = freq_from_offset/divHDMICode[divHDMI];
	}

	if(divAV1 != 0)
	{
		divAV1 &= 0x3;
		dbg_printf(PRN_RES, "divAV1 on: %d\n", divAV1);

		freq_from_dpll = freq_from_dpll/divAV1Code[divAV1];
		freq_from_offset = freq_from_offset/divAV1Code[divAV1];
	}

	if(divAV2 != 0)
	{
		if(divAV3 == 0)
		{
			dbg_printf(PRN_RES, "divAV2 on: %d\n", divAV2);
			freq_from_dpll = freq_from_dpll/divAV2;
			freq_from_offset = freq_from_offset/divAV2;
		}
		else
		{
			dbg_printf(PRN_RES, "divAV2 on: %d\n", divAV2);
			dbg_printf(PRN_RES, "divAV3 on: %d\n", divAV3);
			if( (((int)((double)divAV2/4+.5))-1) != (divAV3&0x7))
			{
				dbg_printf(PRN_RES, "Invalid divAV2 and divAV3 combination\n");
			}
			freq_from_dpll = freq_from_dpll*2/divAV2;
			freq_from_offset = freq_from_offset*2/divAV2;
		}
	}

	dbg_printf(PRN_RES, "PLLA C%d Freq is: %fMHz(Offset) %fMHz(DPLL)\n", chId, freq_from_offset, freq_from_dpll);

	current_freq[0][chId]=freq_from_dpll;
#endif
}

void diag_avpllFreq_B(int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	double vco_freq=diag_vcoFreqs[diag_pll_B_VCO_Setting];
	double freq_from_dpll, freq_from_offset;
	int offset, sync1, sync2, divHDMI, divAV1, divAV2, divAV3;
	int enDPLL;

	T32Gbl_avPllCtl42 regData42;
	T32Gbl_avPllCtl43 regData43;
	T32Gbl_avPllCtl44 regData44;
	T32Gbl_avPllCtl45 regData45;
	T32Gbl_avPllCtl46 regData46;
	T32Gbl_avPllCtl61 regData61;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl42), &regData42.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl43), &regData43.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl44), &regData44.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl45), &regData45.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl46), &regData46.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl61), &regData61.u32);

	enDPLL = regData61.uavPllCtl_avPllEnDpllC_B & (1<<(chId-1));

	//find offset, sync1, sync2, divHDMI, divAV1, divAV2, divAV3
	switch(chId)
	{
		case 1:
		{
			T32Gbl_avPllCtl33 regData33;
			T32Gbl_avPllCtl46 regData46;
			T32Gbl_avPllCtl54 regData54;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl33), &regData33.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl46), &regData46.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl54), &regData54.u32);

			offset = regData33.uavPllCtl_avPllFreqOffsetC1_B;
			sync1 =  regData46.uavPllCtl_avPllPSync1C1_B;
			sync2 =  regData54.uavPllCtl_avPllPSync2C1_B;
			divHDMI= regData42.uavPllCtl_avPllDivHdmiC1_B;
			divAV1 = regData42.uavPllCtl_avPllDivAv1C1_B;
			divAV2 = regData43.uavPllCtl_avPllDivAv2C1_B;
			divAV3 = regData45.uavPllCtl_avPllDivAv3C1_B;
		}
		break;
		case 2:
		{
			T32Gbl_avPllCtl34 regData34;
			T32Gbl_avPllCtl47 regData47;
			T32Gbl_avPllCtl55 regData55;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl34), &regData34.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl47), &regData47.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl55), &regData55.u32);

			offset = regData34.uavPllCtl_avPllFreqOffsetC2_B;
			sync1 =  regData47.uavPllCtl_avPllPSync1C2_B;
			sync2 =  regData55.uavPllCtl_avPllPSync2C2_B;
			divHDMI= regData42.uavPllCtl_avPllDivHdmiC2_B;
			divAV1 = regData43.uavPllCtl_avPllDivAv1C2_B;
			divAV2 = regData43.uavPllCtl_avPllDivAv2C2_B;
			divAV3 = regData45.uavPllCtl_avPllDivAv3C2_B;
		}
		break;
		case 3:
		{
			T32Gbl_avPllCtl35 regData35;
			T32Gbl_avPllCtl48 regData48;
			T32Gbl_avPllCtl56 regData56;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl35), &regData35.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl48), &regData48.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl56), &regData56.u32);
			offset = regData35.uavPllCtl_avPllFreqOffsetC3_B;
			sync1 =  regData48.uavPllCtl_avPllPSync1C3_B;
			sync2 =  regData56.uavPllCtl_avPllPSync2C3_B;
			divHDMI= regData42.uavPllCtl_avPllDivHdmiC3_B;
			divAV1 = regData43.uavPllCtl_avPllDivAv1C3_B;
			divAV2 = regData44.uavPllCtl_avPllDivAv2C3_B;
			divAV3 = regData45.uavPllCtl_avPllDivAv3C3_B;
		}
		break;
		case 4:
		{
			T32Gbl_avPllCtl36 regData36;
			T32Gbl_avPllCtl49 regData49;
			T32Gbl_avPllCtl57 regData57;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl36), &regData36.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl49), &regData49.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl57), &regData57.u32);
			offset = regData36.uavPllCtl_avPllFreqOffsetC4_B;
			sync1 =  regData49.uavPllCtl_avPllPSync1C4_B;
			sync2 =  regData57.uavPllCtl_avPllPSync2C4_B;
			divHDMI= regData42.uavPllCtl_avPllDivHdmiC4_B;
			divAV1 = regData43.uavPllCtl_avPllDivAv1C4_B;
			divAV2 = regData44.uavPllCtl_avPllDivAv2C4_B;
			divAV3 = regData45.uavPllCtl_avPllDivAv3C4_B;
		}
		break;
		case 5:
		{
			T32Gbl_avPllCtl37 regData37;
			T32Gbl_avPllCtl50 regData50;
			T32Gbl_avPllCtl58 regData58;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl37), &regData37.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl50), &regData50.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl58), &regData58.u32);
			offset = regData37.uavPllCtl_avPllFreqOffsetC5_B;
			sync1 =  regData50.uavPllCtl_avPllPSync1C5_B;
			sync2 =  regData58.uavPllCtl_avPllPSync2C5_B;
			divHDMI= regData42.uavPllCtl_avPllDivHdmiC5_B;
			divAV1 = regData43.uavPllCtl_avPllDivAv1C5_B;
			divAV2 = regData44.uavPllCtl_avPllDivAv2C5_B;
			divAV3 = regData45.uavPllCtl_avPllDivAv3C5_B;
		}
		break;
		case 6:
		{
			T32Gbl_avPllCtl38 regData38;
			T32Gbl_avPllCtl51 regData51;
			T32Gbl_avPllCtl59 regData59;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl38), &regData38.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl51), &regData51.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl59), &regData59.u32);
			offset = regData38.uavPllCtl_avPllFreqOffsetC6_B;
			sync1 =  regData51.uavPllCtl_avPllPSync1C6_B;
			sync2 =  regData59.uavPllCtl_avPllPSync2C6_B;
			divHDMI= regData42.uavPllCtl_avPllDivHdmiC6_B;
			divAV1 = regData43.uavPllCtl_avPllDivAv1C6_B;
			divAV2 = regData44.uavPllCtl_avPllDivAv2C6_B;
			divAV3 = regData45.uavPllCtl_avPllDivAv3C6_B;
		}
		break;
		case 7:
		{
			T32Gbl_avPllCtl39 regData39;
			T32Gbl_avPllCtl52 regData52;
			T32Gbl_avPllCtl60 regData60;
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl39), &regData39.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl52), &regData52.u32);
			BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl60), &regData60.u32);
			offset = regData39.uavPllCtl_avPllFreqOffsetC7_B;
			sync1 =  regData52.uavPllCtl_avPllPSync1C7_B;
			sync2 =  regData60.uavPllCtl_avPllPSync2C7_B;
			divHDMI= regData42.uavPllCtl_avPllDivHdmiC7_B;
			divAV1 = regData43.uavPllCtl_avPllDivAv1C7_B;
			divAV2 = regData45.uavPllCtl_avPllDivAv2C7_B;
			divAV3 = regData46.uavPllCtl_avPllDivAv3C7_B;
		}
		break;
		default:
			dbg_printf(PRN_RES, "Invalid Channel\n");
			return;

	}

	if(enDPLL != 0)
		freq_from_dpll = vco_freq*sync2/sync1;
	else
		freq_from_dpll = vco_freq;

	if(offset& (1<<18))
	{
		offset = - (offset & ((1<<18)-1));
	}
	freq_from_offset = vco_freq/((double)offset/4194304+1);

	//figure out which divider used
	if(divHDMI != 0)
	{
		divHDMI &= 0x3;
		dbg_printf(PRN_RES, "divHDMI on: %d\n", divHDMI);

		freq_from_dpll = freq_from_dpll/divHDMICode[divHDMI];
		freq_from_offset = freq_from_offset/divHDMICode[divHDMI];
	}

	if(divAV1 != 0)
	{
		divAV1 &= 0x3;
		dbg_printf(PRN_RES, "divAV1 on: %d\n", divAV1);

		freq_from_dpll = freq_from_dpll/divAV1Code[divAV1];
		freq_from_offset = freq_from_offset/divAV1Code[divAV1];
	}

	if(divAV2 != 0)
	{
		if(divAV3 == 0)
		{
			dbg_printf(PRN_RES, "divAV2 on: %d\n", divAV2);
			freq_from_dpll = freq_from_dpll/divAV2;
			freq_from_offset = freq_from_offset/divAV2;
		}
		else
		{
			dbg_printf(PRN_RES, "divAV2 on: %d\n", divAV2);
			dbg_printf(PRN_RES, "divAV3 on: %d\n", divAV3);
			if( (((int)((double)divAV2/4+.5))-1) != (divAV3&0x7))
			{
				dbg_printf(PRN_RES, "Invalid divAV2 and divAV3 combination\n");
			}
			freq_from_dpll = freq_from_dpll*2/divAV2;
			freq_from_offset = freq_from_offset*2/divAV2;
		}
	}

	dbg_printf(PRN_RES, "PLLB C%d Freq is: %fMHz(Offset) %fMHz(DPLL)\n", chId, freq_from_offset, freq_from_dpll);

	current_freq[1][chId]=freq_from_dpll;
#endif
}

void diag_vcoFreq_A(void)
{
#ifndef FPGA_TAILORED_BOARDVER
	double ref_freq=25.0;
	double freq_from_offset;
	int offset, fbDiv;

	T32Gbl_avPllCtl1 regData1;
	T32Gbl_avPllCtl9 regData9;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl1), &regData1.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData9.u32);

	//find offset, fbDiv
	offset = regData9.uavPllCtl_avPllFreqOffsetC8_A;
	fbDiv =  regData1.uavPllCtl_avPllFbDiv_A;

	if(offset& (1<<18))
	{
		offset = - (offset & ((1<<18)-1));
	}
	//note that the offset formula for VCO(C8) is different from C1-C7
	//according to John Ma, the formula is the same, just that ref_freq is CKOUT
	//and VCO freq is CKIN
	freq_from_offset = ref_freq*((double)offset/4194304+1);

	freq_from_offset = freq_from_offset*fbDiv;

	dbg_printf(PRN_RES, "PLLA VCO Freq is: %f\n", freq_from_offset);
#endif
}

void diag_vcoFreq_B(void)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	double ref_freq=25.0;
	double freq_from_offset;
	int offset, fbDiv;

	T32Gbl_avPllCtl32 regData32;
	T32Gbl_avPllCtl40 regData40;

	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl32), &regData32.u32);
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData40.u32);

	//find offset, fbDiv
	offset = regData40.uavPllCtl_avPllFreqOffsetC8_B;
	fbDiv =  regData32.uavPllCtl_avPllFbDiv_B;

	if(offset& (1<<18))
	{
		offset = - (offset & ((1<<18)-1));
	}
	//note that the offset formula for VCO(C8) is different from C1-C7
	//according to John Ma, the formula is the same, just that ref_freq is CKOUT
	//and VCO freq is CKIN
	freq_from_offset = ref_freq*((double)offset/4194304+1);

	freq_from_offset = freq_from_offset*fbDiv;

	dbg_printf(PRN_RES, "PLLB VCO Freq is: %f\n", freq_from_offset);
#endif
}

int diag_getVCOFreq(int hdmiMode, int freqIndex)
{
    return (vcoSelectionTable[hdmiMode][freqIndex]);
}

int diag_getChannelOutputFreq(int AVPLLIndex,int chID)
{
    if (chID<0 || chID>7)
        return 0;

    if (AVPLLIndex == 0) //AVPLL-A
        return (gAVPLLA_Channel_OutputClock[chID]);
    else
        return (gAVPLLB_Channel_OutputClock[chID]);
}

//freq_index, 0 to 5, the 6 VCLKs
//hdmiMode, HDMI_8BIT_MODE, HDMI_10BIT_MODE, HDMI_12BIT_MODE
//frameRate, FRAME_RATE_59P94 FRAME_RATE_60
//overSampleRate, 1, 2, 4, 8, 10, 12.5, 15
void diag_videoFreq_A(int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId)
{
            int vcoFreqIndex=vcoSelectionTable[hdmiMode][freqIndex];
            double vcoFreq, videoFreq;
            int ppm1K=ppm1kSelectionTable[frameRate][freqIndex];
            double divider;

            if (chId<0 || chId>7)
                return;

            dbg_printf(PRN_RES, "Set PLLA C%d:\n", chId);
            //set VCO freq here
            diag_setVCO_A(vcoFreqIndex);
            dbg_printf(PRN_RES, "VCO freq=%f\n", diag_vcoFreqs[vcoFreqIndex]);

            //check to see if ppm1K is need
            vcoFreq = diag_vcoFreqs[vcoFreqIndex];
            if(ppm1K<0)
            {
                        vcoFreq = vcoFreq*1000/1001;
            }

            if(ppm1K>0)
            {
                        vcoFreq = vcoFreq*1001/1000;
            }
            diag_set1KPPM_A(ppm1K, chId);

            //get the video freq
            videoFreq = diag_videoFreqs[freqIndex];
            if(frameRate)
            {
                        //60Hz vclk is 1001/1000 times 59.94Hz vclk
                        videoFreq = videoFreq*1001/1000;
            }

            dbg_printf(PRN_RES, "Video freq=%f\n", videoFreq);

            //disable all dividers
            diag_setHDMI_Div_A(0, chId);
            diag_setAV_Div_A(0, 0, chId); //divAV1 is for divide by 1, 0 will disable the whole divAV chain
            diag_setAV3_Div_A(0, 0, chId);

            //set the divider
            if(overSampleRate>=10)
            {
                        //use HDMI divider
                        divider = vcoFreq/videoFreq/overSampleRate;
                        dbg_printf(PRN_RES, "HDMI divider is %f %d\n", divider, (int)(divider+0.5));

                        diag_setHDMI_Div_A((int)(divider+0.5), chId);
                        dbg_printf(PRN_RES, "PLLA C%d=%f\n", chId, videoFreq*overSampleRate);
            }
            else
            {
                        divider = vcoFreq*2/videoFreq/overSampleRate;
                        dbg_printf(PRN_RES, "AV divider is %f %d\n", divider/2, (int)(divider+0.5)/2);

                        if(((int)(divider+0.5))&1)
                        {
                                    //fractional divider, use AV2 and AV3
                                    //figure out AV3, AV3 = round(AV2/4)-1
                                    int divAV3 = ((int)((double)divider/4+0.5))-1;

                                    //this enables divAV channel
                                    diag_setAV_Div_A(4, 0, chId);

                                    //add enable bit
                                    divAV3 |= (1<<3);
                                    diag_setAV3_Div_A((int)(divider+0.5), divAV3, chId);
                        }
                        else
                        {
                                    //integer divider, use AV2 only
                                    diag_setAV_Div_A(4, (int)(divider+0.5)/2, chId);
                        }

                        dbg_printf(PRN_RES, "PLLA C%d=%f\n", chId, videoFreq*overSampleRate);
            }

            gAVPLLA_Channel_OutputClock[chId] = (unsigned int)(videoFreq*overSampleRate*1000000);

}

//freq_index, 0 to 5, the 6 VCLKs
//hdmiMode, HDMI_8BIT_MODE, HDMI_10BIT_MODE, HDMI_12BIT_MODE
//frameRate, FRAME_RATE_59P94 FRAME_RATE_60
//overSampleRate, 1, 2, 4, 8, 10, 12.5, 15
void diag_videoFreq_B(int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
            int vcoFreqIndex=vcoSelectionTable[hdmiMode][freqIndex];
            double vcoFreq, videoFreq;
            int ppm1K=ppm1kSelectionTable[frameRate][freqIndex];
            double divider;

            if (chId<0 || chId>7)
                return;

            dbg_printf(PRN_RES, "Set PLLB C%d:\n", chId);

            //set VCO freq here
            diag_setVCO_B(vcoFreqIndex);
            dbg_printf(PRN_RES, "VCO freq=%f\n", diag_vcoFreqs[vcoFreqIndex]);
            //check to see if ppm1K is need
            vcoFreq = diag_vcoFreqs[vcoFreqIndex];

            if(ppm1K<0)
            {
                        vcoFreq = vcoFreq*1000/1001;
            }
            if(ppm1K>0)
            {
                        vcoFreq = vcoFreq*1001/1000;
            }

            diag_set1KPPM_B(ppm1K, chId);

            //get the video freq
            videoFreq = diag_videoFreqs[freqIndex];
            if(frameRate)
            {
                        //60Hz vclk is 1001/1000 times 59.94Hz vclk
                        videoFreq = videoFreq*1001/1000;
            }

            dbg_printf(PRN_RES, "Video freq=%f\n", videoFreq);

            //disable all dividers
            diag_setHDMI_Div_B(0, chId);
            diag_setAV_Div_B(0, 0, chId);
            diag_setAV3_Div_B(0, 0, chId);

            //set the divider
            if(overSampleRate>=10)
            {
                        //use HDMI divider
                        divider = vcoFreq/videoFreq/overSampleRate;
                        dbg_printf(PRN_RES, "HDMI divider is %f %d\n", divider, (int)(divider+0.5));

                        diag_setHDMI_Div_B((int)(divider+0.5), chId);
                        dbg_printf(PRN_RES, "PLLB C%d=%f\n", chId, videoFreq*overSampleRate);
            }
            else
            {
                        divider = vcoFreq*2/videoFreq/overSampleRate;
                        dbg_printf(PRN_RES, "AV divider is %f %d\n", divider/2, (int)((divider+0.5)/2));

                        if((((int)(divider+0.5)))&1)
                        {
                                    //fractional divider, use AV2 and AV3
                                    //figure out AV3, AV3 = round(AV2/4)-1
                                    int divAV3 = ((int)((double)divider/4+0.5))-1;

                                    //this enables divAV channel
                                    diag_setAV_Div_B(4, 0, chId);

                                    //add enable bit
                                    divAV3 |= (1<<3);
                                    diag_setAV3_Div_B((int)(divider+0.5), divAV3, chId);
                        }
                        else
                        {
                                    //integer divider, use AV2 only
                                    diag_setAV_Div_B(4, (int)(divider+0.5)/2, chId);
                        }

                        dbg_printf(PRN_RES, "PLLB C%d=%f\n", chId, videoFreq*overSampleRate);
            }

            gAVPLLB_Channel_OutputClock[chId] = (unsigned int)(videoFreq*overSampleRate*1000000);

#endif
}


//freqIndex is the index into clock_freq[] array
int diag_clockFreq_A(int freqIndex, int chId)
{

	if(freqIndex >= (sizeof(clk_freqs) / sizeof(CLK_FREQ)))
	{
		dbg_printf(PRN_RES, "freq Index not found\n");
		return 1;
	}

	if(diag_pll_A_VCO_Setting != clk_freqs[freqIndex].vco_freq_index)
	{
		dbg_printf(PRN_RES, "VCO frequency is changed to %f!\n",
			diag_vcoFreqs[clk_freqs[freqIndex].vco_freq_index]);

		diag_setVCO_A(clk_freqs[freqIndex].vco_freq_index);
	}

	//change offset
	diag_setChanOffset_A(clk_freqs[freqIndex].freq_offset, chId);

	//change p_sync
	diag_setDPll_A((clk_freqs[freqIndex].p_sync1!=0),
			clk_freqs[freqIndex].p_sync1,
			clk_freqs[freqIndex].p_sync2, chId);

	//disable all dividers
	diag_setHDMI_Div_A(0, chId);
	diag_setAV_Div_A(0, 0, chId);
	diag_setAV3_Div_A(0, 0, chId);

	//update now div
	diag_setAV_Div_A(clk_freqs[freqIndex].divAV1, clk_freqs[freqIndex].divAV2, chId);
	diag_setAV3_Div_A(clk_freqs[freqIndex].divAV2, clk_freqs[freqIndex].divAV3, chId);


	return 0;

}

int diag_clockFreq_B(int freqIndex, int chId)
{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
	if(freqIndex >= (sizeof(clk_freqs) / sizeof(CLK_FREQ)))
	{
		dbg_printf(PRN_RES, "freq Index not found\n");
		return 1;
	}

	if(diag_pll_B_VCO_Setting != clk_freqs[freqIndex].vco_freq_index)
	{
		dbg_printf(PRN_RES, "VCO frequency is changed to %f!\n",
			diag_vcoFreqs[clk_freqs[freqIndex].vco_freq_index]);

		diag_setVCO_B(clk_freqs[freqIndex].vco_freq_index);
	}

	//change offset
	diag_setChanOffset_B(clk_freqs[freqIndex].freq_offset, chId);

	//change p_sync
	diag_setDPll_B((clk_freqs[freqIndex].p_sync1!=0),
			clk_freqs[freqIndex].p_sync1,
			clk_freqs[freqIndex].p_sync2, chId);

	//disable all dividers
	diag_setHDMI_Div_B(0, chId);
	diag_setAV_Div_B(0, 0, chId);
	diag_setAV3_Div_B(0, 0, chId);

	//update now div
	diag_setAV_Div_B(clk_freqs[freqIndex].divAV1, clk_freqs[freqIndex].divAV2, chId);
	diag_setAV3_Div_B(clk_freqs[freqIndex].divAV2, clk_freqs[freqIndex].divAV3, chId);


	return 0;
#endif
}

int get_freqTabIndex(int vco_freq_index, int av_freq_index)
{
	int i, tab_index=-1;

	for(i=0; i<(sizeof(clk_freqs) / sizeof(CLK_FREQ)); i++)
	{
		if(clk_freqs[i].vco_freq_index == vco_freq_index && clk_freqs[i].av_freq_index == av_freq_index)
		{
			tab_index = i; break;
		}
	}
	return tab_index;
}

int AVPLL_Set(int groupId, int chanId, int avFreqIndex)
{
	int freqTabIndex;

	if(groupId==0)	// PLL A
	{
		// Search freqIndex in lookup table by avFreqIndex and vco_freq_index.
		freqTabIndex = get_freqTabIndex(diag_pll_A_VCO_Setting, avFreqIndex);
		if(freqTabIndex == -1)
			return -1;
		diag_clockFreq_A(freqTabIndex, chanId);
	}
	else if(groupId==1) // PLL B
	{
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
		freqTabIndex = get_freqTabIndex(diag_pll_B_VCO_Setting, avFreqIndex);
		if(freqTabIndex == -1)
			return -1;

		diag_clockFreq_B(freqTabIndex, chanId);
#endif
	}

	return 0;
}

// secondary MCLK keeps 4x, other MCLK div4 to 1x
int AVPLL_SetDiv4()
{
#ifndef FPGA_TAILORED_BOARDVER
	T32Gbl_ClkSwitch1  ClkSwitch1;
	T32Gbl_clkSelect2 ClkSelect2;
#ifdef CONFIG_MV_AMP_INTERNAL_ADAC_DISABLE
	T32Gbl_clkSelect3 ClkSelect3;
#endif

        GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_ClkSwitch1), &ClkSwitch1.u32);
        ClkSwitch1.uClkSwitch_audio0ClkSwitch = 1; // primary
        ClkSwitch1.uClkSwitch_audio2ClkSwitch = 1; // mic
        ClkSwitch1.uClkSwitch_audio3ClkSwitch = 1; // spdif
#ifdef CONFIG_MV_AMP_INTERNAL_ADAC_DISABLE
	ClkSwitch1.uClkSwitch_audio1ClkSwitch = 1; // stereo
#endif
        GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_ClkSwitch1), (ClkSwitch1.u32));

	GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkSelect2), &ClkSelect2.u32);
	ClkSelect2.uclkSelect_audio0ClkSel = Gbl_clkSelect_audio0ClkSel_d4;
	ClkSelect2.uclkSelect_audio2ClkSel = Gbl_clkSelect_audio2ClkSel_d4;
	ClkSelect2.uclkSelect_audio3ClkSel = Gbl_clkSelect_audio3ClkSel_d4;
	GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkSelect2), (ClkSelect2.u32));

#ifdef CONFIG_MV_AMP_INTERNAL_ADAC_DISABLE
	GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkSelect3), &ClkSelect3.u32);
	ClkSelect3.uclkSelect_audio1ClkSel = Gbl_clkSelect_audio1ClkSel_d4;
	GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkSelect3), (ClkSelect3.u32));
#endif
#endif
}


// S/W workaround is not required at BG2.
void AVPLL_EnableMicClk(int en)
{
#ifndef FPGA_TAILORED_BOARDVER
        T32Gbl_clkEnable  regClkEnable;
        BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkEnable), &regClkEnable.u32);
	if (en) {
		en = 1;
	}
        regClkEnable.uclkEnable_audio2ClkEn = en;
        BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkEnable), regClkEnable.u32);
#endif
}

/* PCR_TRACKING -- BG2 */
// add read function here for adjustment, real adjustment to nominate frequency is cur-orig
void AVPLL_GetPPM(int grp, double *ppm_base, double *ppm_now)
{
	double ppm0, ppm;
	if(grp ==0)
	{
		ppm0 = org_ppm[0];
		ppm = cur_ppm[0];
	}
	else // AVPLL B
	{
		ppm0 = org_ppm[1];
		ppm = cur_ppm[1];
	}
	if(ppm_base) *ppm_base = ppm0;
	if(ppm_now) *ppm_now = ppm;
}

double AVPLL_AdjustPPM(double ppm_delta, int grp)
{
#ifndef FPGA_TAILORED_BOARDVER
        volatile int i;
        double ppm0, ppm;

        if(grp == 0) { // For AVPLL-A
                T32Gbl_avPllCtl9 regData9;

                BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), &regData9.u32);

                if(avpll_inited[0] == 0) {
                        ppm0 = offset_2_ppm(regData9.uavPllCtl_avPllFreqOffsetC8_A);
                        cur_ppm[0] = org_ppm[0] = ppm0;
                        avpll_inited[0] = 1;
                }else{
                        ppm0 = org_ppm[0];
                }

                cur_ppm[0] += ppm_delta;
                ppm = cur_ppm[0];

                regData9.uavPllCtl_avPllFreqOffsetC8_A = ppm_2_offset(ppm, org_ppm[0]);
                BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData9.u32);

                regData9.uavPllCtl_avPllFreqOffsetReadyC_A = 0x80;
                BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData9.u32);

                for(i=0; i<10000; i++);

                regData9.uavPllCtl_avPllFreqOffsetReadyC_A = 0;
                BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl9), regData9.u32);
        } else { // For AVPLL-B
#if (BERLIN_CHIP_VERSION != BERLIN_BG2_CD) && (BERLIN_CHIP_VERSION != BERLIN_BG2CDP)
                T32Gbl_avPllCtl40 regData40;

                BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), &regData40.u32);

                if(avpll_inited[1] == 0) {
                        ppm0 = offset_2_ppm(regData40.uavPllCtl_avPllFreqOffsetC8_B);
                        cur_ppm[1] = org_ppm[1] = ppm0;
                        avpll_inited[1] = 1;
                }else{
                        ppm0 = org_ppm[1];
                }

                cur_ppm[1] += ppm_delta;
                ppm = cur_ppm[1];

                regData40.uavPllCtl_avPllFreqOffsetC8_B = ppm_2_offset(ppm, org_ppm[1]);
                BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData40.u32);

                regData40.uavPllCtl_avPllFreqOffsetReadyC_B = 0x80;
                BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData40.u32);

                for(i=0; i<10000; i++);

                regData40.uavPllCtl_avPllFreqOffsetReadyC_B = 0;
                BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_avPllCtl40), regData40.u32);
#endif
        }

        MVLOG(MOD_AVS, DBG_MID, "-- cur_ppm: %.1lf org_ppm: %.1lf diff: %.1lf\n", ppm, ppm0, ppm - ppm0);
        return ppm - ppm0;
#endif
}

void AVPLL_InitClock(void)
{
#ifndef FPGA_TAILORED_BOARDVER
#if (BERLIN_CHIP_VERSION >= BERLIN_B_0)
    T32Gbl_ClkSwitch1  ClkSwitch1;
    T32Gbl_clkSelect2 ClkSelect2;
    T32Gbl_I2S2_LRCKCntl LRCKCntl;
    T32Gbl_I2S2_BCLKCntl BCLKCntl;

    GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_ClkSwitch1), &ClkSwitch1.u32);
    ClkSwitch1.uClkSwitch_audio0ClkSwitch = 0; // source clock
    ClkSwitch1.uClkSwitch_audio0ClkD3Switch = 0; // non DIV3 clock
    ClkSwitch1.uClkSwitch_audio1ClkSwitch = 0; // source clock
    ClkSwitch1.uClkSwitch_audio1ClkD3Switch = 0; // non DIV3 clock
    ClkSwitch1.uClkSwitch_audio2ClkSwitch = 0; // source clock
    ClkSwitch1.uClkSwitch_audio2ClkD3Switch = 0; // non DIV3 clock
    ClkSwitch1.uClkSwitch_audio3ClkSwitch = 0; // source clock
    ClkSwitch1.uClkSwitch_audio3ClkD3Switch = 0; // non DIV3 clock
    GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_ClkSwitch1), (ClkSwitch1.u32));

    GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkSelect2), &ClkSelect2.u32);
    ClkSelect2.uclkSelect_audio1ClkPllSel = Gbl_clkSelect_audio1ClkPllSel_AVPllA3;
    GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_clkSelect2), (ClkSelect2.u32));


    // Set LRClk and BCLK to Pull Down.
    GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_LRCKCntl), (&LRCKCntl.u32));
    LRCKCntl.uI2S2_LRCKCntl_PD_EN = 1;
    GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_LRCKCntl), (LRCKCntl.u32));

    GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_BCLKCntl), (&BCLKCntl.u32));
    BCLKCntl.uI2S2_BCLKCntl_PD_EN = 1;
    GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_BCLKCntl), (BCLKCntl.u32));

#endif
#endif
}

#else

unsigned int gauchAvpllBaseAddr[AVPLL_MAX] = {(MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA), (MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_AVPLLB)};
unsigned int gauchChannelAddr[] = {RA_avPll_C1, RA_avPll_C2, RA_avPll_C3, RA_avPll_C4, RA_avPll_C5, RA_avPll_C6, RA_avPll_C7, RA_avPll_C8};

int diag_videoFreq(int avpllGroupId, int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId);

int diag_setDPll(int avpllGroupId, int enable, int p_sync1, int p_sync2, int chId)
{
#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if (avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

	T32avPllCh_CTRL1 stAvPllCtrl1;
	T32avPllCh_CTRL2 stAvPllCtrl2;
	T32avPll_CTRL3 stAvPllCtrl3;
	double ppm_from_dpll = 0;
	unsigned int uiAvPllBase = 0;

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

       //Disable DPLL first
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL3, &stAvPllCtrl3.u32);
	stAvPllCtrl3.uCTRL3_EN_DPLL_C &= ~(1<<(chId-1));
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL3, stAvPllCtrl3.u32);

        if(!p_sync1)
        {
                if(enable)
			dbg_printf(PRN_RES, "Warning p_sync1 is 0\n");
        }
        else
        {
                ppm_from_dpll = (double)p_sync2/p_sync1;
                dbg_printf(PRN_RES, "  DPLL PPM is %f\n", ppm_from_dpll);
        }


	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL1, &stAvPllCtrl1.u32);
	stAvPllCtrl1.uCTRL1_P_SYNC1 = p_sync1;
	BFM_HOST_Bus_Write32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL1, stAvPllCtrl1.u32);

	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL2, &stAvPllCtrl2.u32);
	stAvPllCtrl2.uCTRL2_P_SYNC2 = p_sync2;
	BFM_HOST_Bus_Write32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL2,  stAvPllCtrl2.u32);

	if(enable)
	{
			//enable DPLL
		BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL3, &stAvPllCtrl3.u32);
		stAvPllCtrl3.uCTRL3_EN_DPLL_C |= (1<<(chId-1));
		BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL3, stAvPllCtrl3.u32);
	}

	return AVPLL_E_OK;
}

void diag_assertReset(int avpllGroupId)
{
	T32avPll_CTRL0 stAvPllCtrl0;
	unsigned int uiAvPllBase = 0;

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	//assert reset
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL0 , &stAvPllCtrl0.u32);
	stAvPllCtrl0.uCTRL0_RESET_PLL = 1;
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL0 , stAvPllCtrl0.u32);
}

void diag_deassertReset(int avpllGroupId)
{
	volatile int i;
	T32avPll_CTRL0 stAvPllCtrl0;
	unsigned int uiAvPllBase = 0;

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	for(i=0; i<10000; i++);
	//deassert reset
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL0 , &stAvPllCtrl0.u32);
	stAvPllCtrl0.uCTRL0_RESET_PLL = 0;
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL0 , stAvPllCtrl0.u32);
}

void diag_powerDown(int avpllGroupId)
{
	T32avPll_CTRL0 stAvPllCtrl0;
	T32avPll_CTRL2 stAvPllCtrl2;

	unsigned int uiAvPllBase = 0;
	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL0 , &stAvPllCtrl0.u32);
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL2 , &stAvPllCtrl2.u32);
	stAvPllCtrl0.uCTRL0_PU = 0;
	stAvPllCtrl2.uCTRL2_PU_C = 0;
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL0 , stAvPllCtrl0.u32);
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2 , stAvPllCtrl2.u32);
}

void diag_powerUp(int avpllGroupId)
{
	T32avPll_CTRL0 stAvPllCtrl0;
	T32avPll_CTRL2 stAvPllCtrl2;

	unsigned int uiAvPllBase = 0;
	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL0 , &stAvPllCtrl0.u32);
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL2 , &stAvPllCtrl2.u32);
	stAvPllCtrl0.uCTRL0_PU = 1;
	stAvPllCtrl2.uCTRL2_PU_C = 0x7F;
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL0 , stAvPllCtrl0.u32);
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2 , stAvPllCtrl2.u32);

}

int diag_setVCO(int avpllGroupId, int vco_freq_index)
{
	unsigned int uiAvPllBase = 0;
	T32avPll_CTRL0 stAvPllCtrl0;
	T32avPll_CTRL1 stAvPllCtrl1;
	T32avPll_CTRL2 stAvPllCtrl2;
	T32avPllCh_CTRL0 stAvPllChCtrl0;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if (avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	dbg_printf(PRN_INFO, "PLL group: %d, Idx:%d, VCO freq change to %f\n", avpllGroupId, vco_freq_index,  diag_vcoFreqs[vco_freq_index]);

	//following three settings are done under reset to improve long term reliability
	diag_assertReset(avpllGroupId);
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL0 , &stAvPllCtrl0.u32);
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL1 , &stAvPllCtrl1.u32);

	stAvPllCtrl0.uCTRL0_SPEED = diag_avpllRegSPEED[vco_freq_index];
	stAvPllCtrl1.uCTRL1_FBDIV = diag_avpllRegFBDIV[vco_freq_index];
	stAvPllCtrl0.uCTRL0_INTPI = diag_avpllRegINTPI[vco_freq_index];

	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL0 , stAvPllCtrl0.u32);
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL1 , stAvPllCtrl1.u32);
	diag_deassertReset(avpllGroupId);

	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[7] + RA_avPllCh8_CTRL0 , &stAvPllChCtrl0.u32);
	stAvPllChCtrl0.uCTRL0_FREQ_OFFSET = diag_avpllRegFREQ_OFFSET_C8[vco_freq_index];
	BFM_HOST_Bus_Write32(uiAvPllBase + gauchChannelAddr[7] + RA_avPllCh8_CTRL0 , stAvPllChCtrl0.u32);

	if(AVPLL_A == avpllGroupId)
		diag_pll_A_VCO_Setting = vco_freq_index;
	else
		diag_pll_B_VCO_Setting = vco_freq_index;

	cur_ppm[avpllGroupId] = org_ppm[avpllGroupId] = offset_2_ppm(stAvPllChCtrl0.uCTRL0_FREQ_OFFSET);
	avpll_inited[avpllGroupId] = 1;

	//toggle the offset_rdy bit
	{
		volatile int i;

		BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL2 , &stAvPllCtrl2.u32);
		stAvPllCtrl2.uCTRL2_FREQ_OFFSET_READY_C = (1<<7);
		BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2 , stAvPllCtrl2.u32);

		//add some delay because need to be asserted by 30ns
		for(i=0; i<10000; i++);

		BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL2 , &stAvPllCtrl2.u32);
		stAvPllCtrl2.uCTRL2_FREQ_OFFSET_READY_C = 0;
		BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2 , stAvPllCtrl2.u32);
	}

	return AVPLL_E_OK;
}

int diag_setChanOffset(int avpllGroupId, int offset, int chId)
{
	unsigned int uiAvPllBase = 0, reg_offset = 0;
	double ppm_from_offset = 0;
	T32avPll_CTRL2 stAvPllCtrl2;
	T32avPllCh_CTRL0 stAvPllChCtrl0;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	if(offset<0)
	{
		reg_offset = (1<<18) | (-offset) ;
	}
	else
	{
		reg_offset = offset;
	}

	dbg_printf(PRN_INFO, "set A%d offset to 0x%x\n", chId, reg_offset);

	if(offset != 0)
	{
		ppm_from_offset = 1/((double)offset/4194304+1);
		dbg_printf(PRN_RES, "offset PPM is %f\n", ppm_from_offset);
	}

		//set offset register
	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL0, &stAvPllChCtrl0.u32);
	stAvPllChCtrl0.uCTRL0_FREQ_OFFSET = reg_offset;
	BFM_HOST_Bus_Write32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL0, stAvPllChCtrl0.u32);

		//toggle the offset_rdy bit
	{
		volatile int i;

		BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL2 , &stAvPllCtrl2.u32);
		stAvPllCtrl2.uCTRL2_FREQ_OFFSET_READY_C = (1<<(chId-1));
		BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2 , stAvPllCtrl2.u32);

		//add some delay because need to be asserted by 30ns
		for(i=0; i<10000; i++);

		BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL2 , &stAvPllCtrl2.u32);
		stAvPllCtrl2.uCTRL2_FREQ_OFFSET_READY_C = 0;
		BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2 , stAvPllCtrl2.u32);

	}
	return AVPLL_E_OK;
}


int diag_setHDMI_Div(int avpllGroupId, int divHDMI, int chId)
{
	unsigned int uiAvPllBase = 0, div_reg = 0;
	T32avPllCh_CTRL3 stAvPllChCtrl3;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if (avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	switch(divHDMI)
	{
		case 0:
			div_reg = 0; //disable DIV_HDMI
			break;
		case 1:
			div_reg = 4; //div by 1, by pass
			break;
		case 2:
			div_reg = 5; //div by 2
			break;
		case 4:
			div_reg = 6; //div by 4
			break;
		case 6:
			div_reg = 7; //div by 6
			break;
		default:
			{
				dbg_printf(PRN_RES, "Invalid divider for HDMI\n");
				return AVPLL_E_BADPARAM;
			}
	}

	BFM_HOST_Bus_Read32(uiAvPllBase +  gauchChannelAddr[chId-1] + RA_avPllCh_CTRL3 , &stAvPllChCtrl3.u32);
	stAvPllChCtrl3.uCTRL3_DIV_HDMI = div_reg;
	BFM_HOST_Bus_Write32(uiAvPllBase +  gauchChannelAddr[chId-1] + RA_avPllCh_CTRL3 , stAvPllChCtrl3.u32);

	return AVPLL_E_OK;
}


//divAV1 is the register value, not the actual divider
//mapping is (binary):
//000 = bypass, the whole divAV channel will be bypassed
//100 = div by 1
//101 = div by 2
//110 = div by 5
//111 = div by 5
int diag_setAV_Div(int avpllGroupId, int divAV1, int divAV2, int chId)
{
	unsigned int uiAvPllBase = 0;
	T32avPllCh_CTRL3 stAvPllChCtrl3;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	if(divAV2 !=0 && (divAV2<4 || divAV2>127))
	{
		dbg_printf(PRN_RES, "Invalid divider for AV1\n");
			return AVPLL_E_BADPARAM;
	}

		//program DIV_AV1 and DIV_AV2
	BFM_HOST_Bus_Read32(uiAvPllBase +  gauchChannelAddr[chId-1] + RA_avPllCh_CTRL3 , &stAvPllChCtrl3.u32);
	stAvPllChCtrl3.uCTRL3_DIV_AV1 = divAV1;
	stAvPllChCtrl3.uCTRL3_DIV_AV2 = divAV2;
	BFM_HOST_Bus_Write32(uiAvPllBase +  gauchChannelAddr[chId-1] + RA_avPllCh_CTRL3 , stAvPllChCtrl3.u32);

	return AVPLL_E_OK;
}


//Input is the hex code for AV2 and AV3, see table III
int diag_setAV3_Div(int avpllGroupId, int divAV2, int divAV3, int chId)
{
	unsigned int uiAvPllBase = 0;
	T32avPllCh_CTRL3 stAvPllChCtrl3;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
                return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	//program DIV_AV2, DIV_AV3
	BFM_HOST_Bus_Read32(uiAvPllBase +  gauchChannelAddr[chId-1] + RA_avPllCh_CTRL3 , &stAvPllChCtrl3.u32);
	stAvPllChCtrl3.uCTRL3_DIV_AV2 = divAV2;
	stAvPllChCtrl3.uCTRL3_DIV_AV3 = divAV3;
	BFM_HOST_Bus_Write32(uiAvPllBase +  gauchChannelAddr[chId-1] + RA_avPllCh_CTRL3 , stAvPllChCtrl3.u32);

	return AVPLL_E_OK;
}


//this is done by using offset and dpll
//inc=1 means increase freq to 1001/1000
//inc=0 means disable 1KPPM (by setting offset to 0 and disable DPLL)
//inc=-1 means decrease freq to 1000/1001
int diag_set1KPPM(int avpllGroupId, int inc, int chId)
{
	unsigned int uiAvPllBase = 0;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
                return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];
	if(inc)
	{
		if(inc>0)
		{
			//increase by 1KPPM
			diag_setChanOffset(avpllGroupId, OFFSET_1KPPM_INC, chId);
			diag_setDPll(avpllGroupId, 1, 1000, 1001, chId);
		}
		else
		{
			//decrease by 1KPPM
			diag_setChanOffset(avpllGroupId, OFFSET_1KPPM_DEC, chId);
			diag_setDPll(avpllGroupId, 1, 1001, 1000, chId);
		}
	}
	else
	{
		//set offset to 0 and disable DPLL
		diag_setChanOffset(avpllGroupId, 0, chId);
		diag_setDPll(avpllGroupId, 0, 0, 0, chId);
	}
	return AVPLL_E_OK;
}

/* new reference in MHz, chan : chan_A 0, Chan_B 1 */
void diag_changeRefFreq(int vco_freq_index, int grp)
{
	if(grp ==0)
	{
		diag_setVCO(AVPLL_A, vco_freq_index);
	}
	else
	{
#if HAS_AVPLL_B
		diag_setVCO(AVPLL_B, vco_freq_index);
#else
		dbg_printf(PRN_RES,"\n AVPLL_B is removed\n");
#endif
	}
}


void diag_setMasterSlaveB(int MasterSlaveB_A, int MasterSlaveB_B)
{
	T32avPll_CTRL1 stAvPllCtrl1_0, stAvPllCtrl1_1;

	BFM_HOST_Bus_Read32(gauchAvpllBaseAddr[0] + BA_avPll_CTRL1_MASTER_SLAVEB, &stAvPllCtrl1_0.u32);
	BFM_HOST_Bus_Read32(gauchAvpllBaseAddr[1] + BA_avPll_CTRL1_MASTER_SLAVEB, &stAvPllCtrl1_1.u32);

	stAvPllCtrl1_0.uCTRL1_MASTER_SLAVEB = MasterSlaveB_A;
	stAvPllCtrl1_1.uCTRL1_MASTER_SLAVEB = MasterSlaveB_B;

	BFM_HOST_Bus_Write32(gauchAvpllBaseAddr[0] + BA_avPll_CTRL1_MASTER_SLAVEB, stAvPllCtrl1_0.u32);
	BFM_HOST_Bus_Write32(gauchAvpllBaseAddr[1] + BA_avPll_CTRL1_MASTER_SLAVEB, stAvPllCtrl1_1.u32);

}

//this function will enable AVPLL A and B and turn on caliberation
void AVPLL_Enable(void)
{
	T32avPll_CTRL0	stAvPllCtrl0_A, stAvPllCtrl0_B;
	T32avPll_CTRL1	stAvPllCtrl1_A, stAvPllCtrl1_B;
	T32avPll_CTRL2	stAvPllCtrl2_A, stAvPllCtrl2_B;
	T32avPllCh_CTRL0	stAvPllChCtrl0_A, stAvPllChCtrl0_B;
	T32avPllCh_CTRL0	stAvPllCh8Ctrl0_A, stAvPllCh8Ctrl0_B;
	T32Gbl_PadSelect	regPadSelect;
	T32Gbl_clkEnable	regClkEnable;
	volatile int i;
	int avpll_initialized=0;
	unsigned char chId = 0;
	unsigned int uiAvPll_A_BaseAddr = 0, uiAvPll_B_BaseAddr = 0;

	T32avPll_CTRL0 stAvPllCtrl0;
	T32avPll_CTRL1 stAvPllCtrl1;
	T32avPll_CTRL2 stAvPllCtrl2;
	T32avPll_CTRL3 stAvPllCtrl3;
	unsigned int reg1=0, reg2=0, reg3=0, reg4=0;

	uiAvPll_A_BaseAddr = gauchAvpllBaseAddr[0];
	uiAvPll_B_BaseAddr = gauchAvpllBaseAddr[1];

	// recover the default values first, added by HY
	stAvPllCtrl0.uCTRL0_RESET_PLL = 0;
	stAvPllCtrl0.uCTRL0_PU = 1;
	stAvPllCtrl0.uCTRL0_INTPI = 3;
	stAvPllCtrl0.uCTRL0_VDDR_1P45V = 1;
	stAvPllCtrl0.uCTRL0_VDDR_0P9V = 5;
	stAvPllCtrl0.uCTRL0_VTHVCOCAL = 2;
	stAvPllCtrl0.uCTRL0_RESERVE_PLL = 1;
	stAvPllCtrl0.uCTRL0_KVCO_EXT = 7;
	stAvPllCtrl0.uCTRL0_KVCO_EXT_EN = 0;
	stAvPllCtrl0.uCTRL0_V2I_EXT = 0xf;
	stAvPllCtrl0.uCTRL0_V2I_EXT_EN = 0;
	stAvPllCtrl0.uCTRL0_SPEED = 0;
	stAvPllCtrl0.uCTRL0_CLK_DET_EN = 1;
	stAvPllCtrl0.RSVDx0_b31 = 0;

	stAvPllCtrl1.uCTRL1_REFDIV = 0;
	stAvPllCtrl1.uCTRL1_FBDIV = 0;
	stAvPllCtrl1.uCTRL1_ICP = 5;;
	stAvPllCtrl1.uCTRL1_LOADCAP = 0;
	stAvPllCtrl1.uCTRL1_PLL_CALI_START =0 ;
	stAvPllCtrl1.uCTRL1_MASTER_SLAVEB = 1;
	stAvPllCtrl1.uCTRL1_TEST_ANA = 0;
	stAvPllCtrl1.RSVDx4_b27 = 0;

	stAvPllCtrl2.uCTRL2_FREQ_OFFSET_READY_C = 0;
	stAvPllCtrl2.uCTRL2_EN_LP_C = 0;
	stAvPllCtrl2.uCTRL2_PU_C = 1;
	stAvPllCtrl2.uCTRL2_RESET_C = 0;
	stAvPllCtrl2.RSVDx8_b30 = 0;

	stAvPllCtrl3.uCTRL3_EN_DPLL_C= 0;
	stAvPllCtrl3.RSVDxC_b8 = 0 ;

	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + RA_avPll_CTRL0 , stAvPllCtrl0.u32);
	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + RA_avPll_CTRL1 , stAvPllCtrl1.u32);
	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + RA_avPll_CTRL2 , stAvPllCtrl2.u32);
	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + RA_avPll_CTRL3 , stAvPllCtrl3.u32);

	BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + RA_avPll_CTRL0 , stAvPllCtrl0.u32);
	BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + RA_avPll_CTRL1 , stAvPllCtrl1.u32);
	BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + RA_avPll_CTRL2 , stAvPllCtrl2.u32);
	BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + RA_avPll_CTRL3 , stAvPllCtrl3.u32);

	BFM_HOST_Bus_Read32(uiAvPll_A_BaseAddr + RA_avPll_CTRL0 , &reg1);
	BFM_HOST_Bus_Read32(uiAvPll_A_BaseAddr + RA_avPll_CTRL1 , &reg2);
	BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr + RA_avPll_CTRL0 , &reg3);
	BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr + RA_avPll_CTRL1 , &reg4);

	dbg_printf("AVPLL A CTRL0:%x,AVPLL A CTRL1:%x,AVPLL B CTRL0:%x,AVPLL B CTRL1:%x,\n", reg1, reg2, reg3, reg4);

	//speed is not defaut value, then pll must has been initialized.
	BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr + RA_avPll_CTRL0 , &stAvPllCtrl0_B.u32);
	if(stAvPllCtrl0_B.uCTRL0_SPEED != diag_avpllRegSPEED[0])
		avpll_initialized=1;

	//turn on the reset_A and reset_B
	//these should be always on
	BFM_HOST_Bus_Read32(uiAvPll_A_BaseAddr +  RA_avPll_CTRL2, &stAvPllCtrl2_A.u32);
	BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr +  RA_avPll_CTRL2, &stAvPllCtrl2_B.u32);
	stAvPllCtrl2_A.uCTRL2_RESET_C = 0;
	stAvPllCtrl2_B.uCTRL2_RESET_C = 0;
	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr +  RA_avPll_CTRL2, stAvPllCtrl2_A.u32);
	if(!avpll_initialized)
		BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr +  RA_avPll_CTRL2, stAvPllCtrl2_B.u32);

	//turn on offset
	//these should be always on
	for(chId=1; chId<=7; chId++)
	{
		BFM_HOST_Bus_Read32(uiAvPll_A_BaseAddr + gauchChannelAddr[chId - 1] +  RA_avPllCh_CTRL0 , &stAvPllChCtrl0_A.u32);
		BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr + gauchChannelAddr[chId - 1] +  RA_avPllCh_CTRL0 , &stAvPllChCtrl0_B.u32);
		stAvPllChCtrl0_A.uCTRL0_RESERVE = 1;
		stAvPllChCtrl0_B.uCTRL0_RESERVE = 1;
		BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + gauchChannelAddr[chId - 1] +  RA_avPllCh_CTRL0 , stAvPllChCtrl0_A.u32);
		if(!avpll_initialized)
			BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + gauchChannelAddr[chId - 1] +  RA_avPllCh_CTRL0 , stAvPllChCtrl0_B.u32);
	}

	BFM_HOST_Bus_Read32(uiAvPll_A_BaseAddr + gauchChannelAddr[7] + RA_avPllCh8_CTRL0 , &stAvPllCh8Ctrl0_A.u32);
	BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr + gauchChannelAddr[7] + RA_avPllCh8_CTRL0 , &stAvPllCh8Ctrl0_B.u32);
	stAvPllCh8Ctrl0_A.uCTRL0_RESERVE = 1;
	stAvPllCh8Ctrl0_B.uCTRL0_RESERVE = 1;
	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + gauchChannelAddr[7] + RA_avPllCh8_CTRL0 , stAvPllCh8Ctrl0_A.u32);
	if(!avpll_initialized)
		BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + gauchChannelAddr[7] + RA_avPllCh8_CTRL0 , stAvPllCh8Ctrl0_B.u32);

	//set p9vi and icp to correct value
	BFM_HOST_Bus_Read32(uiAvPll_A_BaseAddr + RA_avPll_CTRL0 , &stAvPllCtrl0_A.u32);
	BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr + RA_avPll_CTRL0 , &stAvPllCtrl0_B.u32);
	BFM_HOST_Bus_Read32(uiAvPll_A_BaseAddr + RA_avPll_CTRL1 , &stAvPllCtrl1_A.u32);
	BFM_HOST_Bus_Read32(uiAvPll_B_BaseAddr + RA_avPll_CTRL1 , &stAvPllCtrl1_B.u32);
	stAvPllCtrl0_A.uCTRL0_VDDR_0P9V = 0x0A;
	stAvPllCtrl0_B.uCTRL0_VDDR_0P9V = 0x0A;
	stAvPllCtrl0_A.uCTRL0_VTHVCOCAL = 0x2;
	stAvPllCtrl0_B.uCTRL0_VTHVCOCAL = 0x2;
	stAvPllCtrl1_A.uCTRL1_ICP = 0x5;
	stAvPllCtrl1_B.uCTRL1_ICP = 0x5;
	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + RA_avPll_CTRL0 , stAvPllCtrl0_A.u32);
	BFM_HOST_Bus_Write32(uiAvPll_A_BaseAddr + RA_avPll_CTRL1 , stAvPllCtrl1_A.u32);
	if(!avpll_initialized)
	{
		BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + RA_avPll_CTRL0 , stAvPllCtrl0_B.u32);
		BFM_HOST_Bus_Write32(uiAvPll_B_BaseAddr + RA_avPll_CTRL1 , stAvPllCtrl1_B.u32);
	}

        diag_assertReset(AVPLL_A);
#if HAS_AVPLL_B
        if(!avpll_initialized)
                diag_assertReset(AVPLL_B);
#endif

        diag_setVCO(AVPLL_A, AVPLL_VCO_FREQ_2_227G);
        if(!avpll_initialized)
                diag_setVCO(AVPLL_B, AVPLL_VCO_FREQ_1_620G);
        //power up
        diag_powerUp(AVPLL_A);
#if HAS_AVPLL_B
        if(!avpll_initialized)
                diag_powerUp(AVPLL_B);
#endif

        //reset, has delay inside function
        diag_deassertReset(AVPLL_A);
#if HAS_AVPLL_B
        if(!avpll_initialized)
                diag_deassertReset(AVPLL_B);
#endif

        //caliberate
        diag_calibrate(AVPLL_A);
#if HAS_AVPLL_B
        if(!avpll_initialized)
                diag_calibrate(AVPLL_B);
#endif

        //set up VCO frequency.
        diag_setVCO(AVPLL_A, AVPLL_VCO_FREQ_2_227G);
#if HAS_AVPLL_B
        if(!avpll_initialized)
                diag_setVCO(AVPLL_B, AVPLL_VCO_FREQ_1_620G);
#endif

        dbg_printf(PRN_RES, "Enabled AvPLL A and B Channels\n");

	//Enable Clokcs from AVPLL-A to channels
	BFM_HOST_Bus_Read32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), &regClkEnable.u32);
	regClkEnable.u32 = 0x3F;
	BFM_HOST_Bus_Write32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), regClkEnable.u32);

	dbg_printf(PRN_RES, "Enable DVIO Pad\n");
	BFM_HOST_Bus_Read32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_PadSelect), &regPadSelect.u32);
	regPadSelect.uPadSelect_DVIO_OEN = 1;
	BFM_HOST_Bus_Write32((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_PadSelect), regPadSelect.u32);

	for (i=0; i<8; i++) {
		gAVPLLA_Channel_OutputClock[i] = 0;
		gAVPLLB_Channel_OutputClock[i] = 0;
        }

        diag_videoFreq(AVPLL_A, 5, 0, 0, 1, 1); // set default video0clk to 148.5M
        diag_videoFreq(AVPLL_A, 5, 0, 0, 1, 2); // set default video1clk to 148.5M
#if HAS_AVPLL_B
        diag_videoFreq(AVPLL_B, 1, 2, 0, 1, 1); // set default video1clk to 148.5M
#endif

}

int diag_calibrate(int avpllGroupId)
{
	volatile int i;
	unsigned int uiAvPllBase = 0;
	T32avPll_CTRL1 stAvPllCtrl1;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
                return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	//turn off caliberation
	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL1, &stAvPllCtrl1.u32);
	stAvPllCtrl1.uCTRL1_PLL_CALI_START = 0;;
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL1, stAvPllCtrl1.u32);

	//add some delay
	for(i=0; i<10000; i++);

	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL1, &stAvPllCtrl1.u32);
	stAvPllCtrl1.uCTRL1_PLL_CALI_START = 1;
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL1, stAvPllCtrl1.u32);
	return AVPLL_E_OK;
}

int diag_avpllFreq(int avpllGroupId, int chId)
{
	double vco_freq;
	double freq_from_dpll, freq_from_offset;
	int offset, sync1, sync2, divHDMI, divAV1, divAV2, divAV3;
	int enDPLL;
	unsigned int uiAvPllBase = 0;
	T32avPllCh_CTRL0 stAvPllChCtrl0;
	T32avPllCh_CTRL1 stAvPllChCtrl1;
	T32avPllCh_CTRL2 stAvPllChCtrl2;
	T32avPllCh_CTRL3 stAvPllChCtrl3;
	T32avPll_CTRL3 stAvPllCtrl3;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
		return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
                return AVPLL_E_BADPARAM;
#endif

	uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	if(avpllGroupId == AVPLL_A)
		vco_freq = diag_vcoFreqs[diag_pll_A_VCO_Setting];
	else
		vco_freq = diag_vcoFreqs[diag_pll_B_VCO_Setting];

	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL3, &stAvPllCtrl3.u32);
	enDPLL = stAvPllCtrl3.uCTRL3_EN_DPLL_C & (1<<(chId-1));

	//find offset, sync1, sync2, divHDMI, divAV1, divAV2, divAV3 for this channel
	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL0, &stAvPllChCtrl0.u32);
	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL1, &stAvPllChCtrl1.u32);
	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL2, &stAvPllChCtrl2.u32);
	BFM_HOST_Bus_Read32(uiAvPllBase + gauchChannelAddr[chId-1] + RA_avPllCh_CTRL3, &stAvPllChCtrl3.u32);
	offset = stAvPllChCtrl0.uCTRL0_FREQ_OFFSET;
	sync1 =  stAvPllChCtrl1.uCTRL1_P_SYNC1;
	sync2 =  stAvPllChCtrl2.uCTRL2_P_SYNC2;
	divHDMI= stAvPllChCtrl3.uCTRL3_DIV_HDMI;
	divAV1 = stAvPllChCtrl3.uCTRL3_DIV_AV1;
	divAV2 = stAvPllChCtrl3.uCTRL3_DIV_AV2;
	divAV3 = stAvPllChCtrl3.uCTRL3_DIV_AV3;


	if(enDPLL != 0)
		freq_from_dpll = vco_freq*sync2/sync1;
	else
		freq_from_dpll = vco_freq;

	if(offset& (1<<18))
	{
		offset = - (offset & ((1<<18)-1));
	}
	freq_from_offset = vco_freq/((double)offset/4194304+1);

	//figure out which divider used
	if(divHDMI != 0)
	{
		divHDMI &= 0x3;
		dbg_printf(PRN_RES, "divHDMI on: %d\n", divHDMI);

		freq_from_dpll = freq_from_dpll/divHDMICode[divHDMI];
		freq_from_offset = freq_from_offset/divHDMICode[divHDMI];
	}

	if(divAV1 != 0)
	{
		divAV1 &= 0x3;
		dbg_printf(PRN_RES, "divAV1 on: %d\n", divAV1);

		freq_from_dpll = freq_from_dpll/divAV1Code[divAV1];
		freq_from_offset = freq_from_offset/divAV1Code[divAV1];
	}

	if(divAV2 != 0)
	{
		if(divAV3 == 0)
		{
			dbg_printf(PRN_RES, "divAV2 on: %d\n", divAV2);
			freq_from_dpll = freq_from_dpll/divAV2;
			freq_from_offset = freq_from_offset/divAV2;
		}
		else
		{
			dbg_printf(PRN_RES, "divAV2 on: %d\n", divAV2);
			dbg_printf(PRN_RES, "divAV3 on: %d\n", divAV3);
			if( (((int)((double)divAV2/4+.5))-1) != (divAV3&0x7))
			{
				dbg_printf(PRN_RES, "Invalid divAV2 and divAV3 combination\n");
			}
			freq_from_dpll = freq_from_dpll*2/divAV2;
			freq_from_offset = freq_from_offset*2/divAV2;
		}
	}

	dbg_printf(PRN_RES, "PLLA C%d Freq is: %fMHz(Offset) %fMHz(DPLL)\n", chId, freq_from_offset, freq_from_dpll);

#if 0
	if (chId == 1)
		avpllA1 = freq_from_dpll;
#endif
	current_freq[avpllGroupId][chId] = freq_from_dpll;

	return AVPLL_E_OK;
}


int diag_vcoFreq(int avpllGroupId)
{
	double ref_freq=25.0;
	double freq_from_offset;
	int offset, fbDiv;
	unsigned int uiAvPllBase = 0;
	T32avPllCh_CTRL0 stAvPllChCtrl0;
	T32avPll_CTRL1 stAvPllCtrl1;

#if HAS_AVPLL_B
        if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
                return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

        uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_C8 + RA_avPllCh_CTRL0, &stAvPllChCtrl0.u32);
	offset = stAvPllChCtrl0.uCTRL0_FREQ_OFFSET;

	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL1 , &stAvPllCtrl1.u32);
	fbDiv = stAvPllCtrl1.uCTRL1_FBDIV;

	if(offset& (1<<18))
	{
		offset = - (offset & ((1<<18)-1));
	}
	//note that the offset formula for VCO(C8) is different from C1-C7
	//according to John Ma, the formula is the same, just that ref_freq is CKOUT
	//and VCO freq is CKIN
	freq_from_offset = ref_freq*((double)offset/4194304+1);

	freq_from_offset = freq_from_offset*fbDiv;

	dbg_printf(PRN_RES, "PLLA VCO Freq is: %f\n", freq_from_offset);

	return AVPLL_E_OK;
}

int diag_getVCOFreq(int hdmiMode, int freqIndex)
{
    return (vcoSelectionTable[hdmiMode][freqIndex]);
}


int diag_getChannelOutputFreq(int AVPLLIndex,int chID)
{
    if (chID<0 || chID>7)
        return 0;

    if (AVPLLIndex == 0) //AVPLL-A
        return (gAVPLLA_Channel_OutputClock[chID]);
    else
        return (gAVPLLB_Channel_OutputClock[chID]);
}


int AVPLL_GetVcoFreq(int hdmiMode, int freqIndex)
{
	int uiVcoFreq = 0;

	uiVcoFreq = diag_getVCOFreq(hdmiMode, freqIndex);

	return uiVcoFreq;
}

int AVPLL_GetChannelOutputFreq(int AVPLLIndex, int chID)
{
	int uiChannelOutFreq = 0;

	if (chID<0 || chID>7)
                return 0;

	uiChannelOutFreq = diag_getChannelOutputFreq(AVPLLIndex, chID);

	return uiChannelOutFreq;
}


//freq_index, 0 to 5, the 6 VCLKs
//hdmiMode, HDMI_8BIT_MODE, HDMI_10BIT_MODE, HDMI_12BIT_MODE
//frameRate, FRAME_RATE_59P94 FRAME_RATE_60
//overSampleRate, 1, 2, 4, 8, 10, 12.5, 15
int diag_videoFreq(int avpllGroupId, int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId)
{

	unsigned int uiAvPllBase = 0;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
                return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

        uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	int vcoFreqIndex=vcoSelectionTable[hdmiMode][freqIndex];
	double vcoFreq, videoFreq;
	int ppm1K=ppm1kSelectionTable[frameRate][freqIndex];
	double divider;

        if (chId<0 || chId>7)
            return;

        dbg_printf(PRN_RES, "Set PLLA C%d:\n", chId);
        //set VCO freq here
        diag_setVCO(avpllGroupId, vcoFreqIndex);
        dbg_printf(PRN_RES, "VCO freq=%f\n", diag_vcoFreqs[vcoFreqIndex]);

        //check to see if ppm1K is need
        vcoFreq = diag_vcoFreqs[vcoFreqIndex];
        if(ppm1K<0)
        {
                    vcoFreq = vcoFreq*1000/1001;
        }

        if(ppm1K>0)
        {
                    vcoFreq = vcoFreq*1001/1000;
        }
        diag_set1KPPM(avpllGroupId, ppm1K, chId);

        //get the video freq
        videoFreq = diag_videoFreqs[freqIndex];
        if(frameRate)
        {
                    //60Hz vclk is 1001/1000 times 59.94Hz vclk
                    videoFreq = videoFreq*1001/1000;
        }

        dbg_printf(PRN_RES, "Video freq=%f\n", videoFreq);

        //disable all dividers
        diag_setHDMI_Div(avpllGroupId, 0, chId);
        diag_setAV_Div(avpllGroupId, 0, 0, chId); //divAV1 is for divide by 1, 0 will disable the whole divAV chain
        diag_setAV3_Div(avpllGroupId, 0, 0, chId);

        //set the divider
        if(overSampleRate>=10)
        {
                    //use HDMI divider
                    divider = vcoFreq/videoFreq/overSampleRate;
                    dbg_printf(PRN_RES, "HDMI divider is %f %d\n", divider, (int)(divider+0.5));

                    diag_setHDMI_Div(avpllGroupId, (int)(divider+0.5), chId);
                    dbg_printf(PRN_RES, "PLLA C%d=%f\n", chId, videoFreq*overSampleRate);
        }
        else
        {
                    divider = vcoFreq*2/videoFreq/overSampleRate;
                    dbg_printf(PRN_RES, "AV divider is %f %d\n", divider/2, (int)(divider+0.5)/2);

                    if(((int)(divider+0.5))&1)
                    {
                                //fractional divider, use AV2 and AV3
                                //figure out AV3, AV3 = round(AV2/4)-1
                                int divAV3 = ((int)((double)divider/4+0.5))-1;

                                //this enables divAV channel
                                diag_setAV_Div(avpllGroupId, 4, 0, chId);

                                //add enable bit
                                divAV3 |= (1<<3);
                                diag_setAV3_Div(avpllGroupId, (int)(divider+0.5), divAV3, chId);
                    }
                    else
                    {
                                //integer divider, use AV2 only
                                diag_setAV_Div(avpllGroupId, 4, (int)(divider+0.5)/2, chId);
                    }

                    dbg_printf(PRN_RES, "PLLA C%d=%f\n", chId, videoFreq*overSampleRate);
        }

	if(AVPLL_A == avpllGroupId)
		gAVPLLA_Channel_OutputClock[chId] = (unsigned int)(videoFreq*overSampleRate*1000000);
	else
		gAVPLLB_Channel_OutputClock[chId] = (unsigned int)(videoFreq*overSampleRate*1000000);

	return AVPLL_E_OK;
}


//freqIndex is the index into clock_freq[] array
int diag_clockFreq(int avpllGroupId, int freqIndex, int chId)
{
	unsigned int uiAvPllBase = 0;
	int diag_pll_VCO_Setting = 0;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
                return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

        uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];


	if(avpllGroupId == AVPLL_A)
		diag_pll_VCO_Setting = diag_pll_A_VCO_Setting;
	else
		diag_pll_VCO_Setting = diag_pll_B_VCO_Setting;

	if(freqIndex >= (sizeof(clk_freqs) / sizeof(CLK_FREQ)))
	{
		dbg_printf(PRN_RES, "freq Index not found\n");
		return 1;
	}

	if(diag_pll_VCO_Setting != clk_freqs[freqIndex].vco_freq_index)
	{
		dbg_printf(PRN_RES, "VCO frequency is changed to %f!\n",
			diag_vcoFreqs[clk_freqs[freqIndex].vco_freq_index]);

		diag_setVCO(avpllGroupId, clk_freqs[freqIndex].vco_freq_index);
	}

	//change offset
	diag_setChanOffset(avpllGroupId, clk_freqs[freqIndex].freq_offset, chId);

	//change p_sync
	diag_setDPll(avpllGroupId, (clk_freqs[freqIndex].p_sync1!=0),
			clk_freqs[freqIndex].p_sync1,
			clk_freqs[freqIndex].p_sync2, chId);

	//disable all dividers
	diag_setHDMI_Div(avpllGroupId, 0, chId);
	diag_setAV_Div(avpllGroupId, 0, 0, chId);
	diag_setAV3_Div(avpllGroupId, 0, 0, chId);

	//update now div
	diag_setAV_Div(avpllGroupId, clk_freqs[freqIndex].divAV1, clk_freqs[freqIndex].divAV2, chId);
	diag_setAV3_Div(avpllGroupId, clk_freqs[freqIndex].divAV2, clk_freqs[freqIndex].divAV3, chId);

	return 0;
}


//freq_index, 0 to 5, the 6 VCLKs
//hdmiMode, HDMI_8BIT_MODE, HDMI_10BIT_MODE, HDMI_12BIT_MODE
//frameRate, FRAME_RATE_59P94 FRAME_RATE_60
//overSampleRate, 1, 2, 4, 8, 10, 12.5, 15
int AVPLL_SetVideoFreq(int avpllGroupId, int freqIndex, int hdmiMode, int frameRate, float overSampleRate, int chId)
{
	unsigned int uiErrCode = 0;

	if(hdmiMode < HDMI_8BIT_MODE || hdmiMode > HDMI_MAX_MODE)
		return AVPLL_E_BADPARAM;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
                return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
                return AVPLL_E_BADPARAM;
#endif

	uiErrCode = diag_videoFreq(avpllGroupId, freqIndex, hdmiMode, frameRate, overSampleRate, chId);

	return uiErrCode;
}

int get_freqTabIndex(int vco_freq_index, int av_freq_index)
{
	int i, tab_index=-1;

	for(i=0; i<(sizeof(clk_freqs) / sizeof(CLK_FREQ)); i++)
	{
		if(clk_freqs[i].vco_freq_index == vco_freq_index && clk_freqs[i].av_freq_index == av_freq_index)
		{
			tab_index = i; break;
		}
	}

	return tab_index;
}

int AVPLL_Set(int groupId, int chanId, int avFreqIndex)
{
	int freqTabIndex;

	if(groupId==0)	// PLL A
	{
		// Search freqIndex in lookup table by avFreqIndex and vco_freq_index.
		freqTabIndex = get_freqTabIndex(diag_pll_A_VCO_Setting, avFreqIndex);
		if(freqTabIndex == -1)
			return -1;
		diag_clockFreq(AVPLL_A, freqTabIndex, chanId);
	}
#if HAS_AVPLL_B
	else if(groupId==1) // PLL B
	{
		freqTabIndex = get_freqTabIndex(diag_pll_B_VCO_Setting, avFreqIndex);
		if(freqTabIndex == -1)
			return -1;

		diag_clockFreq(AVPLL_B, freqTabIndex, chanId);
	}
#endif

	return 0;
}

	//Fix Me -- (Satya) The below registers are not found
// secondary MCLK keeps 4x, other MCLK div4 to 1x
int AVPLL_SetDiv4()
{
	T32avioGbl_ACLK0_CTRL stClk0Switch;
	T32avioGbl_ACLK2_CTRL stClk2Switch;
	T32avioGbl_ACLK3_CTRL stClk3Switch;
#ifdef CONFIG_MV_AMP_INTERNAL_ADAC_DISABLE
	T32avioGbl_ACLK1_CTRL stClk1Switch;
#endif

	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK0_CTRL), &stClk0Switch.u32);
	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK2_CTRL), &stClk2Switch.u32);
	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK3_CTRL), &stClk3Switch.u32);

	stClk0Switch.uACLK0_CTRL_clkSwitch = 1;	// primary
	stClk2Switch.uACLK2_CTRL_clkSwitch = 1;	// mic
	stClk3Switch.uACLK3_CTRL_clkSwitch = 1;	// spdif

#ifdef CONFIG_MV_AMP_INTERNAL_ADAC_DISABLE
	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), &stClk1Switch.u32);
	stClk1Switch.uACLK1_CTRL_clkSwitch = 1;	// stereo
	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), stClk1Switch.u32);
#endif
	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK0_CTRL), stClk0Switch.u32);
	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK2_CTRL), stClk2Switch.u32);
	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK3_CTRL), stClk3Switch.u32);


	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK0_CTRL), &stClk0Switch.u32);
	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK2_CTRL), &stClk2Switch.u32);
	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK3_CTRL), &stClk3Switch.u32);

	stClk0Switch.uACLK0_CTRL_clkSel = avioGbl_ACLK0_CTRL_clkSel_d4;
	stClk2Switch.uACLK2_CTRL_clkSel = avioGbl_ACLK2_CTRL_clkSel_d4;
	stClk3Switch.uACLK3_CTRL_clkSel = avioGbl_ACLK3_CTRL_clkSel_d4;

	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK0_CTRL), stClk0Switch.u32);
	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK2_CTRL), stClk2Switch.u32);
	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK3_CTRL), stClk3Switch.u32);

#ifdef CONFIG_MV_AMP_INTERNAL_ADAC_DISABLE
	GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), &stClk1Switch.u32);
	stClk1Switch.uACLK1_CTRL_clkSel = avioGbl_ACLK1_CTRL_clkSel_d4;
	GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), stClk1Switch.u32);
#endif
}


	//Fix Me -- (Satya) The below registers are not found
// S/W workaround is not required at BG2.
void AVPLL_EnableMicClk(int en)
{
/*	T32avioGbl_AVPLLA_CLK_EN regClkEnable;
        BFM_HOST_Bus_Read32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), &regClkEnable.u32);
        if (en) {
                en = 1;
        }
        regClkEnable.uAVPLLA_CLK_EN_ctrl = en;
        BFM_HOST_Bus_Write32((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_AVPLLA_CLK_EN), regClkEnable.u32);*/
        return;
}

/* PCR_TRACKING -- BG2 */
// add read function here for adjustment, real adjustment to nominate frequency is cur-orig
void AVPLL_GetPPM(int grp, double *ppm_base, double *ppm_now)
{
        double ppm0, ppm;
        if(grp ==0)
        {
                ppm0 = org_ppm[0];
                ppm = cur_ppm[0];
        }
        else // AVPLL B
        {
                ppm0 = org_ppm[1];
                ppm = cur_ppm[1];
        }
        if(ppm_base) *ppm_base = ppm0;
        if(ppm_now) *ppm_now = ppm;
}


double AVPLL_AdjustPPM(double ppm_delta, int grp)
{
        volatile int i;
        double ppm0, ppm;
	unsigned int uiAvPllBase = 0;

		// For AVPLL-A
        if(grp == 0)
		uiAvPllBase = gauchAvpllBaseAddr[AVPLL_A];
	else
#if HAS_AVPLL_B
		uiAvPllBase = gauchAvpllBaseAddr[AVPLL_B];
#else
		return 0;
#endif

 	T32avPllCh8_CTRL0 stAvPllChCtrl0;
 	T32avPll_CTRL2 stAvPllCtrl2;
 	uiAvPllBase = gauchAvpllBaseAddr[AVPLL_A];


 	BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_C8 + RA_avPllCh8_CTRL0, &stAvPllChCtrl0.u32);

        if(avpll_inited[grp] == 0) {
                 ppm0 = offset_2_ppm(stAvPllChCtrl0.uCTRL0_FREQ_OFFSET);
                 cur_ppm[grp] = org_ppm[grp] = ppm0;
                 avpll_inited[grp] = 1;
        }else{
                 ppm0 = org_ppm[grp];
        }

        cur_ppm[grp] += ppm_delta;
        ppm = cur_ppm[grp];

        stAvPllChCtrl0.uCTRL0_FREQ_OFFSET = ppm_2_offset(ppm, org_ppm[grp]);
        BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_C8 + RA_avPllCh8_CTRL0, stAvPllChCtrl0.u32);

        BFM_HOST_Bus_Read32(uiAvPllBase + RA_avPll_CTRL2, &stAvPllCtrl2.u32);
        stAvPllCtrl2.uCTRL2_FREQ_OFFSET_READY_C = 0x80;
        BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2, stAvPllCtrl2.u32);

        for(i=0; i<10000; i++);

        stAvPllCtrl2.uCTRL2_FREQ_OFFSET_READY_C = 0;
	BFM_HOST_Bus_Write32(uiAvPllBase + RA_avPll_CTRL2, stAvPllCtrl2.u32);

        MVLOG(MOD_AVS, DBG_MID, "-- cur_ppm: %.1lf org_ppm: %.1lf diff: %.1lf\n", ppm, ppm0, ppm - ppm0);
        return ppm - ppm0;
}

void AVPLL_InitClock(void)
{
    T32Gbl_I2S2_LRCKCntl LRCKCntl;
    T32Gbl_I2S2_BCLKCntl BCLKCntl;
    T32avioGbl_ACLK0_CTRL stClk0Switch;
    T32avioGbl_ACLK1_CTRL stClk1Switch;
    T32avioGbl_ACLK2_CTRL stClk2Switch;
    T32avioGbl_ACLK3_CTRL stClk3Switch;

    GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK0_CTRL), &stClk0Switch.u32);
    GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), &stClk1Switch.u32);
    GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK2_CTRL), &stClk2Switch.u32);
    GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK3_CTRL), &stClk3Switch.u32);

    stClk0Switch.uACLK0_CTRL_clkSwitch = 0;	// source clock
    stClk0Switch.uACLK0_CTRL_clkD3Switch = 0;	// non DIV3 clock
    stClk1Switch.uACLK1_CTRL_clkSwitch = 0;
    stClk1Switch.uACLK1_CTRL_clkD3Switch = 0;
    stClk2Switch.uACLK2_CTRL_clkSwitch = 0;
    stClk2Switch.uACLK2_CTRL_clkD3Switch = 0;
    stClk3Switch.uACLK3_CTRL_clkSwitch = 0;
    stClk3Switch.uACLK3_CTRL_clkD3Switch = 0;


    GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK0_CTRL), stClk0Switch.u32);
    GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), stClk1Switch.u32);
    GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK2_CTRL), stClk2Switch.u32);
    GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK3_CTRL), stClk3Switch.u32);


    GA_REG_WORD32_READ((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), &stClk1Switch.u32);
    stClk1Switch.uACLK1_CTRL_pllSel = 1; 	//Select A3
    GA_REG_WORD32_WRITE((MEMMAP_AVIO_GBL_BASE + RA_avioGbl_ACLK1_CTRL), stClk1Switch.u32);

    // Set LRClk and BCLK to Pull Down.
    GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_LRCKCntl), (&LRCKCntl.u32));
    LRCKCntl.uI2S2_LRCKCntl_PD_EN = 1;
    GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_LRCKCntl), (LRCKCntl.u32));

    GA_REG_WORD32_READ((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_BCLKCntl), (&BCLKCntl.u32));
    BCLKCntl.uI2S2_BCLKCntl_PD_EN = 1;
    GA_REG_WORD32_WRITE((MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_I2S2_BCLKCntl), (BCLKCntl.u32));
}

int resetAvPll(int avpllGroupId)
{

	unsigned int uiAvPllBase = 0;

#if HAS_AVPLL_B
	if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
                return AVPLL_E_BADPARAM;
#else
	if(avpllGroupId != AVPLL_A)
		return AVPLL_E_BADPARAM;
#endif

        uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];

	diag_assertReset(avpllGroupId);
	diag_deassertReset(avpllGroupId);

	return AVPLL_E_OK;
}


//freqIndex is the index into clk_freqs_computed[] array
int diag_clockFreq_computed(int avpllGroupId, int freqIndex, int chId)
{
#if 0
	int diag_pll_VCO_Setting;
	unsigned int uiAvPllBase = 0;

        if(avpllGroupId < AVPLL_A || avpllGroupId > AVPLL_B)
                return AVPLL_E_BADPARAM;

        uiAvPllBase =  gauchAvpllBaseAddr[avpllGroupId];


	if(avpllGroupId == AVPLL_A)
		diag_pll_VCO_Setting=diag_pll_A_VCO_Setting;
	else
		diag_pll_VCO_Setting=diag_pll_B_VCO_Setting;

	if(freqIndex >= (sizeof(clk_freqs_computed) / sizeof(CLK_FREQ)))
	{
		dbg_printf(PRN_RES, "VCO Freq Index not found\n");
		return 1;
	}

	if(clk_freqs_computed[freqIndex].invalid)
	{
		dbg_printf(PRN_RES, "Frequency entry is invalid!\n");
		return 1;
	}

	if(diag_pll_VCO_Setting != clk_freqs_computed[freqIndex].vco_freq_index)
	{
		dbg_printf(PRN_RES, "VCO frequency is changed to %f!\n",
			diag_vcoFreqs[clk_freqs_computed[freqIndex].vco_freq_index]);

		diag_setVCO(uiAvPllBase, clk_freqs_computed[freqIndex].vco_freq_index);
	}

	//change offset
	diag_setChanOffset(uiAvPllBase, clk_freqs_computed[freqIndex].freq_offset, chId);

	//change p_sync
	diag_setDPll(uiAvPllBase, (clk_freqs_computed[freqIndex].p_sync1!=0),
			clk_freqs_computed[freqIndex].p_sync1,
			clk_freqs_computed[freqIndex].p_sync2, chId);

	//disable all dividers
	diag_setHDMI_Div(uiAvPllBase, 0, chId);
	diag_setAV_Div(uiAvPllBase, 0, 0, chId);
	diag_setAV3_Div(uiAvPllBase, 0, 0, chId);

	//update now div
	diag_setAV_Div(uiAvPllBase, clk_freqs_computed[freqIndex].divAV1, clk_freqs_computed[freqIndex].divAV2, chId);
	diag_setAV3_Div(uiAvPllBase, clk_freqs_computed[freqIndex].divAV2, clk_freqs_computed[freqIndex].divAV3, chId);

	//dbg_printf(PRN_RES, "set A%d to %fMHz\n", chId, clk_freqs_computed[freqIndex].clkout_freq);

	return 0;
#endif
}


#endif // <- #if (BERLIN_CHIP_VERSION < BERLIN_BG2_Q)
#endif // <- #if defined(BERLIN)

#endif