/*
 Copyright 2007, 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.

 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, 2007
 */
 
 /**
 * @file		ScatterWeb.Net.c
 * @brief		Network API / CC1020 MAC layer
 * @ingroup		cc1020
 * @version		$Rev: 507 $
 *
 * @todo		Move CC1020 MAC layer to libCC1020 leaving ScatterWeb.Net.c
 *				as the Network API.
 *
 * $Id: ScatterWeb.Net.c 507 2008-02-05 12:56:03Z baar@PCPOOL.MI.FU-BERLIN.DE $
 */

#include <stdint.h>
#include <string.h>
#include "ScatterWeb.System.h"
#include "ScatterWeb.CC1020.h"

/**
 * @brief Enable for synchronization debugging
 * @internal
 */
//#define NETSYN_DEBUG(...)	printf(__VA_ARGS__)
#define NETSYN_DEBUG(...)

// 256 Ringbuffer macro implemenation.
#define RB(name)   \
    volatile struct {     \
      uint16_t guard1;      \
      uint8_t buffer[256];  \
      uint8_t head, tail;   \
      uint16_t guard2;      \
    } name

// Gets the first byte of the buffer and remove it.
#define RB_GET(b, x) do { x = (b).buffer[(b).head]; (b).head++; } while(0)

// Get current length.
#define RB_LENGTH(b) ((uint8_t)( (b).tail - (b).head ))

// Appends another bytes to the buffer.
#define RB_PUT(b, x) do { (b).buffer[(b).tail] = x; (b).tail++; } while(0)

// Initializes the ringbuffer.
#define RB_INIT(b) do { (b).head = 0; (b).tail = 0; (b).guard1 = 0xABCD; (b).guard2 = 0xABCD; } while(0)

#define PREAMBLE_SIZE	9		///< preamble and sync word sent before each packet
#define TAILsize		2		///< tail bytes sent after packet + crc
// TX
RB(txPackets);

// Interrupt byte radio ringbuffer.
volatile uint8_t numPackets; // Current number of packets in radio tx buffer.
volatile uint8_t txNum; // Current tx Number.
volatile uint16_t txBufPos; // Current position in txBuf.
unsigned char txLockStatus; // Saves last value from SetupCC1020TX.
volatile uint8_t txBuf[140]; // Buffer for bulding packets with preamble.
volatile uint8_t ackBuf[PREAMBLE_SIZE + sizeof(netheader_t) + 2]; //   ""          ""      ACKS
volatile uint16_t txBufLength; // How much to send in TxBuffer

static netheader_t sentPacket;
static struct netpacket_callback_args txCallbackArgs = {&sentPacket,0,0};
/// Callback what to do after a packet has been sent.
static fp_net_cb txCallback = NULL; 

volatile uint16_t txBackoff; // Var for storing backoff in states TX_WAITING, TX_BACKOFF.
volatile uint16_t ackTimeout; // Timeout how long to wait for an ack in RADIO_STROKE ticks, depends on baudrate and should come with the config.
volatile enum net_txmode txMode;
volatile bool radioEnabled = false;
uint16_t v0, v1, v2;
uint16_t tim1, tim2, tim3, tim4;

#define TIMESTART()   do { tim1=TAR; tim2=TBR; }while(0)
#define TIMESTOP()    do { tim3=TAR; tim4=TBR; print("TIM %i %i\n", tim3-tim1, tim4-tim2); }while(0)
#define MAX_REPEATED  (5)         // How often a packet get repeated.
#define RADIO_STROKE  (9011)      // ~ 2 ms @ 4505600 Hz.
// RX
__attribute__((packed)) typedef struct {
	uint8_t state; ///< state of this buffer (FF=Empty 00=Rec 01=FULL)
	uint8_t reserved; ///< space to align following data to 16 bit
	union {
		uint8_t raw[140]; ///< raw data as received
		struct {
			netheader_t nwh;
			uint8_t payload[140 - sizeof(netheader_t)];
		} decoded; ///< decoded packet data
	} buffer;
} rxbuffer_t; ///< receive buffer
rxbuffer_t* rxBuf; ///< currently selected receive buffer
rxbuffer_t rxBuf1; // Buffer for received packets.
rxbuffer_t rxBuf2; // Buffer for received packets.
uint8_t rxBufPos; // Position in this buffer.
volatile time_t rxTime; // When Synch byte was discovered.
volatile uint8_t rssi; // Received Signal Strength Indicator read from chipcon during receive.
volatile uint8_t afc;  // Drift

// Forward declarations
void TX_STOPPED();
void TX_IDLE();
void TX_DONE();
void rxsearching();
void rxsynched_left0();
void rxsynched_right0();
void rxsynched_left();
void rxsynched_right();
void rxsynched0();
void rxreceived();
void acktxdma();
void TX_BACKOFF();
void TX_WAITING();
void TX_WAITACK();

/// Add received packet to history table
static bool _Net_HistoryAdd(netheader_t* nwh);
/// Check if a packet has been received
static bool _Net_HistoryFind(netheader_t* nwh);
/// Incoming packet handler
static void _Net_packetHandler(rxbuffer_t* fullBuf);

#define DISABLE_RX_IRQ() do { IE1 &= ~(URXIE0); } while(0)
#define ENABLE_RX_IRQ() do { IFG1 &= ~URXIFG0; IE1 |= URXIE0; } while(0)
#define DIRQSTART()  
#define DIRQEND()    

#define CC_LOCK (P2IN&0x04)
#define LNA_POWER_ON  do { P2OUT &= ~0x10; } while(0) // Enable power for LNA (P24, low-active)
#define LNA_POWER_OFF do { P2OUT |=  0x10; } while(0) // Disable power for LNA (P24, low-active)
#define ADR2INT(x)  (*((uint16_t*)& x))

