/*
Copyright 2003/2004, Freie Universitaet Berlin. All rights reserved.

These sources were developed at the Freie Universität Berlin, Computer
Systems and Telematics group.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

- Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.

- Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
 
- Neither the name of Freie Universitaet Berlin (FUB) nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.

This software is provided by FUB and the contributors on an "as is"
basis, without any representations or warranties of any kind, express
or implied including, but not limited to, representations or
warranties of non-infringement, merchantability or fitness for a
particular purpose. In no event shall FUB or contributors be liable
for any direct, indirect, incidental, special, exemplary, or
consequential damages (including, but not limited to, procurement of
substitute goods or services; loss of use, data, or profits; or
business interruption) however caused and on any theory of liability,
whether in contract, strict liability, or tort (including negligence
or otherwise) arising in any way out of the use of this software, even
if advised of the possibility of such damage.

This implementation was developed by the CST group at the FUB.
Contributors: Bjoern Lichtblau

For documentation and questions please use the web site
http://scatterweb.mi.fu-berlin.de and the mailinglist
scatterweb@lists.spline.inf.fu-berlin.de (subscription via the Website).
Berlin, 2003/2004/2005
*/

#include <stdarg.h> // important for variable argument lists: function(x, y, ...)
#include "ScatterWeb.System.h"

#define WDSTATE   (*((UINT16*)0x09fe))      ///< 2 bytes are reserved at main

#if defined(ECR430)
#define CS_LOW    (P2OUT &= ~0x08)
#define CS_HIGH   (P2OUT |= 0x08)
#define SDA_HIGH  (P5OUT |= 0x01)   ///< RTC data line high
#define SDA_LOW   (P5OUT &= 0xFE)   ///< RTC data line low
#define SDA       (P5IN & 0x01)     ///< Test SDA.
#define SCL_HIGH  (P5OUT |= 0x02)   ///< RTC clock line high 
#define SCL_LOW   (P5OUT &= 0xFD)   ///< RTC clock line low
#define NOPS    do{ _NOP(); _NOP(); _NOP(); _NOP(); _NOP(); _NOP(); _NOP(); _NOP(); _NOP(); _NOP();}while(0)
#endif

#if defined(ESB430) || defined(EYE430) || defined(ESS430)
#define SDA_HIGH  (P5OUT |= 0x01) ///< RTC data line high
#define SDA_LOW   (P5OUT &= 0xFE) ///< RTC data line low
#define SCL_HIGH  (P5OUT |= 0x02) ///< RTC clock line high 
#define SCL_LOW   (P5OUT &= 0xFD) ///< RTC clock line low
#define BUS_READ          0x9F
#define BUS_WRITE         0x9E
#define ACC_CSR           0xAC      ///< Access Configuration/Control Register
#define ACC_CLOCK         0xC0      ///< Access Clock Register
#define ACC_CLOCK_ALARM   0xC7      ///< Access Clock Alarm Register
#define ACC_TH            0xA1      ///< Access Thermostat Setpoint High
#define ACC_TL            0xA2      ///< Access Thermostat Setpoint Low
#define ACC_CSRAM         0x17      ///< Access Clock 32 byte SRAM
#define ACC_RT            0xAA      ///< Access Read Temperatur Register
#define CSR_OS1          (0x80)      
#define CSR_OS0          (0x40)      
#define CSR_A1           (0x20)      
#define CSR_A0           (0x10)    
#define CSR_CNV          (0x04)      
#define CSR_POL          (0x02)     
#define CSR_1SH          (0x01)      
#define CSR_DEFAULT     (CSR_OS1 + CSR_OS0 + CSR_A1 + CSR_CNV + CSR_1SH + CSR_POL)
#endif

#define DCOLOW      (1)                   ///< Symbolic name, DCO was last measure too low.
#define DCOHIGH     (2)                   ///< Symbolic name, DCO was last measure too high.
#define DCODIFF     (3)                   ///< Difference of dcoDiff to DELTA when DCO measure is taken as too high/low. 
#define RESET 			(WDTCTL = 0x0000)
#if defined(DCO_2458KHZ)
#define DELTA  600                      ///< Target DCO = DELTA*(4096) = 2457600 */
#elif defined(DCO_4506KHZ)
#define DELTA  1100                      ///< Target DCO = DELTA*(4096Hz) , DELTA=550 => 4505600 Hz  */
#else
#error No DCO frequency in Set_DCO.
#endif
#define UP		 1                        ///< Temporary.
#define DOWN     2                      ///< Temporary.
  
/** The callbacks set via ::registerCallback are stored here. */
fp_vp callbacks[NUMBER_CALLBACKS];
// present possible flags
volatile UINT16 runModule;

UINT16 wd_startup;

UINT8* panic_dest;

const UINT8 reason_strings[7][10] = { "POR", "SWRESET", "REPROGRAM", "PANIC", 
                                             "WATCHDOG", "KEYV", "UNKNOWN" };

