mcu/driver/devdrv/pll/src/pll_gen95.c

/*****************************************************************************
*  Copyright Statement:
*  --------------------
*  This software is protected by Copyright and the information contained
*  herein is confidential. The software may not be copied and the information
*  contained herein may not be used or disclosed except with the written
*  permission of MediaTek Inc. (C) 2012
*
*  BY OPENING THIS FILE, BUYER HEREBY UNEQUIVOCALLY ACKNOWLEDGES AND AGREES
*  THAT THE SOFTWARE/FIRMWARE AND ITS DOCUMENTATIONS ("MEDIATEK SOFTWARE")
*  RECEIVED FROM MEDIATEK AND/OR ITS REPRESENTATIVES ARE PROVIDED TO BUYER ON
*  AN "AS-IS" BASIS ONLY. MEDIATEK EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES,
*  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED WARRANTIES OF
*  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE OR NONINFRINGEMENT.
*  NEITHER DOES MEDIATEK PROVIDE ANY WARRANTY WHATSOEVER WITH RESPECT TO THE
*  SOFTWARE OF ANY THIRD PARTY WHICH MAY BE USED BY, INCORPORATED IN, OR
*  SUPPLIED WITH THE MEDIATEK SOFTWARE, AND BUYER AGREES TO LOOK ONLY TO SUCH
*  THIRD PARTY FOR ANY WARRANTY CLAIM RELATING THERETO. MEDIATEK SHALL ALSO
*  NOT BE RESPONSIBLE FOR ANY MEDIATEK SOFTWARE RELEASES MADE TO BUYER'S
*  SPECIFICATION OR TO CONFORM TO A PARTICULAR STANDARD OR OPEN FORUM.
*
*  BUYER'S SOLE AND EXCLUSIVE REMEDY AND MEDIATEK'S ENTIRE AND CUMULATIVE
*  LIABILITY WITH RESPECT TO THE MEDIATEK SOFTWARE RELEASED HEREUNDER WILL BE,
*  AT MEDIATEK'S OPTION, TO REVISE OR REPLACE THE MEDIATEK SOFTWARE AT ISSUE,
*  OR REFUND ANY SOFTWARE LICENSE FEES OR SERVICE CHARGE PAID BY BUYER TO
*  MEDIATEK FOR SUCH MEDIATEK SOFTWARE AT ISSUE.
*
*  THE TRANSACTION CONTEMPLATED HEREUNDER SHALL BE CONSTRUED IN ACCORDANCE
*  WITH THE LAWS OF THE STATE OF CALIFORNIA, USA, EXCLUDING ITS CONFLICT OF
*  LAWS PRINCIPLES.  ANY DISPUTES, CONTROVERSIES OR CLAIMS ARISING THEREOF AND
*  RELATED THERETO SHALL BE SETTLED BY ARBITRATION IN SAN FRANCISCO, CA, UNDER
*  THE RULES OF THE INTERNATIONAL CHAMBER OF COMMERCE (ICC).
*
*****************************************************************************/