#define MAX_RXNUM_TABLE     (20)    // Length of rxNumTable.
struct rxNumTableEntry {
	netaddr_t id;
	uint8_t num;
};

static struct rxNumTableEntry rxNumTable[MAX_RXNUM_TABLE];
static const struct rxNumTableEntry* rxNumTableLast = &rxNumTable[MAX_RXNUM_TABLE];
uint8_t rxNumTablePos = 0; // Next pos to enter a new value into ::rxNumTable.

union {
	struct {
		uint8_t b2;
		uint8_t b1;
		uint8_t b4;
		uint8_t b3;
	};
	struct {
		uint16_t i1;
		uint16_t i2;
	};
} shiftbuf;

#define RESTART_TX_TIMER()  do {    \
	PANICASSERT( WDS_RADIOPANIC, GET_ISR(TIMERB0_VECTOR)!=TX_STOPPED && GET_ISR(TIMERB0_VECTOR)!=TX_DONE ); \
    TBCCR0 = TBR + RADIO_STROKE;  \
    TBCCTL0 &= ~CCIFG;            \
    TBCCTL0 |= CCIE;              \
    } while(0)

#define STOP_TX_TIMER()  do { \
	PANICASSERT( WDS_RADIOPANIC, GET_ISR(TIMERB0_VECTOR)!=TX_WAITING ); \
    TBCCTL0 &= ~(CCIFG+CCIE); \
  }while(0)

void _Net_logSent(char symbol, netheader_t* nh)
{
#if (DEBUGLOG_LEVEL & DEBUG_RADIO)
	if( DEBUGLOG_LEVEL_TESTHWRANGE( DEBUG_RADIO, MEDIUM, HIGH ) ) {
		printf("#[net S%c] N:%u F:%#.4x T:%#.4x P:%#.2x Fl:%#.2x L:%u\r\n",
				symbol, nh->_num, nh->from, nh->to,
				nh->type, nh->flags, nh->size - sizeof(netheader_t)
		);
	}
#endif
}

void _Net_logReceived(char symbol, netheader_t* rxHeader, uint16_t checkR,
		uint16_t checkC)
{
#if (DEBUGLOG_LEVEL & DEBUG_RADIO)
	// packet debug output
	if( DEBUGLOG_LEVEL_TESTHWRANGE(DEBUG_RADIO, MEDIUM, HIGH) ) {
		// show faulty packets only on high level
		if( DEBUGLOG_LEVEL_TEST(HIGH) && (symbol == '-') )
		return;
		printf("#[net R%c] N:%u F:%#.4x T:%#.4x P:%#.2x Fl:%#.2x L:%u R:%u Cr:%#x Cc:%#x\r\n",
				symbol, rxHeader->_num,
				rxHeader->from, rxHeader->to, rxHeader->type, rxHeader->flags,
				rxHeader->size - sizeof(netheader_t), rssi, checkR, checkC);
	}
#endif
}

/* ------------ FROM HERE CC1020 MAC LAYER (move) -------------- */

void _Net_logMacDump(char symbol, uint8_t* data, uint16_t length)
{
#if (DEBUGLOG_LEVEL & DEBUG_RADIO) && ((DEBUGLOG_LEVEL & DEBUGLOG_LEVEL_MASK) >= VERBOSE)
	// verbose logging -> dump complete packet
	if( DEBUGLOG_LEVEL_TESTHW(DEBUG_RADIO, VERBOSE) && length) {
		printf("#(%cX) ", symbol);
		// hex dump might be too long for Comm_print buffer
		String_writeHex(putchar, data, length);
		printf("\r\n");
	}
#endif
}

interrupt(NOVECTOR) TX_STOPPED() {
	System_panic(WDS_RADIOPANIC, "TX_STOPPED"); // ADD
}

interrupt(NOVECTOR) TX_IDLE() {
	// If IDLE and something is in buf prepare packet in txBuf and
	// go to txState=TX_WAITING to wait for a free channel.
	if(numPackets > 0) {
		int pos; // position in txbuf
		uint8_t x;
		uint16_t ix;
		uint16_t length;
		uint16_t checksum = INIT_FCS;

		/*
		 Prepare txBuf with packet to send:
		 - 8 byte preamble and sync word
		 - 8 byte header (in ringbuf txPackets)
		 - x byte data   ( "    "       "     )
		 - 2 byte checksm( "    "       "     )
		 dont forget to take 2 bytes callback off ringbuf
		 */

		// Start with 6 bytes preamble + 3 bytes sync word
		((uint16_t*)txBuf)[0] = 0xAAAA;
		((uint16_t*)txBuf)[1] = 0xAAAA;
		((uint16_t*)txBuf)[2] = 0xAAAA;
		((uint16_t*)txBuf)[3] = 0x91D3;
		// third byte saves masking operations in rxsearching
		txBuf[8] = 0x00;
		pos = PREAMBLE_SIZE;

		// take preamble and header from queue, calc checksum
		length = PREAMBLE_SIZE + sizeof(netheader_t);
		for(; pos < length; pos++ ) {
			RB_GET(txPackets, x);
			txBuf[pos] = x;
			checksum = System_ccittFcs(checksum, x);
		}

		// Copy header
		memcpy((void*)&sentPacket, (void*)&txBuf[PREAMBLE_SIZE], sizeof(netheader_t));

		// fetch payload
		length = PREAMBLE_SIZE + sentPacket.size;
		for(; pos < length; pos++ ) {
			RB_GET(txPackets, x);
			txBuf[pos] = x;
			checksum = System_ccittFcs(checksum, x);
		}

		// append CRC and modify (!) size    
		if( (sentPacket.flags & NET_TLFLAG_NOCRC) == 0 ) {
			txBuf[pos++] = checksum >> 8;
			txBuf[pos++] = checksum;
			txBuf[PREAMBLE_SIZE] += 2;
		}

		// Add 2 Bytes FF as puffer.
		txBuf[pos] = 0xFA; pos++;
		txBuf[pos] = 0xFA; pos++;
		txBufLength = pos;

		RB_GET(txPackets, x); ix = (x << 8);
		RB_GET(txPackets, x); ix |= x;
		txCallback = (fp_net_cb)ix;

		numPackets--;
		txCallbackArgs.timesRepeated = 0;
		// go into waiting
		if(txMode & NET_TXMODE_BACKOFF) {
			SET_ISR(TIMERB0_VECTOR, TX_WAITING);
		} else {
			// txMode==TXMODE_NOBACKOFF
			SET_ISR(TIMERB0_VECTOR, TX_BACKOFF);
			txBackoff = 1; // No backoff
			// Dont clear if here so state TX_BACKOFF will run
			// directly after this interrupt
		}
		// Go directly to TX_WAITING or TX_BACKOFF
		TBCCTL0 &= ~(CCIE + CCIFG);
		TBCCTL0 |= CCIE + CCIFG;
	} else {
		// If no packet has to be sent stop here and do nothing.
		// TimerTx interrupt will be reenabled if a packet is queued by
		// radio_queue_packet.
		STOP_TX_TIMER();
		SET_ISR(TIMERB0_VECTOR, TX_STOPPED);
		//RadioOFF
		if(txMode & NET_TXMODE_LP) {
			CC1020_setupPD();
			LNA_POWER_OFF;
		}
	}
}