extern void Process_init();  ///< The only function in userapp that can be called from firmware (except callbacks).

#if defined(ESB430) || defined(EYE430)
volatile UINT8 MIC; ///< 0x01 is like IFG, 0x02 like IE and will set sensorFlags.MIC
#endif

UINT16 _skipADC;
volatile UINT16 rxpTemp;          ///< ::ADC12ISR() stores the current receive power here.
volatile UINT16 rxpValue;         ///< If a packet start is detected by ::ADC12ISR() the receive power of the start is written here.
volatile UINT16 dcoCap;        ///< Contains always last TBCCR6 Value (Capture on ACLK)
volatile UINT16 dcoDiff;       ///< Evreytime dcoDiff==0 TIMER_B1 interrupt writes a new DCO diff measure into it.
volatile UINT8  checkDCOState; ///< DCO will only be changed if two times low or high in a row to avoid peaks. Implemented with this variable.

COMMAND(rtt, 0) { 
  #if defined(ESB430) || defined(EYE430) || defined(ECR430)
  #if defined(ESB430) || defined(EYE430)
  System_clStartConversion(); 
  #endif
  String_write(term_reply, "%T", System_getTemperature());   
  #else
  String_write(term_reply, "NOT SUPPORTED: %s (wrong platform)", str);
  #endif
}                 

COMMAND(swb, 0){ Data_beeperToggle(); }                             ///< Toggles beeper.
COMMAND(sbp, 0){ if(str[4]=='1') Data_beeperOn(); else Data_beeperOff(); } ///< Sets beeper.

static void __inline__ pause(register UINT16 n) {
    __asm__ __volatile__ (
		"1: \n"
		" dec	%[n] \n"
		" jne	1b \n"
        : [n] "+r"(n));
}

NAKED(_reset_vector__){
  extern UINT16 __noinit_end; // End of data segment in RAM.
  UINT16* p = & __noinit_end; // Held in a register because only 
                                    // var in this procedure!
  while(p < (UINT16*)0x09FE){ // Fill rest of RAM with 0xABCD.
    *p = 0xABCD;
    p++;    
  }
  __asm__ __volatile__("br #main"::); // Branch to main.
}

// software wd
interrupt(TIMERA1_VECTOR) timera1_isr (void){
  UINT16 oldpc = * ( ((UINT16*)GET_FRAME_ADDR(timera1_isr)) + 1);
  UINT16 sp = READ_SP;
  System_panic("SW-WD(%x,%x)", oldpc, sp);
  TACCTL1 = 0x0000; // Ensure timer is off. clrs IE&IFG
  System_reset();
}

#if defined(ECR430)
temp_t getTemperature(){
  UINT16 i;
  UINT16 devid = 0;
  temp_t temperature;
  temperature.u = 0;
  SCL_LOW; // Ensure clock is low
  NOPS;
  CS_LOW; // Start communication 
  NOPS;
  // Read temperature
  for(i=0; i<16; i++){
    SCL_HIGH;
    NOPS;
    if(SDA) temperature.u++; // Set 1 in LSB
    if(i<15) temperature.u <<= 1; // Shift one digit left
    SCL_LOW;
    NOPS;
  }
  // Write comamnd 0x1111 = Shutdown
  P5DIR |= 0x01; // SDA is output
  for(i=0; i<16; i++){
    SDA_HIGH; // write all 1s
    SCL_HIGH;
    NOPS;
    SCL_LOW;
    NOPS;
  }
  P5DIR &= ~0x01; // SDA input again
  // Now read device id
  for(i=0; i<16; i++){
    SCL_HIGH;
    NOPS;
    if(SDA) devid++; // Set 1 in LSB
    if(i<15) devid <<= 1; // Shift one digit left
    SCL_LOW;
    NOPS;
  }  
  // Write comamnd 0x0000 = continous conversion
  P5DIR |= 0x01; // SDA is output
  for(i=0; i<16; i++){
    SDA_LOW; // write all 1s
    SCL_HIGH;
    NOPS;
    SCL_LOW;
    NOPS;
  }
  P5DIR &= ~0x01; // SDA input again
  //End of communication
  CS_HIGH;
  temperature.u &= 0xFFFC; // Clear last two bits as they are always 1.
  return temperature;    
}                       
#endif

#if defined(ESB430) || defined(EYE430) || defined(ESS430)
void cl_start() {
  P5DIR |= 0x03; // ensure: P50(SDA), P51(SCL) output
  SCL_LOW;  _NOP(); _NOP();
  SDA_HIGH; _NOP(); _NOP();
  SCL_HIGH; _NOP(); _NOP();
  SDA_LOW;  _NOP(); _NOP();
  SCL_LOW;  _NOP(); _NOP();
}

void cl_stop() {
  SCL_LOW;  _NOP(); _NOP();
  SDA_LOW;  _NOP(); _NOP();
  SCL_HIGH; _NOP(); _NOP();
  SDA_HIGH; _NOP(); _NOP();
  SCL_LOW;  _NOP(); _NOP();
  P5DIR &= ~0x03;
}

