/*
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://www.scatterweb.net and the mailinglist
scatterweb@lists.spline.inf.fu-berlin.de (subscription via the Website).
Berlin, 2003/2004/2005
*/

/** \file funk.c
 ** \ingroup Firmware
 ** \brief Methods and state machine for sending/receiving packets to/from radio.

This file contains following functionality for radio:

- Functions to set the radio mode (Off, OOK, ASK, Rx) for internal use.
- Setting of transmit power (in 100 steps).
- Interrupt driven Rx&Tx radio MAC logic (sending of packets with CSMA and random 
  backoff, CRC-16 checksums, acknowledgement with automatic retransmission).


<h1> General </h1>

Radio software supports acknowledgements, so it is important to know
that all packet types which are no broadcast will get acknowledged by radio software 
and get repeated several times if needed (::MAX_TX_RETRIES). 

The radio software uses the num field for acknowledged packets to recognize
doubles. 

::packet_t is a struct containing a packet header fields and pointers to packet data 
and is used for sending & receiving. 

As a packet may be often constructed out of a header portion and a data portion,
::packet_t supports two data pointers to get passed in for sending, so these portions
don't need to be copied together. When receiving however, data pointer contains header
and data consecutively and header and header_length are null.

A good example for sending & receiving packets is flasher.c.
<br>

\image html packet.gif "ScatterWeb packet format for TR1001"

<h1> How to receive packets </h1>

To receive packets of a specific type one must define a handler function and add
it to ::radio_tx_heartbeat for firmware handlers or to the ::C_RADIO userapp 
callback function. 

When the handler is then called, the function can access the received packet
in ::rxPacket (No direct access to ::rxBuf is given anymore, so it will be
easier to change the implementation). 

<h1> How to send packets </h1>

For sending packets a ::packet_t must be initialized:
- to must contain the destination id of receiver or broadcast address.
- type must contain the packet type.
- header/data must be set to NULL or to the data that should get transported. 
  If set, header_length/data_length should contain the length of the data.
- num can be set to transport a byte for not acknowledged packets, otherwise
  it will get overwritten.
- from is always overwritten with the id of this device.

Such a packet can then be queued into tx ring buffer with ::radio_queue_packet
and a pointer to initialized ::packet_t.

For simple packets (which consist of no data) a wrapper function which
only needs destination and type exists: ::radio_queue_prim.

Additionally these two functions accept a callback pointer to functions which
accept an UINT8. If set, this callback is called when the packet is
removed from the tx ring buffer:
- For unacknowledged packets this is after sending the packet.
- For acknlowedged packets this is after successfully receiving an ACK for
  this packet (callback argument is set to 1) or when ::MAX_TX_RETRIES retries
  are made without sucess (callback argument is set to 0). 

The callback function can check the variable ::txRetries to see how much 
retries it took (always 0 for unacknowledged packets).

Code for terminal command pia demonstrates sending acknowledged packets:
\dontinclude terminal.c
\skip void term_pia_result
\until } // term_pia() end

<h1> Synchronization </h1>
To do synchronization you must implement a handler for ::SYNCH_PACKET.
When a ::SYNCH_PACKET has been received and the local clock should be
synchronized to the received packet call ::doSynch. To send
a ::SYNCH_PACKET use ::radio_queue_synch.


<h1> Rx&Tx State machine overview</h1>
\image html radiosm.gif "Radio Rx and Tx State Machine and interaction"

\attention INTERNAL: when receiving P3SEL for P34(FunkTXD) must be off and a 0 must be output on P34 to achieve good results!!!!! 


**/

#include "ScatterWeb.System.h"

#define STARTBYTE0    (0xFF)
#define STARTBYTE1    (0x01)
#define STARTBYTE2    (0x7F)
#define DUMMYBYTE     (0x00)

/// \name Packet format (names for byte positions).
///
/// Packet format:     | To[0,1] | From[2,3] | Type[4] | Num[5] | Length[6,7] | Data[8]
/// Number of Bytes    |    2    |     2     |    1    |    1   |      2      | Length
//@{
#define   PACKET_TO      	(0)
#define   PACKET_FROM  	 	(2)
#define   PACKET_TYPE  		(4)
#define   PACKET_NUM    	(5)
#define   PACKET_LENGTH  	(6)
#define   PACKET_DATA    	(8)
//@}

/// \name Receiving side state machine (rxState).
//@{    
#define   RXSTATE_ERROR     0          ///< Receied packet is wrong, see receivedFlags. 
#define   RXSTATE_START1    1          ///< Waiting for first start byte 0x01. 
#define   RXSTATE_START2    2          ///< Waiting for second start byte 0x7F. 
#define   RXSTATE_BYTE      3          ///< Waiting for first half of a byte. 
#define   RXSTATE_BYTE2     4          ///< Waiting for second half of a byte.
#define   RXSTATE_CSUM1     5
#define   RXSTATE_CSUM2     6
#define   RXSTATE_CSUM3     7
#define   RXSTATE_CSUM4     8
#define   RXSTATE_FULL      9          ///< Complete packet received. 
//@}

/// \name receivedFlags: Flags for errors while receiving.
/// ::receivedFlags will be set if ::rxState==::RXSTATE_ERROR, not fully implemented
//@{ 
volatile UINT8 receivedFlags;  ///< Flags for received packet.
#define RXERROR_TOO_LONG       (0x02)  ///< Correct length received, but to long for rxBufer.
#define RXERROR_CHECKSUM       (0x10)  /// 
//@}

/// \name Receiving side variables.
//@{

volatile UINT8 rxState;                 ///< State of receiving state machine.
volatile UINT16 rxBufPos;                 ///< Next position in the buffer.
volatile UINT16 rxDataLength;             ///< Length of the packet, set after the sixth byte.
volatile UINT8 rxBuf[RADIO_RXBUF_SIZE+PACKET_DATA+2]; ///< Receiving buffer.
volatile UINT8 cdCounter;               ///< If >0 radio someone is sending.
volatile UINT8 detect;
volatile packet_t rxPacket;                     ///< Static struct for received Packets, user functions should use this!
packet_t sentPacket;

volatile struct {
  time_t synchTime;
  UINT16 checksum;
} synch;

volatile time_t rxTime;
volatile  UINT16 rx_c_csum;
volatile UINT8 rx_r_csum[2];