// Channel busy, waiting for free channel.
// case TX_WAITING: look for free chanel (rxState = RX_SEARCHING) and go to TX_BACKOFF
interrupt(NOVECTOR) TX_WAITING() {
	if(GET_ISR(USART0RX_VECTOR)==rxsearching && (CC_LOCK)) {
		txBackoff = (((TBR + TAR) & 0x001E) + 4);
		SET_ISR(TIMERB0_VECTOR, TX_BACKOFF);
	}
	RESTART_TX_TIMER();
}

// Channel clear, waiting for random backoff time elapsed.
// case TX_BACKOFF: wait until backoff time elapsed, then start sending
interrupt(NOVECTOR) TX_BACKOFF() {
	// If we are currently sending an ack (no matter how this
	// could happen?!)  go back to TX_WAITING.
	if(GET_ISR(DACDMA_VECTOR)==acktxdma) {
		SET_ISR(TIMERB0_VECTOR, TX_WAITING);
		RESTART_TX_TIMER();
	}
	else if((GET_ISR(USART0RX_VECTOR)!=rxsearching || (!(CC_LOCK))) && (txMode & NET_TXMODE_BACKOFF)) {
		SET_ISR(TIMERB0_VECTOR, TX_WAITING);
		RESTART_TX_TIMER();
	}
	else if(txBackoff==0) { // Start sending.
		// REENTRANT!, case needs longer than 7ms
		STOP_TX_TIMER();
		DISABLE_RX_IRQ();
		_NOP();
		eint();
		v0 = TAR;
		// isSending = 1;
		// Switch transceiver to Tx
		LNA_POWER_OFF;
		CC1020_wakeupTX(TX_CURRENT);
		txLockStatus = CC1020_setupTX(TX_CURRENT);

		// low level debug
		_Net_logMacDump('T', (uint8_t*)&txBuf[PREAMBLE_SIZE], txBufLength - PREAMBLE_SIZE - 2);

		// Configure ports for Tx, Assumed UART is in SPI mode for chipcon receive
		P3SEL &= ~0x02; // Ensure Rx line is off
		P3SEL |= 0x0C; // select Tx line and clk
		U0CTL |= SWRST; // NEW
		IFG1 &= ~UTXIFG0; // Reset IFG.
		DMACTL0 = DMA0TSEL_4; // Configure DMA Channel 0 for UART0 TXIFG.
		DMACTL1 = 0x0000; // No DMAONFETCH, ROUNDROBIN, ENNMI.
		// Single transfer Mode, dstadr unchanged, srcadr incremented, byte access
		// Important to use DMALEVEL when using USART TX interrupts so first edge 
		// doesn't get lost (hangs every 50. - 100. time)!
		DMA0CTL = DMADT_0 + DMADSTINCR_0 + DMASRCINCR_3 + DMASBDB + DMALEVEL + DMAIE;
		DMA0SA = (uint16_t) txBuf;
		DMA0DA = (uint16_t) &TXBUF0;
		DMA0SZ = txBufLength;
		U0CTL &= ~SWRST; // NEW, needed??
		DMA0CTL |= DMAEN; // Start tx, when finished ends in radiotxdma
	}
	// Backoff time still running, decrease counter and restart timer.
	else {
		txBackoff--;
		RESTART_TX_TIMER();
	}
}