UINT16 cl_writeOnBus(UINT8 byte) {
  UINT16 i, ack;
  for(i=0;i<8;i++) {
    if(byte & 0x80) SDA_HIGH; else SDA_LOW;
    SCL_HIGH;
    byte = byte << 1;
    SCL_LOW;
  }
  // check ack
  P5DIR &= 0xFE;                              // P50(SDA) input
  SCL_HIGH;
  if(P5IN & 0x01) ack = 0; else ack = 1;      // test if ack=0, else error
  SCL_LOW;
  P5DIR |= 0x01;                              // P50(SDA) output
  return ack;
}

UINT8 cl_readFromBus(UINT16 ack) {
  UINT16 i;
  UINT8 byte = 0;
  P5DIR &= 0xFE;                              // P50(SDA) input
  for(i=0;i<8;i++) {
    byte = byte << 1;                
    SCL_HIGH;
    if(P5IN & 0x01) byte |= 0x01; else byte &= 0xFE;
    SCL_LOW;
  }
  P5DIR |= 0x01;                              // P50(SDA) output 
  if(ack) SDA_LOW; else SDA_HIGH;
  SCL_HIGH;
  SCL_LOW;
  return byte;
}

UINT16 getReg16bit(UINT8 acc,UINT16 bitmask) {
  UINT16 config = 0;
  do cl_start();
  while(!cl_writeOnBus(BUS_WRITE)); 
  cl_writeOnBus(acc);
  cl_start(); 
  cl_writeOnBus(BUS_READ);
  config = cl_readFromBus(1);
  config = config << 8;
  config += cl_readFromBus(0);
  cl_stop();
  config &= bitmask;
  _NOP();
  _NOP();
  return config;
}

void setCSReg(UINT8 setting){
  do cl_start();
  while(!cl_writeOnBus(BUS_WRITE));               
  cl_writeOnBus(ACC_CSR);                         
  cl_writeOnBus(setting);
  cl_stop();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
}
#endif

void _initADU(void){
  #if !defined(ESS430)
  ADC12CTL0 = SHT0_6+SHT1_6+MSC;  // Setup ADC12, ref., sampling time
  #if defined(ESB430) || defined(EYE430)
  ADC12CTL1 = SHP+CONSEQ_3+CSTARTADD_2;   // Use sampling timer, repeat-sequence-of-channels
  // ausserdem p61 p62 pir analog und digital?!
  ADC12MCTL2 = (INCH_0 + SREF_0); // mikrofon (P60)             
  ADC12MCTL3 = (INCH_3 + SREF_0); // externe Spannung (P63)            
  ADC12MCTL4 = (INCH_4 + SREF_0); // Batteriespannung (P64);           
  ADC12MCTL5 = (INCH_5 + SREF_0) + EOS;  // Funk Empfangsleistung (P65)     
  { extern UINT16 _micDiff; _micDiff = 0xFFFF; }
  MIC = 0x00;
  #elif defined(ECR430)
  ADC12CTL1 = SHP+CONSEQ_3+CSTARTADD_4;   // Use sampling timer, repeat-sequence-of-channels
  ADC12MCTL4 = (INCH_4 + SREF_0);        // Batteriespannung (P64);           
  ADC12MCTL5 = (INCH_5 + SREF_0) + EOS;  // Funk Empfangsleistung (P65)    
  #elif defined(ESF430)
  ADC12CTL1 = SHP+CONSEQ_3+CSTARTADD_5;   // Use sampling timer, repeat-sequence-of-channels     
  ADC12MCTL5 = (INCH_5 + SREF_0) + EOS;   // Funk Empfangsleistung (P65)     
  #elif defined(ESS430)
  #endif
  _skipADC = 0; 
  #endif
}

void disableADU(){
  #if !defined(ESS430)
  ADC12CTL0 &= ~ENC;                 // Stop conversion   
  System_wait(600);                        // Wait, TODO exchange with test for ADC12BUSY &test
  ADC12CTL0 &= ~(ADC12ON+REFON);     // turn off ADC12
  #endif
}

