#include "setuppll.h"

void SetupPLL(UWORD wdCoreMult,UWORD wdDiv)
{ 
	UWORD wdPrevPLL  = *pPLL_CTL;
	UWORD wdPrevAIWR = *pSICA_IWR0;
  	UWORD wdPrevBIWR = *pSICB_IWR0;
  	UWORD wdNewPLL   = (wdCoreMult & 0x3f) <<9;
    if(wdNewPLL != wdPrevPLL)
    {		  	
	 	if(((UDWORD)(*pSRAM_BASE_ADDRESS)) == 0xFF800000 )
	 	{						  // do things for Core A
		 	*pSICB_SYSCR |= 0x0080;				// Raise Core B Supplemental-Int0
			//ssync();
			while((*pSICB_SYSCR & 0x080));		// Wait: Core B Acknowledge SUP-B0		 		
		 	*pSICA_IWR0 = (wdPrevAIWR | 0x1); // enable PLL Wakeup Interrupt
  			*pPLL_CTL = wdNewPLL;
			//ssync();
	  		//idle();							  // put in idle

  			*pSICA_IWR0 = wdPrevAIWR;		  // continue here after idle, restore previous IWR content
			//ssync();
	  	}
		else
		{							  		  // do things for Core B
			while(!(*pSICB_SYSCR & 0x0080));		// Wait: For Core A to raise SUP-B0	
			*pSICB_SYSCR = 0x0800;	          // Acknowledge Supplemental Interrupt
			//ssync();
	
			*pSICB_IWR0 = (wdPrevBIWR | 0x1); // enable PLL Wakeup Interrupt
			//ssync();
  		
	  		//idle();							  // put in idle

  			*pSICB_IWR0 = wdPrevBIWR;		  // continue here after idle, restore previous IWR content
			//ssync();	
    	}		
	}

  	*pPLL_DIV = (*pPLL_DIV & 0xFFF0) | wdDiv;
  	//ssync();
}