interrupt(NOVECTOR) radiotxdma() {
	netheader_t* txHeader = (netheader_t*)&txBuf[PREAMBLE_SIZE];

	DMA0CTL &= ~(DMAIFG + DMAIE);
	eint(); // eint part lasts ~7ms
	/////////////////// Reconfigure Port& for receive mode.
	P3SEL &= ~0x04; // Ensure Tx line is off
	P3SEL |= 0x0A; // select rx line and clk
	/////////////////// Switch transceiver to Rx
	CC1020_wakeupRX(RX_CURRENT);
	CC1020_setupRX(RX_CURRENT);
	LNA_POWER_ON;
	dint();

#if (DEBUGLOG_LEVEL & DEBUG_RADIO) && ((DEBUGLOG_LEVEL & DEBUGLOG_LEVEL_MASK) >= HIGH)
	if( DEBUGLOG_LEVEL_TESTHWRANGE(DEBUG_RADIO, HIGH, HIGH) ) {
		// notify about sending from queue
		printf("#[net S+] N:%u R:%u\r\n", txHeader->_num, txCallbackArgs.timesRepeated);
	}
#endif

	if( (txHeader->to != NETADDR_BROADCAST) &&
			((txHeader->flags & NET_TLFLAG_ACKREQ) == NET_TLFLAG_ACKREQ) )
	{
		// Not-Broadcasts need ack
		if( Debuglog_level & DEBUG_RADIO ) {
			// Logging output takes time
			ackTimeout = 20 * (Debuglog_level & DEBUGLOG_LEVEL_MASK);
		} else {
			ackTimeout = 16; // 12; // ATTENTION!!
		}

		SET_ISR(TIMERB0_VECTOR, TX_WAITACK);
		RESTART_TX_TIMER();
	} else {
		// Broadcasts dont need an ack, signal "sent"
		SET_ISR(TIMERB0_VECTOR, TX_DONE); // set net state
		txBuf[0] = 0x00; // mark for no packet in txbuf 
		txCallbackArgs.success = true;
		WAKEUPLPM1(MF_RADIO_TX); // call packet handler
	}
	// isSending = 0; // no more sending
	ENABLE_RX_IRQ();
}

interrupt(NOVECTOR) TX_DONE() {
	// nothing happens here, just to mark this state
	
	/*
	This state is selected to Timer B0 after a transmission has been done.
	It's currently unclear why. Is it just to silently catch a timer interrupt
	or is it an error?
	*/
	LOG_HARDWARE(DEBUG_RADIO, "#(TX) TIMER B0 in TX_DONE", txCallbackArgs.timesRepeated);
	// System_panic(WDS_RADIOPANIC, "TX_DONE");
}

// Currently waiting for an ACK.
// case TX_WAITACK: repeat packet if needed, go to idle if ack received.
interrupt(NOVECTOR) TX_WAITACK() {
	ackTimeout--;
	if(ackTimeout==0) {
		if(txCallbackArgs.timesRepeated > MAX_REPEATED) { // Unreachable destination
			SET_ISR(TIMERB0_VECTOR, TX_DONE);
			txBuf[0] = 0x00; // mark for no packet in txbuf
			txCallbackArgs.success = false;
			WAKEUPLPM1(MF_RADIO_TX);
		} else {
			txCallbackArgs.timesRepeated++;
			SET_ISR(TIMERB0_VECTOR, TX_WAITING);
			RESTART_TX_TIMER();
		}
	} else {
		RESTART_TX_TIMER();
	}
}

// Receiving side: Low level
//
// Preamble is 0xAA, Sync is 0xD391,
// Packet format:  AAAAAAAAAA D391 00 Length[1] To[2] From[2] Type[1] Num[1] 
//  
// 0  AA D3 91 00 | FF 00 |
// 1  55 69 C8 80 | 7F 80 |
// 2  AA B4 E4 40 | 3F C0 |
// 3  55 5A 72 20 | 1F E0 |
// 4  AA AD 39 10 | 0F F0 |
// 5  55 56 9C 88 | 07 F8 |
// 6  AA AB 4E 44 | 03 FC |
// 7  55 55 A7 22 | 01 FE |
// 8  AA AA D3 91 | 00 FF |
//
interrupt(NOVECTOR) acktxdma() {
	DMA0CTL &= ~(DMAIFG + DMAIE);
	eint(); // eint part lasts ~7ms
	/////////////////// Reconfigure Port& for receive mode.
	P3SEL &= ~0x04; // Ensure Tx line is off
	P3SEL |= 0x0A; // select rx line and clk
	/////////////////// Switch transceiver to Rx
	CC1020_wakeupRX(RX_CURRENT);
	CC1020_setupRX(RX_CURRENT);
	LNA_POWER_ON;
	dint();

	// ack has been sent completely

	// isSending = 0; // no more sending
	SET_ISR(DACDMA_VECTOR, radiotxdma);
	ENABLE_RX_IRQ();
}

volatile char rxoffset;
interrupt(NOVECTOR) rxsearching() {
	shiftbuf.b1 = shiftbuf.b2;
	shiftbuf.b2 = shiftbuf.b3;
	shiftbuf.b3 = shiftbuf.b4;
	shiftbuf.b4 = RXBUF0;
		
	if(shiftbuf.i1==0xAAD3 && shiftbuf.i2==0x9100) {		// AA D3 91 00 | FF 00 |
		SET_ISR(USART0RX_VECTOR, rxsynched0);
		//rxBuf->state = 00;
		//rxBuf->buffer.raw[rxBufPos++] = shiftbuf.b4;
		rxoffset = 0;
	}
	else if(shiftbuf.i1==0x5569 && shiftbuf.i2==0xC880) {	// 55 69 C8 80 | 7F 80 |
		rxoffset = -1;
	}
	else if(shiftbuf.i1==0xAAB4 && shiftbuf.i2==0xE440) { // 2  AA B4 E4 40 | 3F C0 |
		rxoffset = -2;
	}
	else if(shiftbuf.i1==0x555A && shiftbuf.i2==0x7220) { // 3  55 5A 72 20 | 1F E0 |
		rxoffset = -3;
	}
	else if(shiftbuf.i1==0xAAAD && shiftbuf.i2==0x3910) { // 4  AA AD 39 10 | 0F F0 |
		rxoffset = -4;
	}
	else if(shiftbuf.i1==0x5556 && shiftbuf.i2==0x9C88) { // 5  55 56 9C 88 | 07 F8 |
		rxoffset = 3;
	}
	else if(shiftbuf.i1==0xAAAB && shiftbuf.i2==0x4E44) { // 6  AA AB 4E 44 | 03 FC |
		rxoffset = 2;
	}
	else if(shiftbuf.i1==0x5555 && shiftbuf.i2==0xA722) { // 7  55 55 A7 22 | 01 FE |
		rxoffset = 1;
	} else {
		return;
	}
	NETSYN_DEBUG("s %i:", rxoffset);
	
	if( rxoffset > 0 ) {
		SET_ISR(USART0RX_VECTOR, rxsynched_right0);
	} else if( rxoffset < 0 ) {
		rxoffset = -rxoffset;
		SET_ISR(USART0RX_VECTOR, rxsynched_left0);
	}
	
	rxTime = CURRENT_TIME;
	rssi = CC1020_readRegister(CC1020_RSS);
	afc = CC1020_readRegister(CC1020_AFC);
}