/*******************************************************************************
 * Filename:
 * ---------
 *   pll_gen95.c
 *
 * Project:
 * --------
 *   UMOLYA
 *
 * Description:
 * ------------
 *   PLL Related Functions
 *
 * Author:
 * -------
 * -------
 *
 * ==========================================================================
 * $Log$
 *
 * 08 22 2018 jun-ying.huang
 * [MOLY00347832] [DFR][Gen95] Assertion Remove
 * .
 *
 * 07 26 2018 jun-ying.huang
 * [MOLY00342533] [DFR][Gen95] Assertion Review-Change ASSERT to DEBUG_ASSERT
 * .
 *
 * 07 25 2018 jun-ying.huang
 * [MOLY00342276] [MT6779]Add MT6779 macro for LATIFE
 * .
 *
 * 06 26 2018 jun-ying.huang
 * [MOLY00335159] [Gen95] MCU PLL Change DVFS Config To 95B
 * Apply 95B PLL init for Eiger.
 *
 * 05 16 2018 jun-ying.huang
 * [MOLY00325066] [MT3967][PLL]Update PLL golden setting
 * Update 26M settle time and remove redundant function.
 *
 * 05 10 2018 jun-ying.huang
 * [MOLY00325066] [MT3967][PLL]Update PLL golden setting
 * Update Init MDVDSPPLL 850M->1000M, MDBRPPLL 600M->745M. Request by Kevin-KH Liu
 *
 * 05 09 2018 jun-ying.huang
 * [MOLY00325066] [MT3967][PLL]Update PLL golden setting
 * Update 26M settle time
 *
 * 03 12 2018 jun-ying.huang
 * [MOLY00312896] [Eiger][PLL]Update 26M settle time
 * Update PLL golden setting and update 26M settle time to 143T
 *
 * 02 13 2018 jun-ying.huang
 * [MOLY00308378] [MT3967][PLL]Corrent MDPLL setting by DE
 * .
 *
 * 01 17 2018 jun-ying.huang
 * [MOLY00302798] [MT3967][PLL]Update ABBPLL_SETTLE_26M to 29us
 * .
 *
 * 01 04 2018 jun-ying.huang
 * [MOLY00299837] [MT3967][PLL]Update 26M settle time
 * .
 *
 * 12 11 2017 jun-ying.huang
 * [MOLY00295410] [PLL][MT3967] Add Macro for MT3967
 * .
 *
 * 09 26 2017 jun-ying.huang
 * [MOLY00261263] [MT6295M]Update PLL driver
 * Update PLL init for 95B
 *
 *
 ****************************************************************************/

#ifdef __MTK_TARGET__ /* should NOT be compiled on MODIS */

/*******************************************************************************
 * Locally Used Options
 *******************************************************************************/

/*******************************************************************************
 * Include header files
 *******************************************************************************/

#include "pll.h"
#include "kal_public_api.h"
#include "sync_data.h"
#include "us_timer.h"

#define PLL_FM_WIMDOW       (0x1FF)

/* Below for debugging */
const char PLL_FM_clock[PLL_FM_NUM][32] =
{
	"MDBPI_PLL_D4",            /* 0 */
	"MDBPI_PLL_D6",
	"MDSYS_MML2_CLOCK",
	"FESYS_RXAGC_CLOCK",
	"MDRXSYS_DFESYNC_CLOCK",
	"FESYS_F208M_CLOCK",       /* 5 */
	"TRACE_MON_CLOCK",
	"MDSYS_208M_CLOCK",	
	"MDRXSYS_RAKE_CLOCK",
	"MDRXSYS_BRP_CLOCK",
	"MDRXSYS_VDSP_CLOCK",      /* 10 */
	"MDTOP_LOG_ATB_CLOCK",		
	"FESYS_CSYS_CLOCK",	
	"FESYS_BSI_CLOCK",		
	"MDSYS_MDCORE_CLOCK",	
	"MDSYS_BUS2X_NODCM_CLOCK", /* 15 */
	"MDSYS_BUS2X_CLOCK",	
	"MDTOP_DBG_CLOCK",		
	"AD_MDBPI_PLL_D7",	
	"AD_MDBPI_PLL_D5",		
	"AD_MDBPI_PLL_D4",         /* 20 */
	"AD_MDBPI_PLL_D3",	
	"AD_MDBPI_PLL_D2",	
	"AD_MDBRP_PLL",		
	"AD_MDVDSP_PLL",
	"AD_MDMCU_PLL",	           /* 25 */
/* below no use */	
	"null_26",	
	"null_27",		
	"null_28",	
	"null_29"   
};

PLL_CLK_INFO g_pll_info = {0};

/* Above for debugging */

/**
 * This function is used to detect ASIC or FPGA version of Palladium
 */
__PLL_CODE_IN_BOOT__ kal_bool PLL_FPGA_IS_ASIC(void)
{
#if defined(__FPGA__)
    kal_uint32 asic_flag = *((volatile kal_uint32 *)(0xA0000018)) & (0x1 << 7);

    if (asic_flag == 0)
        return KAL_TRUE;
    else
        return KAL_FALSE;
#else
    return KAL_TRUE;
#endif
}