#define MAX_RXNUM_TABLE     (5)                 ///< Length of ::rxNumTable.

struct {                        
  UINT16 id;
  UINT8 num;
} rxNumTable[MAX_RXNUM_TABLE]; ///< This table holds previously received packet numbers with corresponding id.

UINT8 rxNumTablePos;                    ///< Next pos to enter a new value into ::rxNumTable.
//@}

/// \name Transmitting side state machine (txState).
//@{
#define RTX_IDLE        (0x10)  ///< Idle state. Set to ::RTX_WAITING when ::radio_tx_heartbeat detects data in buffer. All control is then done by the tx interrupt.
#define RTX_WAITING     (0x11)  ///< Waiting for free channel. Set to ::RTX_BACKOFF when free channel is recognized.
#define RTX_BACKOFF     (0x12)  ///< Backoff Time is counted down. Set to ::RTX_WAITING if channel is busy again and set to ::RTX_TURNON if backoff time elapsed.
#define RTX_TURNON      (0x13)  ///< Puts a constant 1 for some bytes on the receiver for packet detection, then goes to ::RTX_PREAMBLE
#define RTX_PREAMBLE    (0x14)  ///< Send some preamble bytes (0xAA) and then goes to RTX_SENDING1.
#define RTX_FF          (0x15)  ///< Send 0xFF.
#define RTX_01          (0x16)  ///< Send 0x01.
#define RTX_7F          (0x17)  ///< Send 0x7F.
#define RTX_SENDING1    (0x18)  ///< Toggles between ::RTX_SENDING1 and ::RTX_SENDING2 until all data is sent.
#define RTX_SENDING2    (0x19)  ///< Toggles between ::RTX_SENDING1 and ::RTX_SENDING2 until all data is sent.
#define RTX_POSTAMBLE   (0x1A)  ///< Send some postamble bytes.
#define RTX_ACKWAITING  (0x1B)  ///< We are waiting for an ACK.
#define RTX_SENT        (0x1C)  ///< Final state after sending one packet. ::tx_heartbeat takes control again.
#define RTX_NOTSENT     (0x1D)  ///< Final state after packet has been sent MAX_RETRIES without ack.
#define RTX_A_TURNON    (0x20)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_PREAMBLE  (0x21)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_FF        (0x22)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_01        (0x23)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_7F        (0x24)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_SENDING1  (0x25)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_SENDING2  (0x26)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_POSTAMBLE (0x27)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_S_SENDING1  (0x28)
#define RTX_S_SENDING2  (0x29)
//@}

/// \name Transmitting side variables.
//@{
volatile UINT8 txState;                 ///< State of transmitting state machine.
volatile UINT16 txCounter;                ///< General counter used in different ways depending on ::txState.
volatile UINT16 txLength;                 ///< Length of packet to transmit.
volatile UINT16 txPos;                    ///< Current pos in packet to transmit.
volatile UINT16 txEncodeBuf;              ///< Encoding buffer while transmitting.
volatile UINT8 txRetries;               ///< Number of retries done.
volatile UINT8 txNum;                   ///< Current sequence number used in next packet.
volatile UINT8 ackBuffer[8];            ///< Holds the acknowledge packet.
//@}

/// \name Transmitting side ringbuffer.
//@{

#define UINT(x)  (*((UINT16*)& x))

/// Complete size.
#define RADIOTX_RB_SIZE		    (RADIO_TXBUF_SIZE)
/// Get current length.
#define RADIOTX_RB_LEN(b)		  ( (b).tail >= (b).head ? (b).tail - (b).head : (b).tail + RADIOTX_RB_SIZE - (b).head )
/// Get free bytes.
#define RADIOTX_RB_FREE(b)    ( RADIOTX_RB_SIZE - (RADIOTX_RB_LEN(rtxrb) - 1))
/// True if buffer is empty.
#define RADIOTX_RB_EMPTY(b)	  ( (b).head == (b).tail )
/// True if buffer is full.
#define RADIOTX_RB_FULL(b)		( RADIOTX_RB_LEN(b) == (RADIOTX_RB_SIZE-1) )
/// Initializes the ringbuffer.
#define RADIOTX_RB_INIT(b)		{ (b).head = 0; (b).tail = 0;							}
/// Gets the first byte of the buffer and remove it.
#define RADIOTX_RB_GET(b, x)	{ x=(b).buffer[(b).head]; if(++((b).head) >= RADIOTX_RB_SIZE) (b).head = 0;	}
/// Gets the i-th byte from current position without altering anything.
#define RADIOTX_RB_I(b, i)    ( (b).buffer[( ((b).head+i) >= RADIOTX_RB_SIZE ? ((b).head+i) - RADIOTX_RB_SIZE : ((b).head+i) )] )

#define RADIOTX_RB_INT(b, i)   UINT(RADIOTX_RB_I(b, i))

/// Appends another bytes to the buffer.
#define RADIOTX_RB_PUT(b, x)	{ (b).buffer[(b).tail]=x; if(++((b).tail) >= RADIOTX_RB_SIZE) (b).tail = 0; 	}

volatile struct { 
	UINT8 buffer[RADIOTX_RB_SIZE];
	UINT16 head, tail;
} rtxrb; ///< Ringbuffer

//@}

UINT8 funk_readChar(UINT16 adr){
  UINT8 result;
  IO_read(adr, (UINT8*)&result, 1);
  return result; 
}

/// \name Basic functions.
//@{
 
/** \brief Sets transceiver into receive mode.
 **
 **  IMPORTANT: when receiving P3SEL for P34(FunkTXD) must be off 
 **  and a 0 must be output on P34 to achieve good results!!!!!               */

void setFunkRx(void){
  P3SEL = 0xE0; 
  P5OUT |= 0xC0;     
}

/** Sets transceiver into ASK mode (amplitude-shift keyed). */
/*
void setFunkTxASK(void){
  P3SEL = 0xF0;
  P5OUT |= 0x80;
  P5OUT &= 0xBF;
}*/

/** Sets tranceiver into OOK mode (on-off keyed). */
/*void setFunkTxOOK(void){
  P3SEL = 0xF0;
  P5OUT |= 0x40;
  P5OUT &= 0x7F;
}*/