void rxsynched_checkdone() {
	uint8_t size;
	if( rxBufPos < sizeof(netheader_t) ) {
		size = sizeof(netheader_t);
	} else {
		size = rxBuf->buffer.decoded.nwh.size;
	}
	if( rxBufPos >= size )
		SET_ISR(USART0RX_VECTOR, rxreceived);
}

// this is to ignore the last sync-word byte
interrupt(NOVECTOR) rxsynched_left0() {
	shiftbuf.b3 = shiftbuf.b4;
	shiftbuf.b4 = RXBUF0;
	shiftbuf.i1 = shiftbuf.i2 << rxoffset;
	SET_ISR(USART0RX_VECTOR, rxsynched_left);
}

// this is to ignore the last sync-word byte
interrupt(NOVECTOR) rxsynched_right0() {
	shiftbuf.b3 = shiftbuf.b4;
	shiftbuf.b4 = RXBUF0;
	shiftbuf.i1 = shiftbuf.i2 >> rxoffset;
	SET_ISR(USART0RX_VECTOR, rxsynched_right);
}

// receiving: no offset
interrupt(NOVECTOR) rxsynched0() {
	rxBuf->buffer.raw[rxBufPos++] = RXBUF0;
	NETSYN_DEBUG("%.2x ", rxBuf->buffer.raw[rxBufPos]);
	rxsynched_checkdone();
}

// receiving: offset with left-shift
interrupt(NOVECTOR) rxsynched_left() {
	shiftbuf.b3 = shiftbuf.b4;
	shiftbuf.b4 = RXBUF0;
	shiftbuf.i1 = shiftbuf.i2 << rxoffset;
	rxBuf->buffer.raw[rxBufPos++] = shiftbuf.b1;
	NETSYN_DEBUG("%.2x ", shiftbuf.b1);
	rxsynched_checkdone();
}

// receiving: offset with right-shift
interrupt(NOVECTOR) rxsynched_right() {
	shiftbuf.b3 = shiftbuf.b4;
	shiftbuf.b4 = RXBUF0;
	shiftbuf.i1 = shiftbuf.i2 >> rxoffset;
	rxBuf->buffer.raw[rxBufPos++] = shiftbuf.b2;
	NETSYN_DEBUG("%.2x ", shiftbuf.b2);
	rxsynched_checkdone();
}

void _Net_sendACK(netheader_t* rxHeader)
{
	uint16_t pos;
	netheader_t* ackHeader;

	// Sending an ACK, first thing is to check if DMA controller
	// is already in use, if it is the TX state machine i currently sending
	// a packet. Should not happen when a recent packet was received, but
	// for safety:
	if (DMA0CTL & DMAEN)
		return;
	// Next thing is to set the ISR to acktxdma, this will block 
	// the tx machine from starting another dma transfer.
	SET_ISR(DACDMA_VECTOR, acktxdma);
	// Start with 6 bytes preamble + 2 bytes sync word.
	((uint16_t*)ackBuf)[0] = 0xAAAA;
	((uint16_t*)ackBuf)[1] = 0xAAAA;
	((uint16_t*)ackBuf)[2] = 0xAAAA;
	((uint16_t*)ackBuf)[3] = 0x91D3;
	txBuf[8] = 0x00;
	pos = PREAMBLE_SIZE;

	// Queue header
	ackHeader = (netheader_t*)(ackBuf + pos);
	ackHeader->size = sizeof(netheader_t);
	ackHeader->flags = NET_TLFLAG_NOCRC;
	ackHeader->to = rxHeader->from;
	ackHeader->from = Configuration.id;
	ackHeader->type = (uint8_t)ACK_PACKET;
	ackHeader->_num = rxHeader->_num;
	ackHeader->txpower = Configuration.radio_txpower;
	pos += sizeof(netheader_t);
	// Add 2 Bytes FF as puffer.
	ackBuf[pos] = 0xFA;
	pos++;
	ackBuf[pos] = 0xFA;
	pos++;

#if (DEBUGLOG_LEVEL & DEBUG_RADIO)
	// low level debug
	_Net_logMacDump('T', (uint8_t*)&ackBuf[PREAMBLE_SIZE], pos - PREAMBLE_SIZE - 2);
	_Net_logSent('A', (netheader_t*)&ackBuf[PREAMBLE_SIZE]);
#endif

	// isSending = 1;
	DISABLE_RX_IRQ();
	_NOP();
	eint();
	// Switch transceiver to Tx
	LNA_POWER_OFF;
	CC1020_wakeupTX(TX_CURRENT);
	txLockStatus = CC1020_setupTX(TX_CURRENT) ;
	// Configure ports for Tx, Assumed UART is in SPI mode for chipcon receive
	P3SEL &= ~0x02; // Ensure Rx line is off
	P3SEL |= 0x0C; // select Tx line and clk
	U0CTL |= SWRST; // NEW    TODO Uart reinit?
	IFG1 &= ~UTXIFG0; // Reset IFG.
	DMACTL0 = DMA0TSEL_4; // Configure DMA Channel 0 for UART0 TXIFG.
	DMACTL1 = 0x0000; // No DMAONFETCH, ROUNDROBIN, ENNMI.
	// Single transfer Mode, dstadr unchanged, srcadr incremented, byte access
	// Important to use DMALEVEL when using USART TX interrupts so first edge 
	// doesn't get lost (hangs every 50. - 100. time)!
	DMA0CTL = DMADT_0 + DMADSTINCR_0 + DMASRCINCR_3 + DMASBDB + DMALEVEL + DMAIE;
	DMA0SA = (uint16_t) ackBuf;
	DMA0DA = (uint16_t) &TXBUF0;
	DMA0SZ = pos;
	// TODO ????
	// dint(); _NOP();
	U0CTL &= ~SWRST; // NEW
	DMA0CTL |= DMAEN;
}