__PLL_CODE_IN_BOOT__ void INT_SetPLL_Gen95(void)
{
#if defined(MT6295M) || defined(MT3967) || defined(MT6779) /* FPGA & EIGER & LAFITE */

    // Default md_srclkena_ack settle time = 154T 32K  
    *REG_MDTOP_PLLMIXED_DCXO_PLL_SETTLE_CTL = 0x02021C9A;

    //Change ABBPLL_SETTLE_26M to 0x2F2==>29us
    *REG_MDTOP_PLLMIXED_PLL_SETTLE_26M_CTL = 0x17920803;

    // set mdmcupll/mdvdsppll/mdbrppll posdiv to 0  
    *REG_MDTOP_PLLMIXED_MDMCUPLL_CTL1  &= (0x2 ^ 0xFFFFFFFF);
    *REG_MDTOP_PLLMIXED_MDVDSPPLL_CTL1 &= (0x2 ^ 0xFFFFFFFF);
    *REG_MDTOP_PLLMIXED_MDBRPPLL_CTL1  &= (0x101 ^ 0xFFFFFFFF);
    // set pll srouce from AP clock divider
    *REG_MDTOP_PLLMIXED_PLL_SRC_SEL = 0x0;
    // set pll clock enable in AP clock divider
    *REG_MDTOP_PLLMIXED_PLL_DIV_EN |= 0x080202;  
   		
    *REG_MDTOP_PLLMIXED_MDBPIPLL_CTL0  = 0x80114EC5; // Fixed Fvco = 1800Mhz. (/2)900M, (/3)600M, (/4)450M, (/5)360M, (/7)257M
    *REG_MDTOP_PLLMIXED_MDBRPPLL_CTL0  = 0x8023D200; // Fvco = 3725Mhz. 3725/5 = 745Mhz
    *REG_MDTOP_PLLMIXED_MDVDSPPLL_CTL0 = 0x801CD800; // Fvco = 3000Mhz. 3000/3 = 1000Mhz   
    *REG_MDTOP_PLLMIXED_MDMCUPLL_CTL0  = 0x801CD800; // Fvco = 3000Mhz. 3000/3 = 1000Mhz 
	
#else
    #error "Unsupported Chip Target in PLL Module" 
#endif

    MO_Sync();

    *REG_MDTOP_CLKSW_LOG_ATB_FLEXCKGEN_SEL  = 0x02; // LOG_ATB = 100Mhz (mdbpipll_d3_div2_ck/3) 
    *REG_MDTOP_CLKSW_MML2_DFS_FLEXCKGEN_SEL = 0x10; // MML2_DFS_PLLSEL=MDBPIPLL_D4 ; MML2_DFS_DIVSEL=divided-by 1

    MO_Sync();

    /*
     * Polling until MDMCUPLL complete frequency adjustment
     * Once MDMCUPLL complete, other PLL should complete too
     */
    while ((*REG_MDTOP_PLLMIXED_MDMCUPLL_STS >> 14)&0x1);

    /* PLL ON controlled by HW */
    *REG_MDTOP_PLLMIXED_PLL_ON_CTL = 0x0;

    /* Update ABB MDPLL control register default value */
    *REG_MDTOP_PLLMIXED_MDPLL_CTL1 = 0x04C63200;
    
    MO_Sync(); 

  #if defined(__PALLADIUM__)
    if (PLL_FPGA_IS_ASIC() == KAL_TRUE) {
        while ((*REG_MDTOP_CLKSW_MDSYS_BUS_FLEXCKGEN_STS & 0x8000) != 0x8000);
    }
  #else // Not PALLADIUM
    #if !defined(__FPGA__)
    /*
     * Wait MD bus clock ready
     * Once MD bus ready, other clock should be ready too
     * In FPGA, the following status checking must be removed since there is no flex ck gen in FPGA.
     */
    while ((*REG_MDTOP_CLKSW_MDSYS_BUS_FLEXCKGEN_STS & 0x8000) != 0x8000);
    #endif // __FPGA__
  #endif // __PALLADIUM__
  
    /* Switch clock source to PLL */
    *REG_MDTOP_CLKSW_CLKSEL_CTL |= 0x3;  //switch MDMCU & MD BUS clock to PLL frequency 
    MO_Sync();
    *REG_MDTOP_CLKSW_CLKSEL_CTL |= 0x31811F5C;  //switch all clock to PLL frequency
    MO_Sync();
    *REG_MDTOP_CLKSW_CLKON_CTL = 0x1;           //Turn off all SW clock request, except ATB
    MO_Sync();    
    *REG_MDTOP_CLKSW_SDF_CK_CTL |= 0x11; // switch SDF clock to PLL frequency
    MO_Sync();
    
    // Clear PLL ADJ RDY IRQ fired by initial period adjustment
    *REG_MDTOP_PLLMIXED_PLL_HP_RDY_IRQ = 0xFFFF;
    MO_Sync();  
    
    // Mask all PLL ADJ RDY IRQ
    *REG_MDTOP_PLLMIXED_PLL_HP_RDY_IRQ_MASK = 0xFFFF;
    MO_Sync();
  
    // Make a record that means MD pll has been initialized. 
    *REG_MDTOP_PLLMIXED_PLL_DUMMY = MD_PLL_MAGIC_NUM;
    MO_Sync();

}

