MSP430F1611: Timer_B Counter Issue.

#include "sctimer.h"
#include "msp430f1611.h"
#include "sctimer.h"
#include "board.h"
#include "leds.h"
#include "radio.h"
#include "uart.h"

// ========================== define ==========================================

#define TIMERLOOP_THRESHOLD    0x4000     // 0.5 seconds @ 32768Hz clock
#define MINIMUM_COMPAREVALE_ADVANCE  3

// ========================== variable ========================================

typedef struct {
    sctimer_cbt         cb;
    sctimer_capture_cbt startFrameCb;
    sctimer_capture_cbt endFrameCb;
    sctimer_asn_cbt     asnreadCb;
    uint8_t             f_SFDreceived;
} sctimer_vars_t;

sctimer_vars_t sctimer_vars;

// ========================== private ==========================================

// ========================== protocol =========================================

/**
\brief Initialization sctimer.
*/
void sctimer_init(void){
    memset(&sctimer_vars, 0, sizeof(sctimer_vars_t));
   
   // radio's SFD pin connected to P4.1
   P4DIR   &= ~0x02; // input
   P4SEL   |=  0x02; // in CCI1a/B mode
      
   // CCR1 in capture mode
   TBCCTL1  =  CM_3+SCS+CAP+CCIE;
   TBCCR1   =  0;
   
   // CCR2 in compare mode (disabled for now)
   TBCCTL2  =  0;
   TBCCR2   =  0;

   // CCR3 in Count mode (disabled for now)
   TBCCTL3  =  0;
   TBCCR3   =  0;

   // start counting
   TBCTL    =  MC_2+TBSSEL_1;                    // continue mode, clocked from ACLK
}

void sctimer_set_callback(sctimer_cbt cb){
    sctimer_vars.cb = cb;
}

void sctimer_setStartFrameCb(sctimer_capture_cbt cb){
    sctimer_vars.startFrameCb = cb;
}

void sctimer_setEndFrameCb(sctimer_capture_cbt cb){
    sctimer_vars.endFrameCb = cb;
}

void sctimer_setasnreadCb(sctimer_asn_cbt cb){
    sctimer_vars.asnreadCb = cb;

}

void sctimer_asncounter(void){
    TBCCTL3 = CCIE;
    TBCCR3 = 328;
    TBCCTL3 |= CCIFG;
}
/**
\brief set compare interrupt
*/
void sctimer_setCompare(PORT_TIMER_WIDTH val){
    TBCCTL2             =  CCIE;
    if (TBR - val < TIMERLOOP_THRESHOLD){
        // the timer is already late, schedule the ISR right now manually 
        // setting the interrupt flag triggers an interrupt
        TBCCTL2          |=  CCIFG;
    } else {
        if (val-TBR<MINIMUM_COMPAREVALE_ADVANCE){
            // there is hardware limitation to schedule the timer within TIMERTHRESHOLD ticks
            // setting the interrupt flag triggers an interrupt
            TBCCTL2          |=  CCIFG;
        } else {
            // schedule the timer at val
            TBCCR2            =  val;
        }
    }
}

/*
\brief Return the current value of the timer's counter.

 \returns The current value of the timer's counter.
*/
PORT_TIMER_WIDTH sctimer_readCounter(void){
    return TBR;
}

void sctimer_enable(void){
    TBCCTL2 |=  CCIE;
}

void sctimer_disable(void){
    TBCCTL2 &= ~CCIE;
}

// ========================== private =========================================


//=========================== interrupt handlers ==============================

/**
\brief TimerB CCR1-6 interrupt service routine
*/
kick_scheduler_t sctimer_isr() {
   PORT_TIMER_WIDTH tbiv_local;
   
   // reading TBIV returns the value of the highest pending interrupt flag
   // and automatically resets that flag. We therefore copy its value to the
   // tbiv_local local variable exactly once. If there is more than one 
   // interrupt pending, we will reenter this function after having just left
   // it.
   
   tbiv_local = TBIV;
   
   switch (tbiv_local) {
      case 0x0002: // CCR1 fires
         if (TBCCTL1 & CCI) {
            // SFD pin is high: this was the start of a frame
            if (sctimer_vars.startFrameCb!=NULL) {
               sctimer_vars.startFrameCb(TBCCR1);
               sctimer_vars.f_SFDreceived = 1;
               // kick the OS
               return KICK_SCHEDULER;
            }
         } else {
            // SFD pin is low: this was the end of a frame
            if (sctimer_vars.endFrameCb!=NULL) {
               if (sctimer_vars.f_SFDreceived == 1) {
                  sctimer_vars.endFrameCb(TBCCR1);
                  sctimer_vars.f_SFDreceived = 0;
               }
               TBCCTL1 &= ~COV;
               TBCCTL1 &= ~CCIFG;
               // kick the OS
               return KICK_SCHEDULER;
            }
         }
         break;
      case 0x0004: // CCR2 fires
         if (sctimer_vars.cb!=NULL) {
            sctimer_vars.cb();
            // kick the OS
            return KICK_SCHEDULER;
         }
         break;
      case 0x0006: // CCR3 fires
          if (TBCCTL3 & CCI){
            if(sctimer_vars.asnreadCb !=NULL){
              sctimer_vars.asnreadCb();
               // kick the OS
               return KICK_SCHEDULER;
            }
          }
         break;
      case 0x0008: // CCR4 fires
         break;
      case 0x000a: // CCR5 fires
         break;
      case 0x000c: // CCR6 fires
         break;
      case 0x000e: // timer overflow
         break;
   }
   return DO_NOT_KICK_SCHEDULER;
}

#include "stdint.h"
#include "string.h"
#include "board.h"
#include "debugpins.h"
#include "leds.h"
#include "sctimer.h"
#include "uart.h"


//=========================== defines =========================================

#define SCTIMER_PERIOD     32768 // @32kHz = 1s

//=========================== variables =======================================

typedef struct {
   uint16_t num_compare;
   uint16_t num_last_value;
   uint64_t num_asn;
} app_vars_t;

app_vars_t app_vars;

//=========================== prototypes ======================================

void cb_compare(void);
void cb_asnread(void);

//=========================== main ============================================

/**
\brief The program starts executing here.
*/
int mote_main(void) {  
   
   // initialize board. 
   board_init();
   
   sctimer_set_callback(cb_compare);
   sctimer_set_setasnreadCb(cb_asnread);
   sctimer_setCompare(sctimer_readCounter()+SCTIMER_PERIOD);


   while (1) {
      board_sleep();
   }
}

//=========================== callbacks =======================================

void cb_asnread(void){
  app_vars.num_asn++;
  uart_writeByte(app_vars.num_asn);

  sctimer_asncounter();
  sctimer_setCompare(sctimer_readCounter()+SCTIMER_PERIOD);

}

void cb_compare(void) {
   
   // increment counter
   app_vars.num_compare++;

   sctimer_setCompare(sctimer_readCounter()+SCTIMER_PERIOD);
}