interrupt(NOVECTOR) rxreceived() {
	uint16_t checkR = 0; // received checksum
	uint16_t checkC = 0; // calculated checksum
	bool invoke_handler; // invoke system packet handler or not
	netheader_t* rxHeader // network layer packet header
	= &rxBuf->buffer.decoded.nwh;
	netheader_t* txHeader;
	int i;

	NETSYN_DEBUG("rx\r\n");
	
	// low level debug
	_Net_logMacDump('R', rxBuf->buffer.raw, rxHeader->size);

	// test checksum
	if( (rxHeader->flags & NET_TLFLAG_NOCRC) == 0 ) {
		uint8_t* x;

		// read received crc
		checkR = rxBuf->buffer.raw[rxHeader->size - 2] << 8;
		checkR |= rxBuf->buffer.raw[rxHeader->size - 1];

		// checksum done, "remove" from packet
		rxHeader->size -= 2;

		// calculate Checksum
		for(i = 0, checkC = INIT_FCS, x = rxBuf->buffer.raw; i < rxHeader->size; i++, x++)
		checkC = System_ccittFcs(checkC, *x);
	}

	// packet debug output
#if (DEBUGLOG_LEVEL & DEBUG_RADIO)
	if( DEBUGLOG_LEVEL_TESTHWRANGE(DEBUG_RADIO, MEDIUM, HIGH) ) {
		char c;
		if( rxHeader->type == ACK_PACKET )
		c = 'A';
		else if( checkR == checkC )
		c = '+';
		else
		c = '-';
		_Net_logReceived( c, rxHeader, checkR, checkC );
	}
#endif

	if( checkR == checkC ) {

		/* ---- network layer packet handler  ---- */
		switch(rxHeader->type) {
			case ACK_PACKET:
			// Waiting for this ack?!? This part perhaps not irq safe ?! TODO
			txHeader = (netheader_t*)&txBuf[PREAMBLE_SIZE];
			if( rxHeader->from == txHeader->to ) {
				if( (GET_ISR(TIMERB0_VECTOR) == TX_WAITACK) &&
						(rxHeader->_num == sentPacket._num) )
				{
					STOP_TX_TIMER();
					SET_ISR(TIMERB0_VECTOR, TX_DONE);
					txBuf[0] = 0x00; // mark for no packet in txbuf
					txCallbackArgs.success = true;
					WAKEUPLPM1(MF_RADIO_TX);
					if( DEBUGLOG_LEVEL_TEST(LOW) )
					LOG_HARDWARE(DEBUG_RADIO, "#(RX) %i retries to ACK", txCallbackArgs.timesRepeated);
				} else {
#if (DEBUGLOG_LEVEL & DEBUG_RADIO)
					LOG_HARDWARE(DEBUG_RADIO, "#(RX) ACK in wrong state", NULL);
#endif
				}
			}
			// do not pass ack packets to the application
			invoke_handler = false;
			break;

			default:
			// send ack if requested
			if( (rxHeader->to == Configuration.id) &&
					((rxHeader->flags & NET_TLFLAG_ACKREQ) == NET_TLFLAG_ACKREQ)
			)
			{
				_Net_sendACK(rxHeader);
			}

			invoke_handler = true;
			break;
		}

	} else {
		// checksum mismatch
		LOG_HARDWARE(DEBUG_RADIO, "#(RX) CRC error", NULL);
		invoke_handler = false;
	}

	/* ---- clean up buffers  ---- */
	dint(); _NOP();

	if( invoke_handler ) {
		// this buffer is ready
		rxBuf->state = 0x01;
		// need to switch to other buffer to keep the current packet for the handler
		// TODO check and warn if buffer is not already empty
		rxBuf = (rxBuf == &rxBuf1) ? &rxBuf2 : &rxBuf1;
		rxBufPos = 0;
		// pass packet to application
		WAKEUPLPM1(MF_RADIO_RX);
	} else {
		rxBufPos = 0;
	}

	/* ---- set ready to receive  ---- */
	SET_ISR(USART0RX_VECTOR, rxsearching);
	IE1 |= URXIE0; // Enable interrupt.
	IFG1 &= ~URXIFG0;
	DIRQEND();
}

// name UART0 Switching functions
void _Net_resetU0()
{
	//Reset SEL for P3[1-3] (CC DIO, DIO, DCLK) and P3[4-5] (Camera Rx+Tx)
	P3SEL &= ~0x3E;
	IFG1 &= ~(UTXIE0 + URXIE0);
	ME1 &= ~(UTXE0 + URXE0); // Disable Uart0 Tx + Rx
	UCTL0 = SWRST; // U0 into reset state.
}