__PLL_CODE_IN_BOOT__ kal_uint8 pll_get_current_vpe_id(void)
{
    unsigned int vpe_id = 0;

    __asm__ __volatile__(
        "mfc0 %0, $15, 1" \
        : "=r" (vpe_id) \
        :);

    return (vpe_id & 0xF);
}

/*************************************************************************
 * FUNCTION
 *  INT_SetPLL
 *
 * DESCRIPTION
 *  This function dedicates for PLL setting.
 *
 * PARAMETERS
 *  Init mode of PLL
 *
 * RETURNS
 * Note    :    This function would only call by MD. 
 *************************************************************************/
__PLL_CODE_IN_BOOT__ void INT_SetPLL(void)
{
#if !defined(__COSIM_BYPASS__) && !defined(__ESL_MASE__)
    if ((pll_get_current_vpe_id() == 0)) 
    {
        if (*REG_MDTOP_PLLMIXED_PLL_DUMMY != MD_PLL_MAGIC_NUM) 
        {/* PLL didn't init by BootRom */
            PLL_MD_Pll_Init();

            // Make a record that means MD pll has init by MD. 
            *REG_MDTOP_PLLMIXED_PLL_DUMMY = MD_PLL_MAGIC_MD;           
        }
    }
#endif // __COSIM_BYPASS__
}

/*************************************************************************
 * FUNCTION
 *  PLL_MD_Pll_Init
 *
 * DESCRIPTION
 *  This function dedicates for PLL setting.
 *
 * PARAMETERS
 *  Init mode of PLL
 *
 * RETURNS
 * Note    :    This function would call by BootRom and MD!! 
 *************************************************************************/
__PLL_CODE_IN_BOOT__ void PLL_MD_Pll_Init(void)
{
#if !defined(__COSIM_BYPASS__) && !defined(__ESL_MASE__)
    INT_SetPLL_Gen95();
#endif // __COSIM_BYPASS__
}

 /*------------------------------------------------------------------------
 * void    PLL_Set_CLK_To_26M
 * Purpose:	Re-configure all the module clocks from PLL to 26M.
 * Parameters:
 *    Input:	None.
 *              
 *    Output:	None.
 *              
 * returns :	void.
 * Note    :    This function would call by BootRom!!
 *               
 *------------------------------------------------------------------------
 */ 
__PLL_CODE_IN_BOOT__ void PLL_Set_CLK_To_26M(void)
{
    // set mdmcupll/mdvdsppll/mdbrppll posdiv to 0  
    *REG_MDTOP_CLKSW_CLKSEL_CTL = 0x2A000000;  //switch all clock to XTAL frequency
    *REG_MDTOP_CLKSW_SDF_CK_CTL = 0x201; // switch SDF clock to XTAL frequency

    // Make a record that means MD pll has been changed to 26M. 
    *REG_MDTOP_PLLMIXED_PLL_DUMMY = MD_PLL_MAGIC_26M;
}

 /*------------------------------------------------------------------------
 * void    PLL_FrequencyMeter_GetFreq
 * Purpose:	Get specified PLL/module's clock(Mhz).
 * Parameters:
 *    Input:	PLL_FM_SOURCE index: The module you want to measure.
 *              
 *    Output:	None.
 *              
 * returns :	The PLL/module's clock(Mhz).
 * Note    :    This function would spend at least 20us to measure the clock.
 *               
 *------------------------------------------------------------------------
 */ 
