#include <msp430f249.h>

typedef unsigned char BYTE;
typedef unsigned int  WORD;
typedef unsigned long LONG;

LONG  MSP430_GetDCOFreq();
  volatile LONG result;

  	void MSP430_SetClock();
	void  MSP430_Clock_from_XT2();

	// ----- Impostazione del DCO con resistenza interna - circa 4.7MHz -----
#define DCO_DCOx    3
#define DCO_RSELx   12
	
void main(void)
{

  WORD i;

  WDTCTL = WDTPW + WDTHOLD; // Stop WDT

  BCSCTL1 = CALBC1_1MHZ;                    // Set DCO
  DCOCTL = CALDCO_1MHZ;

	MSP430_SetClock();

  // Wait for xtal to stabilize
  do
  {
  IFG1 &= ~OFIFG;                           // Clear OSCFault flag
  for (i = 0x47FF; i > 0; i--);             // Time for flag to set
  }
  while ((IFG1 & OFIFG));                   // OSCFault flag still set?

  // output ACLK, MCLK, SMCLK
  P5SEL |= BIT4 + BIT5 + BIT6;
  P5DIR |= BIT4 + BIT5 + BIT6;

  while(1)
  {
    result = MSP430_GetDCOFreq();

    __no_operation();
  }
}

//-----------------------------------------------------------------------------------------------
// GetDCOFreq
// Utilizza il TimerB e ACLK(32KHz) per misurare la frequenza del DCO (torna valore in Hz)
//-----------------------------------------------------------------------------------------------
LONG  MSP430_GetDCOFreq()
{
BYTE  SmclkFromXt2;
WORD  n1,n2;

    // Setta un flag se SMCLK è ricavato da XT2
    if (BCSCTL2 & SELS) SmclkFromXt2=1; else SmclkFromXt2=0;

    // Ricava il clock per SMCLK da DCO
    BCSCTL2 &= ~SELS;
    //BCSCTL2 |= SELS;

    // Accende TimerB: conteggio continuo a 16 bit, clock = SMCLK
    TBCTL = TBSSEL1 | MC1 | TBCLR;
    // Capture da CC16B=ACLK, rising edge
    TBCCTL6 = CM1 | CCIS0 | SCS | CAP;
    // Attende il primo evento di capture
    while ( (TBCCTL6 & CCIFG)==0 ) {};
    // Memorizza valore attuale
    n1 = TBCCR6;
    // Azzera flag di capture e overflow
    TBCCTL6 &= ~(COV|CCIFG);
    // Attende il prossimo evento di capture
    while ( (TBCCTL6 & CCIFG)==0 ) {};
    // Memorizza valore attuale
    n2 = TBCCR6;
    // Se si e' verificato un overflow azzera il valore, se no calcola la differenza
    if ( (TBCCTL6 & COV) != 0 ) n2=0; else n2=n2-n1;
    // Ferma il TimerB
    TBCTL = 0; TBCCTL6 = 0;
    // Ripristina il clock a SMCLK da XT2 se necessario
    if (SmclkFromXt2 != 0) BCSCTL2 |= SELS;
    // Ritorna il valore calcolato in KHertz
    return( (n2*32768L)/1000L );
}

//---------------------------------------------------------------------------
// SetClock
// Inizializzazione sotto-sistema clock
//---------------------------------------------------------------------------
void MSP430_SetClock()
{
unsigned long int d=200000;
int i;

	// ========== Accende XT2 e lascia il resto come da default dopo POR ==========
	// XT2OFF=0[ON] XTS=0[LF] DIVAx=00[/1] XT5V=0 RSELx=100
	BCSCTL1 = RSEL2;
	// SELM=00[DCO] DIVM=00[/1] SELS=0[DCO] DIVS=00[/1] DCOR=0
	BCSCTL2 = 0;
	// ========== Attende accesione XT2 ==========
	do { IFG1 &= ~OFIFG; for(i=0;i<250;i++) d--; } while ( (IFG1&OFIFG)!=0 && d);
	// ========== Impostazione operativa del sottosistema clock ==========
	MSP430_Clock_from_XT2();
}

//-----------------------------------------------------------------------------------------------
// Clock from XT2
//
// Usa XT2 per MCLK/SMCLK - XT2 deve gia essere in funzione !!!
//-----------------------------------------------------------------------------------------------
void  MSP430_Clock_from_XT2()
{
	// SELM=10[XT2] DIVM=00[/1] SELS=1[XT2] DIVS=00[/1] DCOR=1
	//BCSCTL2 = SELM1 | SELS | DCOR;
	// Paolo 5.02.0A modifica per utilizzare la resistenza interna di pullup ca. 300K
	BCSCTL2 = SELM1 | SELS;
	
	// XT2OFF=0[ON] XTS=0[LF] DIVAx=00[/1] XT5V=0 RSELx=[SETUP]
	BCSCTL1 = DCO_RSELx;
	// DCOx come da impostazione , niente modulatore
	DCOCTL = (DCO_DCOx<<5);
	// Accende SMCLK
	__bic_SR_register(SCG1);
}