/*
 *   brief Initializes UART0 for receiving from Chipcon
 *
 *   Data (for transmit+receive) must be stable at the rising edge of the clock.
 *   This means:
 *     Transmit : Set bit on falling edge of clock.
 *     Receive  : Read bit on rising edge of clock.
 *
 *   CKPH: Normal UCLK clocking scheme.
 *   CKPL: 1 The inactive level is high; data is output with the falling edge of
 *         UCLK; input data is latched with the rising edge of UCLK.
 */
void _Net_initU0()
{
	_Net_resetU0();
	UCTL0 |= CHAR + SYNC; // 8-bit character, SPI, Slave mode
	UTCTL0 = CKPL + STC; // CKPH works also, but not CKPH+CKPL or none of them!!
	URCTL0 = 0x00;
	UBR00 = 0x00; // No baudrate divider 
	UBR10 = 0x00; // settings for a spi
	UMCTL0 = 0x00; // slave.
	ME1 |= USPIE0; // Enable USART0 TXD/RXD, disabling does not yield any powersavings
	P3SEL |= 0x0A; // Select rx line and clk
	UCTL0 &= ~SWRST; // Last thing is clear reset bit
	IE1 |= URXIE0; // Enable interrupt.
}

bool Net_rxHandler()
{
	//////////////////// Look into both buffers for something to handle
	do {
		if (rxBuf1.state == 0x01) {
			_Net_packetHandler(&rxBuf1);
			rxBuf1.state = 0xFF; // Set buffer empty again.
		} else if (rxBuf2.state == 0x01) {
			_Net_packetHandler(&rxBuf2);
			rxBuf2.state = 0xFF;
		} else {
			runModule &= ~MF_RADIO_RX;
			return true;
		}
	} while (1);
}

bool Net_txHandler()
{
	if(txCallback != NULL) {
		txCallback(&txCallbackArgs);
	}
	SET_ISR(TIMERB0_VECTOR, TX_IDLE);
	runModule &= ~MF_RADIO_TX;
	RESTART_TX_TIMER();
	return true;
}

/* ---------------- CC1020/MIXED NETWORK API (restructure) --------------------- */

bool Net_send(netpacket_send_args_t* args)
{
	int i;
	uint8_t x;
	uint16_t size;

	if (!radioEnabled)
		return false;

	size = sizeof(netheader_t);
	for( i = 0; i < NETPACKET_SEND_BUFFER_COUNT; i++ )
		size += args->payload[i].size;
	
	if ((254 - RB_LENGTH(txPackets)) > size) {
		// we have space in buf, so queue it
		args->netheader.size = size;

		// Header
		args->netheader._num = ++txNum;
		args->netheader.txpower = Configuration.radio_txpower;
		for (i = 0; i < sizeof(netheader_t); i++) {
			RB_PUT(txPackets, ((uint8_t*)&args->netheader)[i]);
		}

		// Payload
		for( x = 0; x < NETPACKET_SEND_BUFFER_COUNT; x++ ) {
			for (i = 0; i < args->payload[x].size; i++) {
				RB_PUT(txPackets, ((uint8_t*)args->payload[x].buffer)[i]);
			}
		}		

		// Queue function callback 
		x = ((uint16_t)args->fpcallback) >> 8;
		RB_PUT(txPackets, x);
		// upper 8 bits of callback
		x = ((uint16_t)args->fpcallback) & 0x00FF;
		RB_PUT(txPackets, x);
		// lower 8 bits of callback

		numPackets++;

		// Debug output
		_Net_logSent('Q', &args->netheader);

		// If first packet in buf immediatly start Interrupt for sending
		if(GET_ISR(TIMERB0_VECTOR)==TX_STOPPED) {
			//RadioON
			if(txMode & NET_TXMODE_LP) {
				//uint16_t t1, t2, t3, t4;
				//t1 = TAR;
				//t3 = TBR;
				CC1020_wakeupRX(RX_CURRENT);
				CC1020_calibrate();
				CC1020_wakeupTX(TX_CURRENT);
				CC1020_calibrate();
				CC1020_wakeupRX(RX_CURRENT);
				CC1020_setupRX(RX_CURRENT);
				LNA_POWER_ON;
				//t2 = TAR;
				//t4 = TBR;
			}
			dint();
			_NOP();
			TBCCTL0 &= ~(CCIE + CCIFG);
			SET_ISR(TIMERB0_VECTOR, TX_IDLE);
			TBCCTL0 |= CCIE + CCIFG;
		}
		eint();
		return true;
	} else {
		// Not enough space in ringbuf!
		// not queued source->destination P:protocol F:flags N:number L:length
		_Net_logSent('N', &args->netheader);

		/// TODO  2005-12-7 ONLY QUICK HACK TO REBOOT DEVICE WHEN TX STATE MACHINE IS HANGING IN TX_WAITING:
		/// TODO  2007-08-16 WHY?
		/*
		if(GET_ISR(TIMERB0_VECTOR) == TX_WAITING) {
			char* msg = "#(MACTX) DEADLOCK TX_WAITING";
			LOG_HARDWARE(DEBUG_RADIO, msg, NULL);
			System_panic(WDS_RADIOPANIC, msg);
		}
		*/
		return false;
	}
}

bool Net_init()
{
	CC1020_init();
	return true;
}