kal_uint32 PLL_FrequencyMeter_GetFreq(PLL_FM_SOURCE index)
{
    kal_uint32 count = 2000, output = 0;

    if ((index < PLL_FM_SOURCE_START) || (index > PLL_FM_SOURCE_END))
        return 0;

    *REG_MDTOP_CLKSW_CKMON_CTL = PLL_FM_MDSYS_MDCORE_CLOCK; //select source to a valid clock to let reset success.

    *REG_MDTOP_CLKSW_FREQ_METER_CTL = 0;        //reset frequency meter

    MO_Sync(); 
    ust_us_busyloop(2);//let Frequency Meter reset done   
    
    *REG_MDTOP_CLKSW_CKMON_CTL = 0x300 | index; //divided by 8 and select source
    *REG_MDTOP_CLKSW_FREQ_METER_XTAL_CNT = PLL_FM_WIMDOW;
    *REG_MDTOP_CLKSW_FREQ_METER_CTL = 1;        //enable frequency meter
    MO_Sync();

    // wait measure done or timeout
    while (((*REG_MDTOP_CLKSW_FREQ_METER_CTL) & (1 << 1)) == 0) 
    {
        count--;
        if (count == 0)
            break;
    }

    if (count == 0)
        return 0;

    output = *REG_MDTOP_CLKSW_FREQ_METER_CKMON_CNT * 26 * 8 / (PLL_FM_WIMDOW+3);

    *REG_MDTOP_CLKSW_CKMON_CTL = 0;        //select source to NULL to save power in flip-flop, save about 0.07mA in 6293

    *REG_MDTOP_CLKSW_FREQ_METER_CTL = 0;        //reset frequency meter

    return output;
}

void PLL_exception_dump(void)
{ 
    g_pll_info.MDBPI_PLL_D4 = PLL_FrequencyMeter_GetFreq(PLL_FM_MDBPI_PLL_D4);
    g_pll_info.MDBPI_PLL_D6 = PLL_FrequencyMeter_GetFreq(PLL_FM_MDBPI_PLL_D6);
    g_pll_info.MDSYS_MML2_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDSYS_MML2_CLOCK);
    g_pll_info.FESYS_RXAGC_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_FESYS_RXAGC_CLOCK);
    g_pll_info.MDRXSYS_DFESYNC_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDRXSYS_DFESYNC_CLOCK);
    g_pll_info.FESYS_F208M_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_FESYS_F208M_CLOCK);
    g_pll_info.TRACE_MON_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_TRACE_MON_CLOCK);
    g_pll_info.MDSYS_208M_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDSYS_208M_CLOCK);
    g_pll_info.MDRXSYS_RAKE_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDRXSYS_RAKE_CLOCK);
    g_pll_info.MDRXSYS_BRP_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDRXSYS_BRP_CLOCK);
    g_pll_info.MDRXSYS_VDSP_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDRXSYS_VDSP_CLOCK);
    g_pll_info.MDTOP_LOG_ATB_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDTOP_LOG_ATB_CLOCK);
    g_pll_info.FESYS_CSYS_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_FESYS_CSYS_CLOCK);
    g_pll_info.FESYS_BSI_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_FESYS_BSI_CLOCK);
    g_pll_info.MDSYS_MDCORE_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDSYS_MDCORE_CLOCK);
    g_pll_info.MDSYS_BUS2X_NODCM_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDSYS_BUS2X_NODCM_CLOCK);
    g_pll_info.MDSYS_BUS2X_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDSYS_BUS2X_CLOCK);
    g_pll_info.MDTOP_DBG_CLOCK = PLL_FrequencyMeter_GetFreq(PLL_FM_MDTOP_DBG_CLOCK);
    
    g_pll_info.AD_MDBPI_PLL_D7 = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDBPI_PLL_D7);
    g_pll_info.AD_MDBPI_PLL_D5 = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDBPI_PLL_D5);
    g_pll_info.AD_MDBPI_PLL_D4 = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDBPI_PLL_D4);
    g_pll_info.AD_MDBPI_PLL_D3 = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDBPI_PLL_D3);
    g_pll_info.AD_MDBPI_PLL_D2 = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDBPI_PLL_D2);
    g_pll_info.AD_MDBRP_PLL = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDBRP_PLL);
    g_pll_info.AD_MDVDSP_PLL = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDVDSP_PLL);
    g_pll_info.AD_MDMCU_PLL = PLL_FrequencyMeter_GetFreq(PLL_FM_AD_MDMCU_PLL);    
}