void checkDCO(){
  if(dcoDiff<(DELTA-DCODIFF)){ // frequency too low
    if(checkDCOState != DCOLOW){ checkDCOState = DCOLOW; }
    else {
      if(Comm_logLevel & DEBUG_DCO) Comm_print("DCO++");
      DCOCTL++;
      if (DCOCTL == 0x00){
         if((BCSCTL1 & 0x07) == 0x07){
           Comm_print("Frequency too high!!!\r\n");
           WDTCTL = 0xDEAD;
         }      
         BCSCTL1++;     // Did DCO role over? // -> Select next higher RSEL
      }
      if(Comm_logLevel & DEBUG_DCO) Comm_print(" %b %b >%i\r\n", DCOCTL, BCSCTL1, dcoDiff);        
    }
  }
  else if(dcoDiff>(DELTA+DCODIFF)){ // frequency too high
    if(checkDCOState != DCOHIGH){ checkDCOState = DCOHIGH; }
    else {
      if(Comm_logLevel & DEBUG_DCO) Comm_print("DCO--");
      DCOCTL--;
      if (DCOCTL == 0xFF){
         if(!((BCSCTL1 & 0x07) == 0x00)){
           Comm_print("Frequency too low!!!\r\n");
           WDTCTL = 0xDEAD;
         }      
         BCSCTL1--;     // Did DCO role under? -> Select next lower RSEL
      }    
      if(Comm_logLevel & DEBUG_DCO) Comm_print(" %b %b >%i\r\n", DCOCTL, BCSCTL1, dcoDiff);        
    }
  }
  else { checkDCOState = 0; } // freqency was ok 
  Timers_add(100, checkDCO, 0xFFFF);
}

void enableADU() {
  #if !defined(ESS430)
  _initADU();
  ADC12CTL0 |= ADC12ON;
  ADC12IE = 0x0020;                     // Enable ADC12IFG.5, Interrupt when sequence is
                                        // completed
  System_wait(600);                     // Delay for reference start-up
  ADC12CTL0 |= ENC;                     // enable conversion
  ADC12CTL0 |= ADC12SC;                 // Start conversion
  _skipADC = 100;
  #endif
}

void initClock() {
  #if defined(ESB430) || defined(EYE430) || defined(ESS430)
  UINT8 csr = getReg16bit(ACC_CSR,0xFF00) >> 8;
  if(csr!=CSR_DEFAULT) setCSReg(CSR_DEFAULT); // if desired Configuration isnt in clock => set it
  /// IMPORTANT: Ensure quartz is generating 32768 Hz 
  /// (sometimes CH bit gets set when clock is read while reset)
  do cl_start();                    /// Do start until BUS_WRITE is acked.
  while(!cl_writeOnBus(BUS_WRITE)); /// Send control byte
  cl_writeOnBus(ACC_CLOCK);         /// Send command byte ::ACC_CLOCK.
  cl_writeOnBus(0x00);              /// Send starting address 0x00.
  cl_writeOnBus(0x00); /// Set CH to 0, tseconds and seconds will also be reset!
  cl_stop();           /// Stop condition.   
  #endif
}

#if defined(ESF430)
interrupt(PORT2_VECTOR) p2_irq() {}
interrupt(PORT1_VECTOR) p1_irq() {}
#endif

void _unexpected_() {}                                   ///< unsed ISR
interrupt(UART1TX_VECTOR)     uart1_tx() {}            ///< unsed ISR
interrupt(COMPARATORA_VECTOR) compa_isr() {}           ///< unsed ISR
interrupt(NMI_VECTOR) nmi_isr() {}

#if !defined(ESS430)
interrupt(ADC_VECTOR) ADC12ISR (void) {
  rxpTemp = ADC12MEM5; // Important as this will reset the IFG.
  if(_skipADC) _skipADC--;
  else {
    // Test if CarrierDetect and not already inside a packet.
    if(rxpTemp > Configuration.rxReceiveLimit && cdCounter==0){
      // Packet start should be detected within next 13 bytes.
      // (TURNON_LENGTH+(PREAMBLE_LENGTH+1)+3[FF, 01, 7F]+2 bytes, else it was noise.
      cdCounter = 13;  
      rxpValue = rxpTemp;
    } 
    #if defined(ESB430) || defined(EYE430)
    // Microphone
    extern UINT16 _micDiff;
    if( (abs(2048 - ADC12MEM2) > _micDiff) && (!(MIC & MICIFG))){
      MIC |= MICIFG;
      if(MIC & MICIE){
        sensorFlags |= SENSOR_MICROPHONE; //WAKEUP   
        WAKEUPLPM1(ADC12ISR, MF_SENSORS);  
      } 
    }
    #endif
  }
}
#endif

interrupt(TIMERB1_VECTOR) Timer_B1() {
  dcoDiff = TBCCR6 - dcoCap;
  dcoCap = TBCCR6;            
  TBCCTL6 &= ~(CCIFG);
}

interrupt(WDT_VECTOR) wdt_isr() {
  WAKEUPLPM1(wdt_isr, 0x01);
}