/** Turns transceiver completely off. */
void setFunkOff(void){
  P5OUT &= 0x3F;
}

#define POTI_PORT     (P2OUT)
#define POTI_INC      (0x10)
#define POTI_UD       (0x20)
#define POTI_CS       (0x40)

/** Set the transmit power (0 <= n <= 100). */
bool Net_setTxPower(UINT8 n, UINT8 write) {
  int i;
  if(n>100) return False;
  // Potipegel auf null fahren
  POTI_PORT &= ~POTI_UD;                                // P25 = 0 (down selected)
  POTI_PORT &= ~POTI_CS;                                // P26 = 0 (chipselect on)
  for(i=0;i<102;i++) {
     POTI_PORT &= ~POTI_INC;                             // P24 = 0 (inc)
     POTI_PORT |= POTI_INC;    
  }
  // Potipegel==0, Potipegel auf n setzen
  POTI_PORT |= POTI_UD;                                // P25 = 1 (up selected)
  for(i=0;i<n;i++) {
    POTI_PORT &= ~POTI_INC;                              // P24 = 0 (inc)
    POTI_PORT |= POTI_INC;    
  }
  POTI_PORT |= POTI_CS;                                // P26 = 1 (chipselect off)
  Configuration.transmitPower=n;
  if(write) Configuration_save();
  return True;
}

/** \brief Sends a single byte over radio (don't use it).
 ** 
 ** This method will block until byte can be written and is of no use anymore
 ** as the interrupt handler does this.
 */
void sendFunk(UINT8 data){
  while (!(IFG1 & UTXIFG0));        // loop until USART0 TX buffer ready
  TXBUF0 = data;
}

//@}

/** \name Encoding functions
The actual implementation of the Manchester Encoding algorithm is slightly different from described 
above and seen in Figure 3. In order to avoid unnecessary bit manipulation, the bits are encoded in
a unique way. It is faster to encode alternate bits in each byte so that we can process four bits 
at a time.. Just shift, mask, complement and OR the bits together to get an encoded byte. 
The odd bits (7,5,3,1) are encoded into the most significant 8 bits of data to return and 
the even bits (6,4,2,0) are encoded into the least significant 8 bits of data to return. 
In this implementation, 0x0E (00001110) encodes into 0x5AA6 (0101101010100110) instead of the 
standard 0x55A9 (0101010110101001). 

Macros are now used instead of the original functions:
\code
UINT16 man_encode(UINT8 unenc){
  UINT8 oddbyte, evenbyte;
  oddbyte  = (unenc & 0xAA) | ((~unenc & 0xAA)>>1);
  evenbyte = (unenc & 0x55) | ((~unenc & 0x55)<<1);
  return ((oddbyte<<8) | evenbyte);
}
\endcode

\code
UINT8 man_decode(UINT8 odd_byte, UINT8 even_byte, UINT8* dest) {
	if((odd_byte&0xAA)^((~odd_byte&0x55)<<1)){
		return 0;
	} else odd_byte&=0xAA;

	if((even_byte&0x55)^((~even_byte&0xAA)>>1)) {
		return 0;
	} else even_byte&=0x55;

	*dest = (odd_byte|even_byte);
    return 1;
}
\endcode

*/
//@{
#define MAN_ENCODE(unenc)     ((((unenc & 0xAA) | ((~unenc & 0xAA)>>1))<<8) | ((unenc & 0x55) | ((~unenc & 0x55)<<1)))
#define MAN_DECODE(odd, even) ((odd & 0xAA)|(even & 0x55))
//@}

/// \name Receiving side state machine.
//@{

/** Resets state machine. Used at init and after reception of each packet. */
void rxClear(void){
  runModule &= ~MF_RADIO_RX;
  rxBufPos = 0;
  rxDataLength = 0;
  rxpValue = 0;
  receivedFlags = 0;
  rxState = RXSTATE_START1; 
  rx_c_csum = INIT_FCS;
  rx_r_csum[0] = 0x00;
  rx_r_csum[1] = 0x00;
}

void printRxPacket(){
  if(rxState==RXSTATE_FULL)  Comm_print("+ ");
  if(rxState==RXSTATE_ERROR) Comm_print("- ");
  if(receivedFlags & RXERROR_CHECKSUM)     Comm_print("CS [csr: %x csc: %x]", UINT(rx_r_csum[0]), rx_c_csum);
  Comm_print("(%i)", detect);
  Comm_print("%b[%i -> %i] %i,%i\r\n", rxBuf[PACKET_TYPE], UINT(rxBuf[PACKET_FROM]), UINT(rxBuf[PACKET_TO]), 
                                                        UINT(rxBuf[PACKET_LENGTH]), rxBuf[PACKET_NUM]);
  if(rxBuf[PACKET_TYPE] == PING_PACKET && rxState==RXSTATE_ERROR){
    UINT16 i; 
    Comm_print("WRONG BYTES: ");
    for(i=0; i<rxPacket.data_length; i++){
      if( rxPacket.data[i] != (UINT8)i ){
        Comm_print("%i(%i) ", i, rxPacket.data[i]);
      }
    }
    Comm_print("\r\n");
  }
    //print("BAD packet received: rxp=%i length=%i wrong=%i which=%i pos=%i ",
    //rxpValue, rxDataLength, receivedWrongBytes, whichByteWasWrong, rxBufPos);
}

/** Handles a full or erronous receive buffer, needs to be regularly called by main. 
 ** Also some debugging information can be provided and for packets with acknowledgement 
 ** a simple dublette checking is done.  
 */
