#include "hardware.h"
.text
.global fllInit                         ; SW FLL to init DCO/SMCLK -frequency
        .type   fllInit, @function
fllInit:
        mov.b   #BCSCTL1_FLL, &BCSCTL1  ; Init basic clock control reg 1
        mov.b   #BCSCTL2_FLL, &BCSCTL2  ; Init basic clock control reg 2
        mov     #TACTL_FLL, &TACTL      ; SMCLK is TA-clock / Timer stopped
        bis     #MC1, &TACTL            ; Start timer: Continuos Mode
        mov     #CCTL2_FLL, &CCTL2      ; Init CCR2 and Clear capture flag

.Lwait0:bit     #CCIFG, &CCTL2          ; Test/Wait for capture flag
        jz      .Lwait0                 ; May be used with INT / LPM0 later ?
        mov     &CCR2, r15              ; Store CCR2 init-value
        bic     #CCIFG, &CCTL2          ; Clear capture flag
.Lwait1:bit     #CCIFG, &CCTL2          ; Test/Wait for capture flag
        jz      .Lwait1                 ; May be used with INT / LPM0 later ?
        bic     #CCIFG, &CCTL2          ; Clear capture flag
        mov.b   &BCSCTL1, r14           ; Store current Rsel value
        bic.b   #0x0f8, r14             ; Mask for Rsel bits
        mov.b   &DCOCTL, r13            ; Store current DCO value

.LfllUP:cmp.b   #DCOCTL_MAX, r13        ; Needs Rsel to be increased ?
        jne     .LfllDN                 ; No
        cmp.b   #7, r14                 ; Is max Rsel already selected ?
        jge     .LfllER                 ; Yes, Rsel can not be increased
        inc.b   &BCSCTL1                ; Increase Rsel
        jmp     .LfllRx                 ; Test DCO again

.LfllDN:cmp.b   #DCOCTL_MIN, r13        ; Needs Rsel to be decreased ?
        jne     .LfllCP                 ; No
        cmp.b   #0, r14                 ; Is min Rsel already selected ?
        jeq     .LfllER                 ; Yes, Rsel can not be increased
        dec.b   &BCSCTL1                ; Decrease Rsel
.LfllRx:mov.b   #60h, &DCOCTL           ; Center DCO (may be optimized later ?)
        jmp     .Lwait0                 ; Test DCO again
.LfllCP:
        mov     &CCR2, r12              ; Read captured value
        sub     r15, r12                ; Subtract last captured value
        mov     &CCR2, r15              ; Store CCR2 value for next pass
        cmp     #DCO_FSET, r12          ; DCO_FSET= SMCLK/(32768/4)
        jl      .LfllI                  ;
        jeq     .LfllOK                 ;
.LfllD: dec.b   &DCOCTL                 ; Decrement value
        jmp     .Lwait0                 ;
.LfllI: inc.b   &DCOCTL                 ; Increment value
        jmp     .Lwait0                 ;

.LfllER:                                ; error, currently ingnored
.LfllOK:clr     &CCTL2                  ; stop CCR2
        ret                             ;