bool initPorts() {
  #if defined(ESB430) || defined(EYE430)
  P1SEL = 0x00;
  P1DIR = 0x81;       // Outputs: P10=IRSend, P17=RS232RTS
                      // Inputs: P11=Light, P12=IRRec, P13=PIR, P14=Vibration, 
                      //         P15=Clockalarm, P16=RS232CTS
  P1OUT = 0x00;
  P2SEL = 0x00;       // Select for beeper, it is controlled via TimerA1
  P2DIR = 0x7F;       // Outpus: P20..P23=Leds+Beeper, P24..P26=Poti
                      // Inputs: P27=Taster
  P2OUT = 0x77;
  P3SEL = 0xE0;       // Sels for P34..P37 to activate UART, 
  P3DIR = 0x5F;       // Inputs: P30..P33=CON4, P35/P37=RXD Transceiver/RS232
                      // Outputs: P34/P36=TXD Transceiver/RS232
  P3OUT = 0x00;       // Output a Zero on P34(TXD Transceiver) and turn SELECT off when receiving!!!
  P4SEL = 0x00;       // CON5 Stecker
  P4DIR = 0xFF;
  P4OUT = 0x00;
  P5SEL = 0x00;       // P50/P51= Clock SDA/SCL, P52/P53/P54=EEPROM SDA/SCL/WP
  P5DIR = 0xF0;       // P56/P57=Transceiver CNTRL0/1, P55=Sensors ON/OFF (0 ON, 1 OFF)
  P5OUT = 0x2F;
  P6SEL = 0x00;       // P60=Microphone, P61=PIR digital (same as P13), P62=PIR analog
  P6DIR = 0x00;       // P63=extern voltage, P64=battery voltage, P65=Receive power
  P6OUT = 0x00;
  #elif defined(ESS430)
  P1SEL = 0x00;       // No Sels
  P1DIR = 0x00;       // Outputs: none
                      // Inputs:  all
  P1OUT = 0x00;
  P2SEL = 0x00;       // No Sels
  P2DIR = 0xCF;       // Outpus: P20..P23=Leds+Beeper, P24..P26=Poti
                      // Inputs: P27=Taster
  P2OUT = 0x07;
  P3SEL = 0xE0;       // Sels for P34..P37 to activate UART, 
  P3DIR = 0x00;       // Inputs: P30..P33=CON4, P35/P37=RXD Transceiver/RS232
                      // OutPuts: P36/P38=TXD Transceiver/RS232
  P3OUT = 0xE0;       // Output a Zero on P34(TXD Transceiver) and turn SELECT off when receiving!!!
  P4SEL = 0x00;       // CON5 Stecker
  P4DIR = 0xFF;
  P4OUT = 0x00;
  P5SEL = 0x00;       // P50/P51= Clock SDA/SCL, P52/P53/P54=EEPROM SDA/SCL/WP
  P5DIR = 0x1C;       // P56/P57=Transceiver CNTRL0/1
  P5OUT = 0x0F;
  P6SEL = 0x00;       // P60=Microphone, P61=PIR digital (same as P13), P62=PIR analog
  P6DIR = 0x00;       // P63=extern voltage, P64=battery voltage, P65=Receive power
  P6OUT = 0x00;
  #elif defined(ECR430)
  P1SEL = 0x00;       // Free: alle ausser P14
  P1DIR = 0xEF;       // Outputs: None
                      // Inputs: P14=Vibration, 
  P1OUT = 0x00;
  P2SEL = 0x00;       // free: P21, P22, P27
  P2DIR = 0xFF;       // Outpus: P20=Red Led, P23=LM71 /CS, P24..P26=Poti
  P2OUT = 0x79;       // Output all 1s for POTI, LM71 CS and 
  P3SEL = 0xE0;       // Sels for P34..P37 to activate UART, 
  P3DIR = 0x5F;       // Inputs: P35/P37=RXD Transceiver/RS232
                      // Outputs: P34/P36=TXD Transceiver/RS232
  P3OUT = 0x00;       // Output a Zero on P34(TXD Transceiver) and turn SELECT off when receiving!!!
  P4SEL = 0x00;       // CON1 / CON2 Stecker
  P4DIR = 0xFF;
  P4OUT = 0x00;
  P5SEL = 0x00;       // P50[In]/P51[Out]= LM71 SDA/SCL, P52/P53/P54=EEPROM SDA/SCL/WP, 
  P5DIR = 0xF2;       // P56/P57=Transceiver CNTRL0/1, P55=[LM71, EEPROM, Con1, Con2] ON/OFF (0 ON, 1 OFF)
  P5OUT = 0x0C; // Power fro LM71,EEPROM,Con1,Con2 is on at initialization to read Configuration
  P6SEL = 0x30;       // Free: P60 P61 P62 P63 P66 P67
  P6DIR = 0xCF;       // P64=battery voltage, P65=Receive power
  P6OUT = 0x00;
  #elif defined(ESF430)
  P1SEL = 0x00;       // No Sels
  P1DIR = 0x02;       // Inputs P10..P16 USB RTS/CTS/DTR/DSR/DCO/RI/SLEEP, Outputs CTS 
  P1OUT = 0x00;       // all
  P2SEL = 0x00;       // No Sels
  P2DIR = 0x70;       // Outputs: P24/P25/P26 DS1804 /INC U/D /CS
  P2OUT = 0x70;
  P3SEL = 0xE0;       // Sels for P34..P37 to activate UART
  P3DIR = 0x5F;       // Inputs: P30..P33=CON4, P35/P37=RXD Transceiver/RS232  
  P3OUT = 0xE0;       // Output a Zero on P34(TXD Transceiver) and turn SELECT off when receiving!!!
  P4SEL = 0x00;       // CON4
  P4DIR = 0xFF;       // Outputs: (JTAG-Interface zur Weiterprogrammierung)
  P4OUT = 0xFF;
  P5SEL = 0x00;       // P55=LED P52/P53/P54=EEPROM SDA/SCL/WP P56/P57=Transceiver CNTRL0/1
  P5DIR = 0xD1;       // Outputs: P50 LED  P54 WP, P56/57 CNTRL0/1
  P5OUT = 0x01;       // Inputs: P52/P53, LED IS ON
  P6SEL = 0x00;       // No Sels
  P6DIR = 0x00;       // Input: P65 BBOut,CMPIn
  P6OUT = 0x00;
  #else
  #error No device for initPorts()!
  #endif
  return True;
}