bool Net_rxHandler(void){
  UINT16 i;
	// If packet received, fill rxPacket struct.
  if(rxState==RXSTATE_FULL){ 
    rxPacket.to     = UINT(rxBuf[PACKET_TO]);
    rxPacket.from   = UINT(rxBuf[PACKET_FROM]);
    rxPacket.type   = rxBuf[PACKET_TYPE];
    rxPacket.num    = rxBuf[PACKET_NUM];
    rxPacket.header_length = 0;
    rxPacket.header = 0;
    rxPacket.data_length = UINT(rxBuf[PACKET_LENGTH]);
    rxPacket.data   = (UINT8*) &rxBuf[PACKET_DATA];
    // If debug flag is set provide info.         
    if(Comm_logLevel & DEBUG_RADIO) printRxPacket();
    if(rxPacket.type==ACK_PACKET){ rxClear(); return True; } 
    // If no broadcast check in rxNumTable for doubles and note packet num.
    // Check if already received
    for(i=0; i<MAX_RXNUM_TABLE; i++){
      if(rxNumTable[i].id == rxPacket.from && rxNumTable[i].num == rxPacket.num){ 
        rxClear(); // We already received that, remove and continue.
        return True;
      }
    }
    // First time received: enter current num into history.
    for(i=0; i<=MAX_RXNUM_TABLE; i++){
      if(i==MAX_RXNUM_TABLE){ // If no entry found from rxPacket.from create a new one..
        rxNumTable[rxNumTablePos].id = rxPacket.from;
        rxNumTable[rxNumTablePos].num = rxPacket.num; 
        rxNumTablePos++;
        if(rxNumTablePos==MAX_RXNUM_TABLE) rxNumTablePos = 0;           
      }
      else if(rxNumTable[i].id == rxPacket.from){ // else just overwrite old number.
        rxNumTable[i].num = rxPacket.num;
      }
    }    
    if( Threading_packetHandler() ) goto clear; // Deliver to ScOS:
    // Now test for receiver address, if it is for us, continue.
    if(rxPacket.to == BROADCAST || rxPacket.to == Configuration.id){
      // Now we can really deliver this packet.
      // Packet handler style functions.
      switch(rxPacket.type){
        // All other firmware internal packets go here.    	    
        case FLASH_HEADER:      handle_FLASH_HEADER();          break;   
        // If no firmware internal packet pass it up to userapp.
        default: if(callbacks[C_RADIO]) callbacks[C_RADIO]((void*)&rxPacket);
      }
    }
    else if(callbacks[C_RADIO]) callbacks[C_RADIO]((void*)&rxPacket);
    clear: rxClear();  
  }
  // Also clear an erronous buffer, if wanted provide some info.
  else if(rxState==RXSTATE_ERROR){ // packet with errors
    if(Comm_logLevel & DEBUG_RADIO) printRxPacket();
    rxClear();    
  }
  return True;
}

volatile UINT8 data1, data2;

#define RX_STANDARD_LENGTH     (50)   ///< How many bytes are received when the length is bad.

/** Receive interrupt for radio. */
interrupt (UART0RX_VECTOR) radio_rx_isr (void){
  UINT8 rec;
  if(U0RCTL & (FE + PE + OE + BRK + RXERR)){ // OE occurs when interrupts disabled for longer time like with sendIR
    U0RCTL &= ~ (FE + PE + OE + BRK + RXERR); // Users guide recommends always resetting this 
  }
  rec = RXBUF0;
  //U0RCTL &= ~(RXERR+FE+PE+OE+BRK); // If these error flags would get set they should be cleared, but they never get..
  switch(rxState){
    case RXSTATE_START1:  // Waiting for the first startbyte 0x01.
      //sendRS232(0x30+rxState);
      if(rec==STARTBYTE1) rxState=RXSTATE_START2;  
      return;
    case RXSTATE_START2: // Waiting for the second startbyte 0x7F.
      //sendRS232(0x30+rxState);
      if(rec==STARTBYTE2){
        rxState = RXSTATE_BYTE;
        detect = cdCounter;
        cdCounter = 20; // It takes 8 bytes to receive the length, doubled = 16
        { extern volatile time_t currentTime; rxTime = currentTime; }
      }
      else rxState = RXSTATE_START1;
      return;
    case RXSTATE_BYTE: // Receive a data byte.
      //sendRS232(0x30+rxState);
      data1 = rec;
      rxState = RXSTATE_BYTE2;
      return;
    case RXSTATE_BYTE2: // Check the negated byte.
      //sendRS232(0x30+rxState);
      // Note: this case gets very long if its the last byte!!
      rxState = RXSTATE_BYTE; // Should be the next state if no error occured  
      rxBuf[rxBufPos] = MAN_DECODE(data1, rec);
      rx_c_csum = System_ccittFcs(rx_c_csum, rxBuf[rxBufPos]);
      rxBufPos++;  
      // if pos==PACKET_DATA whole header is received => read out the length
      if(rxBufPos==PACKET_DATA){
          rxDataLength = UINT(rxBuf[PACKET_LENGTH]);
          if(rxDataLength > RADIO_RXBUF_SIZE){ 
            receivedFlags |= RXERROR_TOO_LONG;
            rxState = RXSTATE_ERROR;
            WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
            return;
          }
          // Update cdCounter to length of Packet.
          cdCounter = (rxDataLength << 1) + 2 + 10; // one cdCounter tick is 1 byte                    
      }
      // check if no more bytes to receive
      if( rxBufPos - (PACKET_DATA) == rxDataLength ){
          // Special case. If its an ACK and ids and numbers match
          // release our transmitting state machine from RTX_ACKWAITING. ACKs have no Checksum.
          if( (rxBuf[PACKET_TYPE]==ACK_PACKET)){
            if( (txState==RTX_ACKWAITING) &&
                ( RADIOTX_RB_INT(rtxrb, PACKET_TO) == UINT(rxBuf[PACKET_FROM]) ) && 
                ( RADIOTX_RB_I(rtxrb, PACKET_NUM)  == rxBuf[PACKET_NUM]) ){
                  txState = RTX_SENT;
                  WAKEUPLPM1(radio_rx_isr, MF_RADIO_TX);
            }
            rxState = RXSTATE_FULL;
            WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
          }  
          else rxState = RXSTATE_CSUM1;         
      } 
      return;
    case RXSTATE_CSUM1:
       //sendRS232(0x30+rxState);
     data1 = rec;
      rxState = RXSTATE_CSUM2;
      return;
    case RXSTATE_CSUM2:
      //sendRS232(0x30+rxState);
      rx_r_csum[0] = MAN_DECODE(data1, rec);
      rxState = RXSTATE_CSUM3;
      return;
    case RXSTATE_CSUM3:
      //sendRS232(0x30+rxState);
      data1 = rec;
      rxState = RXSTATE_CSUM4;
      return;
    case RXSTATE_CSUM4:
      //sendRS232(0x30+rxState);
      rx_r_csum[1] = MAN_DECODE(data1, rec);  
        if( UINT(rx_r_csum[0]) != rx_c_csum ) {
                receivedFlags |= RXERROR_CHECKSUM;
                rxState = RXSTATE_ERROR;
                WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
                break;
        }
      // Checksum ok, If packet is not a broadcast then ack this packet!      
      if(Configuration.id == UINT(rxBuf[PACKET_TO]) && txState <= RTX_BACKOFF){ // If a packet was recently received txState machine should always be somewhere before RTX_SENDING
          // Create acknowledge in ackBuffer.
          // Do ackBuffer[PACKET_TO]=rxBuf[PACKET_FROM] and ackBuffer[PACKET_FROM]=rxBuf[PACKET_TO with int instructions.             
          UINT(ackBuffer[PACKET_TO]) = UINT(rxBuf[PACKET_FROM]);
          UINT(ackBuffer[PACKET_FROM]) = UINT(rxBuf[PACKET_TO]);
          ackBuffer[PACKET_NUM] = rxBuf[PACKET_NUM];                          
          txState = RTX_A_TURNON;
      }       
      rxState = RXSTATE_FULL;
      WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
      cdCounter=8; // Safety gap after packet finish.
      return;
    case RXSTATE_FULL:  // nothing to do here, wait for someone to get the packet or error message
    case RXSTATE_ERROR: // wait until error is processed
      //sendRS232(0x30+rxState);
      return;     
    default:
      Comm_print("! undefined rxState: %b\r\n", rxState); // should never occur
      rxClear();
      return;
  }
}
//@}