/****************************************************************
    Function for SDF module. (SIB)
****************************************************************/
 /*------------------------------------------------------------------------
 * void    PLL_CLKSW_SDF_SRC_CKSEL_Get
 * Purpose:	Get the selection of SDF source clock.
 * Parameters:
 *    Input:    None.
 *              
 *    Output:   None.
 *
 * returns :    The selection of SDF source clock
 *              
 * Note    :    Porting from LR12's PLL_SDF_SRC_CKSEL_GET().
 *               
 *------------------------------------------------------------------------
 */
kal_uint32 PLL_CLKSW_SDF_SRC_CKSEL_Get()
{
    if((*REG_MDTOP_CLKSW_SDF_CK_CTL&(1<<4))==0)
    {
        return CLKSW_SDF_SRC_26M;
    }
    else
    {
        return (((*REG_MDTOP_CLKSW_SDF_CK_CTL) & 0x00000700) >> 8);
    }
}

 /*------------------------------------------------------------------------
 * void    PLL_CLKSW_SDF_SRC_CKSEL_Set
 * Purpose:	Set the selection of SDF source clock.
 * Parameters:
 *    Input:    PLL_CLKSW_SDF_SRC src_ck: CLKSW_SDF_SRC_xxx in "Pll_genxx.h", src_clk index.
 *              
 *    Output:   None.
 *
 * returns :    KAL_TRUE/KAL_FALSE
 *              
 * Note    :    Porting from LR12's PLL_SDF_SRC_CKSEL_SET().
 *               
 *------------------------------------------------------------------------
 */
kal_bool PLL_CLKSW_SDF_SRC_CKSEL_Set(PLL_CLKSW_SDF_SRC src_clk)
{
    //kal_uint32 caller_LR;
    //GET_RETURN_ADDRESS(caller_LR);
    
    if (src_clk >= CLKSW_SDF_SRC_END)
    {
        //EXT_ASSERT(0, caller_LR, src_clk, 0);
        return KAL_FALSE;
    }

    if(src_clk == CLKSW_SDF_SRC_26M)
    {/* Restore to default setting */
        
        // SDF clock switch to 26Mhz
        *REG_MDTOP_CLKSW_SDF_CK_CTL &= ~(1<<4);

        //restore SDF clock source
        *REG_MDTOP_CLKSW_SDF_CK_CTL = ((*REG_MDTOP_CLKSW_SDF_CK_CTL) & 0xFFFFF8FF) | (CLKSW_SDF_SRC_BPIPLL<<8);        
    }
    else
    {
        // SDF clock switch to 26Mhz
        *REG_MDTOP_CLKSW_SDF_CK_CTL &= ~(1<<4);
    
        //set SDF clock source
        *REG_MDTOP_CLKSW_SDF_CK_CTL = ((*REG_MDTOP_CLKSW_SDF_CK_CTL) & 0xFFFFF8FF) | (src_clk<<8);
    
        // SDF clock switch to full speed
        *REG_MDTOP_CLKSW_SDF_CK_CTL |= (1<<4);
    }

    MO_Sync();

    return KAL_TRUE;
}

#endif /* should NOT be compiled on MODIS */