void wd_init(){
    // It was a POR/PUC.
    if(TACTL == 0x0000) wd_startup = STARTUP_POR;    
    // PUC only: look further.
    else {                                      
      if(FCTL3 & KEYV){ // PUC: Was it a flash key violation?
        FCTL3 = 0xA500; // Clear KEYV Bit in FCTL3, check if wanted or by accident.
        if(WDSTATE==WDS_SWRESET) wd_startup = STARTUP_SWRESET;
        else if(WDSTATE==WDS_PANIC) wd_startup = STARTUP_PANIC;
        else if(WDSTATE==WDS_REPROGRAM) wd_startup = STARTUP_REPROGRAM;
        else                      wd_startup = STARTUP_KEYV;
      }
      else if(IFG1 & WDTIFG){ // PUC: Was it the watchdog?
        IFG1 &= ~WDTIFG; // Clear WDTIFG Bit. 
        wd_startup = STARTUP_WATCHDOG;
        // WDSTATE could be further evaluated here, where occured the wdt expiry?
      }
      else { // PUC only but no watchdog? 
        wd_startup = STARTUP_UNKNOWN;
      }
    }
    WDSTATE = WDS_RUNNING;
}

void wd_stackCheck(){
  extern UINT16 __noinit_end;
  UINT16* p = & __noinit_end;
  if(p[0]!=0xABCD || p[1]!=0xABCD || p[2]!=0xABCD || p[3]!=0xABCD){
    if(Configuration.panicFlags & PANIC_ON_STACK_OVERFLOW) System_panic("Stack LOW");
    else System_reset();
  }
  Timers_add(1024, wd_stackCheck, 0xFFFF);    
}

/** Writes something to userappControl */
void panic_w(UINT8 c){
  if(panic_dest < (UINT8*)0x1080 || 
     panic_dest > (UINT8*)0x10FF) return;
  if(panic_dest == (UINT8*)0x10FF) c = 0x00;
  ///////////////////////////// disable all interrupts
  IFG1=0;                    // clear interrupt flag1 
  WDTCTL=0x5A80;             // stop WD 
                             // DCO(SMCLK) is 2,4576MHz, /6 = 409600 Hz 
  FCTL2=0xA5C5;              // select SMCLK for flash timing, divider 5+1 
  _DINT();                   // disable all interrupts to protect CPU 
                             // during programming from system crash 
  IE1=0x00;                  // disable all NMI-Interrupt sources
  /////////////////////////////  
  FCTL3 = 0xA500;              // Lock = 0 
  while(FCTL3 & 0x0001) nop(); // Wait for BUSY = 0, not needed unless run from RAM
  FCTL1 = 0xA540;              // WRT = 1 
  *panic_dest =  c;
  panic_dest++;    
  FCTL1 = 0xA500;              // WRT = 0 
  FCTL3 = 0xA510;              // Lock = 1 
  ///////////////////////////// Dotn need to reenable watchdog or ints as reset will follow
}

bool System_clStartConversion(){
  #if defined(ESB430) || defined(EYE430) || defined(ESS430)
  do cl_start();                    // do start until BUS_WRITE is acked
  while(!cl_writeOnBus(BUS_WRITE)); // control byte
  cl_writeOnBus(0xEE);              // start conversion
  cl_stop();  	
  return True;
  #else
  return False;
  #endif
}