/// \name Sending side state machine.
//@{

/** \brief Call this every time sending is enabled again! */
void txClear(){
  	RADIOTX_RB_INIT(rtxrb); // init/clear buffer
  	txState = RTX_IDLE;
  	txCounter = 0;
  	// init ack buffer statics: type and length
    ackBuffer[PACKET_TYPE] = ACK_PACKET;
    UINT(ackBuffer[PACKET_LENGTH]) = 0x0000;
}

/// Queues a packet if enoguh space in txBuf is available.
bool Net_send(packet_t* packet, fp_char_t callback){
  UINT16 i;
  UINT16 cb = (UINT16) callback;
  UINT16 length = packet->header_length + packet->data_length;
  UINT16 checksum = INIT_FCS;
  UINT16 offset;
  if(length & 0x0001){ //if length is uneven make it even
    length++;
    packet->data_length++;
  }
  // if disabled return other error code
  if(RADIOTX_RB_FREE(rtxrb) > length + 8 + 2 + 2){ // we have space in buf, so queue it, length + 8 data bytes, 2 checksum and 2 callback
    // inc txnum
    txNum++;
    offset = RADIOTX_RB_LEN(rtxrb); 
    // Queue header
    RADIOTX_RB_PUT(rtxrb, packet->to); // to l
    RADIOTX_RB_PUT(rtxrb, packet->to >> 8); // to h      
    RADIOTX_RB_PUT(rtxrb, Configuration.id); // from l
    RADIOTX_RB_PUT(rtxrb, Configuration.id >> 8); // from h
    RADIOTX_RB_PUT(rtxrb, packet->type); // type
    RADIOTX_RB_PUT(rtxrb, txNum); // num
    RADIOTX_RB_PUT(rtxrb, length);      // lower 8 bits of length
    RADIOTX_RB_PUT(rtxrb, length >> 8); // upper 8 bits of length  
    // Queue data header and data
    for(i=0; i<packet->header_length; i++) RADIOTX_RB_PUT(rtxrb,packet->header[i]); 
    for(i=0; i<packet->data_length; i++)   RADIOTX_RB_PUT(rtxrb,packet->data[i]);
    // Calc and queue checksum, for SYNCs only precalc header
    if(packet->type==SYNCH_PACKET){
      for(i=0; i<8; i++) checksum = System_ccittFcs(checksum, RADIOTX_RB_I(rtxrb, offset+i));
      synch.checksum = checksum;    
    }
    else {
      for(i=0; i<(8 + packet->header_length + packet->data_length) ; i++) 
        checksum = System_ccittFcs(checksum, RADIOTX_RB_I(rtxrb, offset+i));
    }
    RADIOTX_RB_PUT(rtxrb, checksum); 
    RADIOTX_RB_PUT(rtxrb, checksum >> 8); 
    // Queue function callback 
    RADIOTX_RB_PUT(rtxrb, cb >> 8); // upper 8 bits of callback
    RADIOTX_RB_PUT(rtxrb, cb);      // lower 8 bits of callback  
    // Debug output
    if(Comm_logLevel & DEBUG_RADIO) 
      Comm_print("> %b[%i -> %i] %i,%i csum: %x cb: %x free: %i\r\n", 
        packet->type, Configuration.id, packet->to, length, txNum, checksum, cb, RADIOTX_RB_FREE(rtxrb));
    runModule |= MF_RADIO_TX;
    return True;        
  }
  else {
    if(Comm_logLevel & DEBUG_RADIO) 
      Comm_print("| %b[%i -> %i] %i,%i cb: %x free: %i\r\n", 
        packet->type, Configuration.id, packet->to, length, txNum, cb, RADIOTX_RB_FREE(rtxrb));
    return False;  
  }
}

