#include  <msp430x16x.h>
#include <io.h>
#include <signal.h>
#include <iomacros.h>

            .section    .text

.global Set_DCO

;	BCSCTL1|= DIVA_3;				// ACLK is ACLK/8
;    TACCTL2 = CM_1 | CCIS_1 | CAP;	// Capture on rising edge
;    								// CCI2B is ACLK;
;									// Set Capture mode
;	TACTL= TASSEL_2 | MC_2 | TACLR;	// Tim_3 source SMCLK
;									// Continuous mode
;									// clear counter
;
; Call as:
;	 Set_DCO( int delta )
;
;----------------------------------------------------------------------------- 
Set_DCO:    ;Subroutine: Sets DCO to selected frequency based on Delta.
;           R14 and R15 are used, ACLK= 32768/8 Timer_A clocked by DCOCLK
;			Delta = SMCLK/(32768/8)
;----------------------------------------------------------------------------- 
            push.w  &TACTL					; save the counter state
            push.w  &TACCTL2				; save counter control 2 state
			mov.w	r15,r13					; put call param in r13
            clr.w   R15                     ;
            bis.b	#DIVA_3, &BCSCTL1		; ACLK / 8
            mov.w	#TASSEL_2|MC_2|TACLR, &TACTL	; SMCLK, Continuous, Clear						
			mov.w   #CCIS_1|CM_1|CAP,&CCTL2	; Define CCR2,CAP,ACLK									
Test_DCO:   bit.w   #CCIFG,&CCTL2           ; Test capture flag
            jz      Test_DCO                ;
            bic.w   #CCIFG,&CCTL2           ; Clear capture flag ~CCIFG
                                            ;
AdjDCO:     mov.w   &CCR2,R14               ; R14 = captured SMCLK
            sub.w   R15,R14                 ; R14 = capture difference
            mov.w   &CCR2,R15               ; R15 = captured SMCLK
            cmp.w   R13,R14              	; Delta = SMCLK/(32768/8)
            jlo     IncDCO                  ;
            jeq     DoneDCO                 ;
DecDCO:     dec.b   &DCOCTL                 ; Slow DCO with DCO and MOD
            jnz     Test_DCO                ; Slower?
            bit.b   #7,&BCSCTL1             ; Can RSEL.x be decremented?
            jz      DoneDCO                 ; jmp>DCO at slowest setting
            dec.b   &BCSCTL1                ; Decrement RSEL.x 
            jmp     Test_DCO                ;
IncDCO:     inc.b   &DCOCTL                 ; Speed DCO with DCO and MOD
            jnc     Test_DCO                ; Faster?
            cmp.b   #0x87,&BCSCTL1             ; Can RSEL.x be increased?
            jz      DoneDCO                 ; jmp> DCO at fastest settting
            inc.b   &BCSCTL1                ; Increment RSEL.x
            jmp     Test_DCO                ;
DoneDCO:    clr.w   &CCTL2                  ; Stop CCR2
            pop.w   &TACCTL2				; restore counter control 2 state
            pop.w   &TACTL					; restore the counter state
  			and.b	#~DIVA_3, &BCSCTL1		; ACLK / 1 back to our state
            ret                             ; Return from subroutine