temp_t System_getTemperature(){ 
  #if defined(ESB430) || defined(EYE430)
  temp_t temperature;
  temperature.u = getReg16bit(ACC_RT,0xFFFF); 
  temperature.u >>= 1; // Shift one down
  if(temperature.u & 0x4000) temperature.u |= 0x8000; // extend sign
  return temperature;
  #elif defined(ECR430)
  UINT16 i;
  UINT16 devid = 0;
  temp_t temperature;
  temperature.u = 0;
  SCL_LOW; // Ensure clock is low
  NOPS;
  CS_LOW; // Start communication 
  NOPS;
  // Read temperature
  for(i=0; i<16; i++){
    SCL_HIGH;
    NOPS;
    if(SDA) temperature.u++; // Set 1 in LSB
    if(i<15) temperature.u <<= 1; // Shift one digit left
    SCL_LOW;
    NOPS;
  }
  // Write comamnd 0x1111 = Shutdown
  P5DIR |= 0x01; // SDA is output
  for(i=0; i<16; i++){
    SDA_HIGH; // write all 1s
    SCL_HIGH;
    NOPS;
    SCL_LOW;
    NOPS;
  }
  P5DIR &= ~0x01; // SDA input again
  // Now read device id
  for(i=0; i<16; i++){
    SCL_HIGH;
    NOPS;
    if(SDA) devid++; // Set 1 in LSB
    if(i<15) devid <<= 1; // Shift one digit left
    SCL_LOW;
    NOPS;
  }  
  // Write comamnd 0x0000 = continous conversion
  P5DIR |= 0x01; // SDA is output
  for(i=0; i<16; i++){
    SDA_LOW; // write all 1s
    SCL_HIGH;
    NOPS;
    SCL_LOW;
    NOPS;
  }
  P5DIR &= ~0x01; // SDA input again
  //End of communication
  CS_HIGH;
  temperature.u &= 0xFFFC; // Clear last two bits as they are always 1.
  return temperature;    
  #else
  temp_t temperature;
  temperature.u = 0;
  temperature.s = 0;
  return temperature;
  #endif
}

bool System_setDCO (){
  UINT16 Compare, Oldcapture = 0, i, direction = 0;
  BCSCTL1 = 0xB7; // ACLK is devided by 8 = 4096 Hz. RSEL=6 no division for MCLK and SSMCLK. XT2 is off.
  BCSCTL2 = 0x00; // Init FLL to desired frequency using the 32762Hz crystal DCO frquenzy = 2,4576 MHz   
  DCOCTL = 0xFF;
  for (i = 0xFFFF; i > 0; i--);         // Delay for XTAL to settle  
  CCTL2 = CCIS0 + CM0 + CAP;            // Define CCR2, CAP, ACLK
  TACTL = TASSEL1 + TACLR + MC1;        // SMCLK, continous mode
  while (1){
    CCTL2 &= ~(CCIFG + COV);             // clear IFG and possible overflow
                                        // 1. capture
    while (!(CCTL2 & CCIFG));           // Wait until capture occured!    
    CCTL2 &= ~CCIFG;                    // Capture occured, clear flag
    Oldcapture = CCR2;                  // Save current captured SMCLK
                                        // 2. - 5. capture, "software divider" so aclk stays untouched and 2400 bps are working!
    //for(i=0; i<4; i++){
      while (!(CCTL2 & CCIFG));         // Wait until capture occured!    
      CCTL2 &= ~CCIFG;                  // Capture occured, clear flag
      Compare = CCR2;
    //}
    Compare = Compare - Oldcapture;     // SMCLK difference
    if (DELTA == Compare){  
      break;        // if equal, leave "while(1)"
    }
    else if (DELTA < Compare){           // DCO is too fast, slow it down
      if(direction == UP) break;          // stop if we were going up (jumped over exact delta)
      if(!direction) direction = DOWN;    // if starting set direction
      DCOCTL--;
      if (DCOCTL == 0xFF){
         if(!((BCSCTL1 & 0x07) == 0x00)){
           WDTCTL = 0xDEAD;
         }      
         BCSCTL1--;     // Did DCO role under? -> Select next lower RSEL
      }
    }    
    else {                      
      if(direction == DOWN) break;          // stop if we were going down (jumped over exact delta)
      if(!direction) direction = UP;       // if starting set direction
      DCOCTL++;
      if (DCOCTL == 0x00){
         if((BCSCTL1 & 0x07) == 0x07){
           WDTCTL = 0xDEAD;
         }      
         BCSCTL1++;     // Did DCO role over? // -> Select next higher RSEL
      }                                               
    }
  }  
  CCTL2 = 0;                              // Stop CCR2 function
  TACTL = 0;                              // Stop Timer_A 
  dcoDiff = 0;
  checkDCOState = 0; 
  return True;
}

bool System_startWatchdog() {
  do { WDTCTL = WDT_ARST_1000; TACCR1 = TAR + 4096; TACCTL1 = CCIE; }while(0);
  return True;
}

bool System_stopWatchdog() {
  do { WDTCTL = WDTPW + WDTHOLD; TACCTL1 &= ~CCIE; }while(0);
  return True;
}


bool System_registerCallback(UINT8 type, fp_t fp) {
  callbacks[type] = fp;
  return True;
}

bool System_reset() { 
  WDSTATE = WDS_SWRESET;
  FCTL3 = 0xDEAD;
  return True;
}

bool System_init() {
  wd_stackCheck();
  if((Configuration.panicFlags & PANIC_ON_BAD_RESET) && (wd_startup > STARTUP_PANIC)){
    System_panic("RESET[%s]", System_resetReason());
  }
  return True;
}