/// Queues a packet if enoguh space in txBuf is available.
char Net_esend(packet_t* packet, fp_char_t callback){
  UINT16 i;
  UINT16 cb = (UINT16) callback;
  UINT16 length = packet->header_length + packet->data_length;
  UINT16 checksum = INIT_FCS;
  UINT16 offset;
  if(packet->type==ACK_PACKET || packet->type==SYNCH_PACKET) return -2; // ACKs are internal only
  if(length & 0x0001){ //if length is uneven make it even
    length++;
    packet->data_length++;
  }
  // if disabled return other error code
  if(RADIOTX_RB_FREE(rtxrb) > length + 8 + 2 + 2){ // we have space in buf, so queue it, length + 8 data bytes, 2 checksum and 2 callback
    // inc txnum
    txNum++;
    offset = RADIOTX_RB_LEN(rtxrb); 
    // Queue header
    RADIOTX_RB_PUT(rtxrb, packet->to); // to l
    RADIOTX_RB_PUT(rtxrb, packet->to >> 8); // to h      
    RADIOTX_RB_PUT(rtxrb, Configuration.id); // from l
    RADIOTX_RB_PUT(rtxrb, Configuration.id >> 8); // from h
    RADIOTX_RB_PUT(rtxrb, packet->type); // type
    RADIOTX_RB_PUT(rtxrb, txNum); // num
    RADIOTX_RB_PUT(rtxrb, length);      // lower 8 bits of length
    RADIOTX_RB_PUT(rtxrb, length >> 8); // upper 8 bits of length  
    // Queue data header and data
    for(i=0; i<packet->header_length; i++) RADIOTX_RB_PUT(rtxrb,packet->header[i]); 
    // Take data from eeprom
    for(i=0; i<packet->data_length; i++){
      RADIOTX_RB_PUT(rtxrb, funk_readChar( ((UINT16)packet->data) + i ) );
    }
    // Calc checksum
    for(i=0; i<(8 + packet->header_length + packet->data_length) ; i++){ 
        checksum = System_ccittFcs(checksum, RADIOTX_RB_I(rtxrb, offset+i));
    }
    RADIOTX_RB_PUT(rtxrb, checksum); 
    RADIOTX_RB_PUT(rtxrb, checksum >> 8); 
    // Queue function callback 
    RADIOTX_RB_PUT(rtxrb, cb >> 8); // upper 8 bits of callback
    RADIOTX_RB_PUT(rtxrb, cb);      // lower 8 bits of callback  
    // Debug output
    if(Comm_logLevel & DEBUG_RADIO) 
      Comm_print("> %b[%i -> %i] %i,%i csum: %x cb: %x free: %i\r\n", 
        packet->type, Configuration.id, packet->to, length, txNum, checksum, cb, RADIOTX_RB_FREE(rtxrb));
    runModule |= MF_RADIO_TX;
    return 1;        
  }
  else {
    if(Comm_logLevel & DEBUG_RADIO) 
      Comm_print("| %b[%i -> %i] %i,%i cb: %x free: %i\r\n", 
        packet->type, Configuration.id, packet->to, length, txNum, cb, RADIOTX_RB_FREE(rtxrb));
    return -1;  
  }
}

/// Heartbeat function for transmitting bytes via radio.
bool Net_txHandler(){
  UINT8 tmp;
  UINT16 cb;
  fp_char_t callback;
  if(txState == RTX_SENT || txState == RTX_NOTSENT){ // if sending of one packet is finished do cleanup and go to idle
    // Copy out to sentPacket what was sent.
    sentPacket.to = RADIOTX_RB_INT(rtxrb, PACKET_TO);
    sentPacket.from = RADIOTX_RB_INT(rtxrb, PACKET_FROM);
    sentPacket.type = RADIOTX_RB_I(rtxrb, PACKET_TYPE);    
    sentPacket.num = RADIOTX_RB_I(rtxrb, PACKET_NUM);
    sentPacket.data = NULL;
    sentPacket.data_length = RADIOTX_RB_INT(rtxrb, PACKET_LENGTH);
    //txLength still contains length of packet!, use it to remove packet, TODO: rb skip operation
    // read header out to sentPacket
    while(txLength > 0){    
      RADIOTX_RB_GET(rtxrb, tmp);
      txLength--;
    }
    // remove callback and call it if set
    RADIOTX_RB_GET(rtxrb, tmp); cb = tmp << 8;
    RADIOTX_RB_GET(rtxrb, tmp); cb |= tmp;
    if(cb != 0){
      if(cb<0x1000){ // must be a pid
        if(txState==RTX_SENT) Threading_sentPacketHandler(cb, 1);
        else                  Threading_sentPacketHandler(cb, 0);
      }
      else { 
        callback = (fp_char_t) cb;
        if(txState==RTX_SENT) callback(1);
        else                  callback(0);
      }
    }
    txState = RTX_IDLE;
    if(Comm_logLevel & DEBUG_RADIO) Comm_print("radio-tx: packet SENT and removed - %i %i cb: %x\r\n", rtxrb.head, rtxrb.tail, cb);
  } 
  // if idle and something in buffer ==> try to send 
  if(txState == RTX_IDLE && !RADIOTX_RB_EMPTY(rtxrb)){ 
    if(Comm_logLevel & DEBUG_RADIO) Comm_print("radio-tx: something in buf - %i %i \r\n", rtxrb.head, rtxrb.tail);
    txRetries = 0;                
    txState = RTX_WAITING;
  }
  runModule &= ~MF_RADIO_TX;
  return True;
}

#define BACKOFF_LENGTH        (50)  ///< Backoff delay will be randomally chosen from [1, BACKOFF_DELAY+1]
#define TURNON_LENGTH         (2)   ///< How much bytes is the constant 1 kept when turning on?
#define PREAMBLE_LENGTH       (5)   ///< Length of preamble (in fact its +1 long).
#define POSTAMBLE_LENGTH      (3)   ///< Length of postamble.
#define ACKWAITING_LENGTH     (30)  ///< Number of bytes to wait for an acknowledgement.
#define MAX_TX_RETRIES        (15)  ///< Maximum count of retries.