bool Net_on(enum net_txmode mode)
{
	if (radioEnabled)
		return true;
	LNA_POWER_ON;
	// Transmitting side vars
	txMode = mode;
	numPackets = 0;
	txBackoff = 1; //Init
	// Receiving side vars
	rxBuf1.state = 0xFF;
	rxBuf2.state = 0xFF;
	rxBuf = &rxBuf1;
	rxBufPos = 0;
	// Interrupt routings
	SET_ISR(USART0RX_VECTOR, rxsearching);
	STOP_TX_TIMER();
	SET_ISR(TIMERB0_VECTOR, TX_STOPPED);
	SET_ISR(DACDMA_VECTOR, radiotxdma);
	// Ring buffers  
	RB_INIT(txPackets);
	// Uart
	_Net_initU0();
	CC1020_setupRX(RX_CURRENT);
	if (txMode & NET_TXMODE_LP) {
		CC1020_setupPD();
		LNA_POWER_OFF;
	}
	radioEnabled = true;
	return true;
}

bool Net_changeMode(enum net_txmode mode)
{
	txMode = mode;
	if (!(mode & NET_TXMODE_LP)) {
		CC1020_wakeupRX(RX_CURRENT);
		CC1020_calibrate();
		CC1020_wakeupTX(TX_CURRENT);
		CC1020_calibrate();
		CC1020_wakeupRX(RX_CURRENT);
		CC1020_setupRX(RX_CURRENT) ;
		LNA_POWER_ON;
	}
	return true;
}

bool Net_off()
{
	if(radioEnabled) {
		dint();
		_NOP(); 
	   	TBCCTL0 &= ~(CCIFG+CCIE);
		IE1 &= ~(URXIE0);
		SET_ISR(TIMERB0_VECTOR, TX_STOPPED);
		eint();
	   	CC1020_setupPD();
		LNA_POWER_OFF;
		radioEnabled = false;
	}
	return true;
}

/* ---------------- NEUTRAL NETWORK API (keep) --------------------- */

void Net_sendArgsInit(netpacket_send_args_t* args)
{
	memset(args, 0, sizeof(netpacket_send_args_t)); // clear struct
	args->netheader.from = Configuration.id; // this node's address
	args->netheader.flags = NET_TLFLAG_ACKREQ; // network layer config flags
}

enum netaddr_compareresult
Net_compareAddress(const netaddr_t (*const adr), const bool directConnection)
{
	if (*adr == Configuration.id)
		return ADRCOMP_THIS;

	if (directConnection) {
		if( (*adr == 0) )
			return ADRCOMP_THIS;
	} else if( *adr == 0 )
		return ADRCOMP_ERROR;
	if( *adr == NETADDR_BROADCAST )
		return ADRCOMP_BROADCAST;
	else
		return ADRCOMP_OTHER;
}

bool Net_sendPing(netaddr_t node) {
	netpacket_send_args_t npsargs;

	// fill structure with default values
	Net_sendArgsInit( &npsargs );

	// fill network header custom values
	npsargs.netheader.to = node;					// receiver's address
	npsargs.netheader.type = PING_REQUEST_PACKET;	// application protocol
	npsargs.netheader.flags = 0;					// network layer config flags (CRC

	// queue packet for sending
	return Net_send( &npsargs );
}

bool Net_sendPong(netaddr_t node) {
	netpacket_send_args_t npsargs;

	// fill structure with default values
	Net_sendArgsInit( &npsargs );

	// fill network header custom values
	npsargs.netheader.to = node;					// receiver's address
	npsargs.netheader.type = PING_REPLY_PACKET;		// application protocol
	npsargs.netheader.flags = 0;					// network layer config flags (CRC

	// queue packet for sending
	return Net_send( &npsargs );
}

static void _Net_packetHandler(rxbuffer_t* fullBuf)
{
	netheader_t* nwh = &fullBuf->buffer.decoded.nwh;
	/*
	 second part of network layer radio handler. this part is invoked by the
	 "scheduler".
	 */

	// Packet already received?
	if( _Net_HistoryFind(nwh) ) {
		// We already received that, so do nothing.
		LOG_HARDWARE(DEBUG_RADIO, "(RX) dup F:%u N:%u", nwh->from, nwh->_num);
		return;
	}

	// First time received: enter current num into history.
	_Net_HistoryAdd(nwh);

	// Invoke application layer packet handler
	if (callbacks[C_RADIO] != NULL) {
		enum netaddr_compareresult cpaResult;
		cpaResult = Net_compareAddress((netaddr_t*)&nwh->to, false);

		if (cpaResult == ADRCOMP_ERROR ) {
			return;
		} else {
			struct netpacket_handler_args args =
				{ nwh,
					fullBuf->buffer.decoded.payload,
					nwh->size - sizeof(netheader_t),
					rssi, cpaResult };

			((fp_net_handler)callbacks[C_RADIO])(&args);
		}
	}
}

static bool _Net_HistoryAdd(netheader_t* nwh) {
	register struct rxNumTableEntry* e;
	
	for(e = rxNumTable; e < rxNumTableLast; e++) {
		if(e->id == nwh->from) {
			// update existing entry
			e->num = nwh->_num;
			return true;
		}
	}
	// If no entry found from rxPacket.from create a new one..
	PANICASSERT(WDS_RADIOPANIC, rxNumTablePos < MAX_RXNUM_TABLE);
	e = &rxNumTable[rxNumTablePos];
	rxNumTablePos++;
	if (rxNumTablePos == MAX_RXNUM_TABLE)
		rxNumTablePos = 0;
	e->id = nwh->from;
	e->num = nwh->_num;
	return false;
}

static bool _Net_HistoryFind(netheader_t* nwh) {
	register struct rxNumTableEntry* e;
	
	for(e = rxNumTable; e < rxNumTableLast; e++) {
		if( (e->id == nwh->from) && (e->num == nwh->_num) )
			return true;
	}
	return false;
}