const UINT8* System_resetReason() {
  return reason_strings[wd_startup];
}

bool System_panicClear() {
  UINT8 ie1 = IE1;
  ///////////////////////////// disable all interrupts
  IFG1=0;                    // clear interrupt flag1 
  WDTCTL=0x5A80;             // stop WD 
                             // DCO(SMCLK) is 2,4576MHz, /6 = 409600 Hz 
  FCTL2=0xA5C5;              // select SMCLK for flash timing, divider 5+1 
  _DINT();                   // disable all interrupts to protect CPU 
                             // during programming from system crash 
  IE1=0x00;                  // disable all NMI-Interrupt sources
  /////////////////////////////
  FCTL3 = 0xA500;               // Lock = 0   
  while(FCTL3 & 0x0001) nop();  // Wait for BUSY = 0, not needed unless run from RAM
  FCTL1 = 0xA502;               // ERASE = 1 
  *((UINT16*)0x1080) = 0xFFFF;            // erase 2. Flash info segment 
  FCTL1 = 0xA500;               // ERASE = 0 automatically done?!
  FCTL3 = 0xA510;               // Lock = 1 
  ///////////////////////////// 
  System_startWatchdog();    // 
  _EINT();                   // 
  IFG1 = 0x00;               // 
  IE1= ie1;                  // 
  /////////////////////////////
  return True;
}

bool System_panic(const char *fmt, ...){
  extern volatile time_t currentTime; // interrupts are disabled so access direct
  va_list argp;
  btime_t btime;
  dint();
  IFG1 = 0x00;
  panic_dest = (UINT8*) 0x1080;
  while(*panic_dest != 0xFF && panic_dest < (UINT8*)0x10FF) panic_dest++;
  Time_convertSystemTime((time_t*)&currentTime, &btime);
  Time_writeTime(panic_w, &btime);  
  panic_w(' ');
  va_start(argp, fmt);
  String_abstractWrite(panic_w, fmt, argp);  
  va_end(argp);
  panic_w(0x00);
  WDSTATE = WDS_PANIC;
  _NOP();
  FCTL3 = 0xDEAD;
  return True;
}

UINT8* System_panicReason(UINT16 n){
  UINT8* p = (UINT8*)0x1080;
  UINT16 i;
  for(i=0; i<n; i++){
    while(*p != 0x00) p++;
    p++;
  }
  if(*p != 0xFF && p < (UINT8*)0x10FF) return p;
  else           return NULL;
}

bool System_wait(UINT32 w){
  volatile UINT32 i; 
  for(i=0;i<w;i++) nop(); 
  return True;
}

bool System_waitA(UINT16 i) {
  UINT16 tar = TAR;
  while(TAR - tar < i) System_startWatchdog();
  return True;
}

__attribute__((reserve(2))) int main(void) {
  Data_redOn();
  WDTCTL = WDT_ARST_1000;
  System_stopWatchdog();
  initPorts();
  wd_init();
  runModule = 0;
  #if defined(ESF430)
  while(! P1IN & 0x40); // Wait until FTDI enables power.
  System_wait(10000);
  #endif
  enableADU();
  initClock();
  Timers_init();
  eint();
  Configuration_init();
  Comm_init(BPS_115200);
  Net_init();
  Data_init();
  System_init();
  Configuration_print();
  volatile UINT16 i;
  for(i=0; i<NUMBER_CALLBACKS; i++) callbacks[i] = 0;  
  Threading_init();
  Messaging_enable();
  if((Configuration.panicFlags & PANIC_DISABLES_APP) && (System_panicReason(0) != NULL)){
    Comm_print("Info: Userapp disabled.\r\n");
  }
  else {
    Comm_print("Info: Userapp enabled.\r\n");
    Process_init();
  }
  Data_redOff();
  for(;;) {  
    System_startWatchdog(); // starts&resets the watchdog
    if(runModule & MF_SCOS) Threading_eventHandler();
    //// Radio tasks.
    if(runModule & MF_RADIO_RX) Net_rxHandler();  
    if(runModule & MF_RADIO_TX) Net_txHandler();
    if(runModule & MF_TIMER) Timers_eventHandler();  
    if(runModule & MF_SERIAL_RX) { // Callback for serial line
      extern volatile UINT8* serial_line;
      if(serial_line != 0){
      	if(callbacks[C_SERIAL]) callbacks[C_SERIAL]((void*)serial_line);
      }
    }
    if(runModule & MF_SENSORS) if(sensorFlags != 0x00) Data_sensorHandler();
    if(runModule & MF_RC5) Data_RC5ReceiveHandler();
    dint(); 
    nop();
    if(runModule==0) { // enter lpm3 and enable GIE at once to not loose an interrupt
      System_stopWatchdog();
      eint();
      LPM1;
    } else eint();
  }  
}