interrupt (UART0TX_VECTOR) radio_tx_isr (void){
  // decrease cdCounter  
  if(cdCounter>0) cdCounter--; 
  // Send dummy byte to provide byte timing. Dont forget this in every case, else interrupt will starve.  
  switch(txState){
    // IDLE and SENT are processed by radio_tx_heartbeat!!!
    case RTX_IDLE: 
      TXBUF0 = 0x00;
      return;
    // WAITING: Check for free channel.
    case RTX_WAITING:
      TXBUF0 = 0x00;
      if(cdCounter == 0){                       // Channel is free!
        //txCounter = random(BACKOFF_LENGTH) + 1; // Calculate random backoff..
        txCounter = (ADC12MEM5 & 0x001F)+1;       // better, random contains % call
        txState = RTX_BACKOFF;                  // ..and go to RTX_BACKOFF.
      }
      return;
    // BACKOFF: Decrement backoff and check if elapsed, if channel gets busy again go back.
    case RTX_BACKOFF:
      TXBUF0 = 0x00;
      if(cdCounter > 0){ // Channel is busy again!
        txState = RTX_WAITING;
        return;
      } 
      else {
        txCounter--;
        if(txCounter==0){          // Backoff time elapsed.
          { extern UINT16 _skipADC; _skipADC = 0xFFFF; }
          P5OUT |= 0x40;           // Switch Transceiver into TxOOK
          P5OUT &= 0x7F;           // Switch Transceiver into TxOOK
          P3OUT |= 0x10; // 1 on Transceiver TxD
          txCounter = TURNON_LENGTH;
          txState = RTX_TURNON; 
        }
        return;
      }
    // TURNON: Keep Trasnmitter sending a 1 until counter is elapsed.
    case RTX_TURNON:
      txCounter--;
      TXBUF0 = 0xAA;
      if(txCounter==0){ 
        P3SEL = 0xF0;  // Switch UART Tx Module onto Transceiver
        P3OUT &= ~0x10; // 0 on Transceiver TxD
        txCounter = PREAMBLE_LENGTH;
        txState = RTX_PREAMBLE;
      }
      return;
    // PREAMBLE: Send PREAMBLE_LENGTH 0xAAs.    
    case RTX_PREAMBLE:
      TXBUF0 = 0xAA;
      txCounter--;
      if(txCounter==0){ // Preamble done, calc length and start the packet.
        // length to transmit is packet data length + header length (defined by PACKET_DATA)
        txLength = RADIOTX_RB_INT(rtxrb, PACKET_LENGTH) + PACKET_DATA + 2;
        txPos = 0;
        txState = RTX_FF;                
      }
      return;
    case RTX_FF:  TXBUF0 = STARTBYTE0; txState = RTX_01;       return;
    case RTX_01:  TXBUF0 = STARTBYTE1; txState = RTX_7F;       return;
    case RTX_7F:  TXBUF0 = STARTBYTE2;
                  if(RADIOTX_RB_I(rtxrb, PACKET_TYPE)==SYNCH_PACKET) txState = RTX_S_SENDING1;
                  else                                               txState = RTX_SENDING1; 
                  return; 
    // SENDING1: Encode byte and send the first half.      
    case RTX_SENDING1:
      txEncodeBuf = MAN_ENCODE(RADIOTX_RB_I(rtxrb, txPos));
      TXBUF0 = txEncodeBuf >> 8;
      txState = RTX_SENDING2;
      return;
    // SENDING2: Send the second half. If all bytes sent go to postamble.  
    case RTX_SENDING2:
      TXBUF0 = txEncodeBuf;
      txPos++;
      if(txPos==txLength){
        txCounter = POSTAMBLE_LENGTH;
        txState = RTX_POSTAMBLE;
      }
      else txState = RTX_SENDING1;
      return;
    // POSTAMBLE: Send POSTAMBLE_LENGTH 0x55s. If done continue to SENT
    // for unacknowledged packets (type<0x80) and ACKWAITING for acknowledged packets. 
    case RTX_POSTAMBLE:
      TXBUF0 = 0x00;
      txCounter--;
      if(txCounter==0){
        P5OUT |= 0xC0;      // Switch Transceiver into Rx Mode again
        P3SEL = 0xE0;       // Remove UART Tx Module from Transceiver  
        { extern UINT16 _skipADC; _skipADC = 40; }
      
        if( RADIOTX_RB_INT(rtxrb, PACKET_TO) != 0xFFFF){          
          txCounter = ACKWAITING_LENGTH; 
          txState = RTX_ACKWAITING;
        }
        else {
          txState = RTX_SENT;
          WAKEUPLPM1(radio_tx_isr, MF_RADIO_TX);
        }
      }
      return;
    // ACKWAITING: If counter elapses start a new retry or go to NOTSENT
    // if max retries are reached. ::radio_rx_heartbeat gets us out of this
    // if the correct ack is received.      
    case RTX_ACKWAITING:
      TXBUF = 0x00;
      txCounter--;
      if(txCounter==0){
        txRetries++;
        if(txRetries == MAX_TX_RETRIES){
          txState = RTX_NOTSENT;
          WAKEUPLPM1(radio_tx_isr, MF_RADIO_TX);
        }
        else {
          txCounter = TURNON_LENGTH;
          txState = RTX_WAITING;                 
        }      
      }
      return;
    // Final state for successfull sending (unacknowledged packet or acknowledged packet
    // with ack received). ::radio_tx_heartbeat continues here.
    case RTX_SENT:
      TXBUF0 = 0x00;
      return;
    // Final state for unsucessfull sending (acknowledged packet without ack after max retries).
    // ::radio_tx_heartbeat continues here.
    case RTX_NOTSENT:
      TXBUF0 = 0x00;
      return;
    // Below is the state machine for sending the acks which is mainly a copy of the
    // above states. No Backoff is done here, the data is sent from the ackBuffer
    // and this is started by the radio_rx interrupt receiving a packet with an
    // type >= 0x80. This goes automatically back to RTX_IDLE if done.
    case RTX_A_TURNON:  // TODO
      TXBUF0 = 0xAA;
      { extern UINT16 _skipADC; _skipADC = 0xFFFF; }
      P5OUT |= 0x40;    // Switch Transceiver into TxOOK
      P5OUT &= 0x7F;    // Switch Transceiver into TxOOK
      P3SEL  = 0xF0;     // Switch UART Tx Module onto Transceiver
      txCounter = PREAMBLE_LENGTH+1;
      txState = RTX_A_PREAMBLE;    
      return;
    case RTX_A_PREAMBLE:
      TXBUF0 = 0xAA;
      txCounter--;
      if(txCounter==0){
        // length to transmit is always PACKET_DATA
        txLength = PACKET_DATA;
        txPos = 0;
        txState = RTX_A_FF;                
      }
      return;
    case RTX_A_FF:  TXBUF0 = STARTBYTE0; txState = RTX_A_01;       return;
    case RTX_A_01:  TXBUF0 = STARTBYTE1; txState = RTX_A_7F;       return;
    case RTX_A_7F:  TXBUF0 = STARTBYTE2; txState = RTX_A_SENDING1; return; 
    case RTX_A_SENDING1:
      txEncodeBuf = MAN_ENCODE(ackBuffer[txPos]);
      TXBUF0 = txEncodeBuf >> 8;
      txState = RTX_A_SENDING2;
      return;
    case RTX_A_SENDING2:
      TXBUF0 = txEncodeBuf;
      txPos++;
      if(txPos==txLength){
        txCounter = POSTAMBLE_LENGTH;
        txState = RTX_A_POSTAMBLE;
      }
      else txState = RTX_A_SENDING1;
      return;
    case RTX_A_POSTAMBLE:
      TXBUF0 = 0x00;
      txCounter--;
      if(txCounter==0){
        P5OUT |= 0xC0;      // Switch Transceiver into Rx Mode again
        P3SEL = 0xE0;       // Remove UART Tx Module from Transceiver  
        { extern UINT16 _skipADC; _skipADC = 40; }
        txState = RTX_IDLE;
        WAKEUPLPM1(radio_tx_isr, MF_RADIO_TX);
      }
      return;
    ////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////  
    // SENDING1: Encode byte and send the first half.      
    case RTX_S_SENDING1:
      if(txPos==0){ // Read time when 01 byte was sent ( UART is has a byte buffer)
        extern volatile time_t currentTime;
        synch.synchTime = currentTime; 
        // A bit too long for irq, but must be done here: Calculate checksum for synch. (Other way would be to omit crc for synchs)
        synch.checksum = System_crcCalc(synch.checksum, (UINT8*)&synch, sizeof(time_t));
      }
      if(txPos<PACKET_DATA) txEncodeBuf = MAN_ENCODE(RADIOTX_RB_I(rtxrb, txPos));
      else                  txEncodeBuf = MAN_ENCODE( ((UINT8*)&synch)[txPos-PACKET_DATA] );
      //sendRS232('S');
      TXBUF0 = txEncodeBuf >> 8;
      txState = RTX_S_SENDING2;
      return;
    // SENDING2: Send the second half. If all bytes sent go to postamble.  
    case RTX_S_SENDING2:
      TXBUF0 = txEncodeBuf;
      txPos++;
      if(txPos==txLength){
        txCounter = POSTAMBLE_LENGTH;
        txState = RTX_POSTAMBLE;
      }
      else txState = RTX_S_SENDING1;
      return;      
      default:
      Comm_print("! undefined txState: %b\r\n", txState); // should never occur
      rxClear();
      break;
  }
}
//@}

/// \name Enabling & Disabling 
//@{

/** \brief Init function for radio.
 **
 ** Inits all vars, sets receiving mode and transmit power and then 
 ** enables the interrupt for receivnig.                                      */ 
bool Net_init(void) {
#if defined(DCO_2458KHZ)
  //////////////////////////////////////// Funk auf 19200 Bit/s
  UCTL0 = CHAR;                         // 8-bit character
  UTCTL0 = SSEL1;                       // UCLK = SMCLK
  UBR00 = 0x80; UBR10 = 0x00; UMCTL0 = 0x00; // 2,457MHz/19200 = 128 -> 0x80
  ////////////////////////////////////////                  ///< Target DCO = DELTA*(4096) = 2457600 */
#elif defined(DCO_4506KHZ)
  ////////////////////////////////////////
 UCTL0 = CHAR;                         // 8-bit character
 UTCTL0 = SSEL1;                       // UCLK = SMCLK
 UBR00=0xEA; UBR10=0x00; UMCTL0=0x5B; /* uart0 4505600Hz 19197bps */
 ////////////////////////////////////////
#else
#error No DCO frequency in funk.c::initRadio.
#endif
  Net_setTxPower(Configuration.transmitPower, 0);
  ME1 |= UTXE0 + URXE0;                 // Enable USART0 TXD/RXD, doesnt bring any power savings if switched off
  return True;
}

/** \brief Enables radio. */
bool Net_on(){
  rxNumTablePos = 0;
  cdCounter = 0;
  rxClear();  
  txClear();  
  setFunkRx();
  IFG1 |= UTXIFG0;           // SET TFIFG: ensure interrupt will start
  IE1  |= (URXIE0 + UTXIE0); // SET TXIE and RXIE: and enable it!
  return True;
}

/** \brief Disables radio. */
bool Net_off(){
  IE1 &= ~(URXIE0 + UTXIE0); // Disable Rx&Tx Interrupt
  setFunkOff();
  return True;
}

//@}

/// \name Synchronization
//@{
bool doSynch(){
  time_t* t1 = (time_t*) rxPacket.data;
  time_t* t2 = (time_t*) &rxTime;
  UINT8 sign;
  UINT32 diffs;
  UINT16 diffms;          
  Comm_log(HIGH, "Synch: %z,%i rx: %z,%i    ", t1, t1->millis, t2, t2->millis);
  // Calculate difference
  if(t1->secs < t2->secs || (t1->secs == t2->secs && t1->millis < t2->millis)){ // t1 < t2
    sign = 1; // -
    diffs = t2->secs - t1->secs;
    // 6,2 8,4 -2,2      6,2 8,2 -2,0        6,2 8,1 -1,9
    if(t1->millis <= t2->millis){ diffms = t2->millis - t1->millis; }
    else { diffms = 1024 - (t1->millis - t2->millis); diffs--; }
  }
  else { // t1 > t2
    sign = 0;
    diffs = t1->secs - t2->secs;
    // 8,4 6,2 +2,2     8,2 6,2 +2,0    6,2 6,2 +0,0            8,1 6,2 +1,9
    if(t2->millis <= t1->millis){ diffms = t1->millis - t2->millis; }
    else { diffms = 1024 - (t2->millis - t1->millis); diffs--; }
  }
  Comm_log(HIGH, "%c %l,%i\r\n", (sign==0 ? '+' : '-'), diffs, diffms);
  // Adjust currentTime
  //if(diffs > 1){ // accept syn only if diff is greater than 1 sec
    extern volatile time_t currentTime;
    if(sign==0){
      dint(); _NOP();
      currentTime.secs += diffs;
      currentTime.millis += diffms;
      if(currentTime.millis>1024){ currentTime.millis -= 1024; currentTime.secs++; }
      eint();
    }
    else {
      dint(); _NOP();
      currentTime.secs -= diffs;
      currentTime.millis -= diffms;
      if(currentTime.millis>1024){ currentTime.millis += 1024; currentTime.secs--; }
      eint();
    }
    Comm_log(HIGH, "Synch: Synched!\r\n");          
    return True;
  //}
}
//@}
