OFDMReferenceDesign/Applications/Characterization: csmaMac.c

/*! \file csmaMac.c
 \brief Carrier-Sensing Random Access MAC.
 
 @version 13
 @author Chris Hunter and Patrick Murphy
 

 The csmaMac is a modified ALOHA MAC that
 serves as an example for novel MAC
 development. Nodes transmit whenever
 they have information to transmit, and only
 move on to the next packet once the original
 transmit is acknowledged (ACKed). If no ACK
 is received, a collision is inferred and the
 packet is re-transmitted.
 
 
 By default, the MAC also implements carrier-
 sensing multiple-access with collision-
 avoidance (CSMA-CA). This functionality is
 built into hardware peripherals in the project
 so very little software state is affected.
 
 In its current state, the project acts as
 a true ethernet MAC-level wireless bridge.
 Any ethernet activity that appears on one
 WARP will be sent to another via the custom
 wireless link.
 
 Also, the current versions supports hardware-triggered
 ACK transmissions. This reduces the turn-around time
 as well as reduces the jitter on the start time of the ACK
 relative to the start time of the preceeding DATA.

 */
#include "warpmac.h"
#include "warpphy.h"
#include "csmaMac.h"
#include "xparameters.h"
#include "string.h"
#include "errno.h"
#include "stdlib.h"
#include "stdio.h"
#include "ascii_characters.h"
#include "warp_userioboard_util.h"
#include "radio_controller_ext.h"
#include "warpnet_node2.h"
#include "xtime_l.h"


#define htons(A) ((((Xuint16)(A) & 0xff00) >> 8) | (((Xuint16)(A) & 0x00ff) << 8))

Macframe templatePkt;

unsigned int autoResp_matchCond;
unsigned int autoResp_action;
unsigned int autoResp_delay;
unsigned char pktBuf_tx_ACK;
unsigned char pktBuf_tx_DATA;
unsigned char pktBuf_rx;

//Global variables to for user I/O board outputs
unsigned char userIOBoard_LEDs;
unsigned char charsToPrint[16];
unsigned int pktCount_good, pktCount_bad;

//Arrays to track pkt sequence numbers for each partner node
unsigned char rxSequences[16];
unsigned char txSequences[16];

unsigned char maximumReSend;

///MAC address table with agreed upon mapping between dipswitches and physical addresses
typedef struct {
    unsigned char addr[6];
} nodeAddr;


///Byte array containing physical MAC address of this node
unsigned char myAddr[6];
///Index to the routing table that identifies this node
unsigned short int myID;

///Full rate modulation selection; QPSK by default
unsigned int pktFullRate;

//Payload code rate selection; must be CODE_RATE_NONE for uncoded PHY
unsigned int pktCodeRate;

///Buffer for holding a packet-to-xmit across multiple retransmissions
Macframe txBuffer;
///Buffer to hold received packet
Macframe rxBuffer;

///Current antenna selection
unsigned char currentAnt;
///Current 802.11 channel
unsigned char chan;

//Define handy macros for CSMA MAC packet types
///Data packet with payload meant for Ethernet transmission
#define DATAPACKET 1
///Acknowledgement packet meant for halting retransmissions
#define ACKPACKET 0

// Aid for Debugging Purposes, if 1 print from Teraterm
#define PrintDebug 0



//warpnodeChanEst chanEst_AA[WARPNET_NUMNODES-1];


//Arrays to track pkt sequence numbers for each partner node
unsigned char rxSequences[16];
unsigned char txSequences[16];

//Variables used by other commands
unsigned char sendStats;
char sendcallbackpacket;


///Global Structs
warpnetEthernetPktHeader txEthPktHeader;
warpnodeID myNodeID;
warpnodeCommand myNodeCmd;

///Global Stat Struct
warpnodeStats myStats[WARPNET_NUMNODES];

///Place holder for TRANSMIT or RECEIVE state
unsigned char Mode;

///Measures time for beginning and end of transmission
XTime start_timer, end_timer;

unsigned int pktCount_good, pktCount_bad;

///Parameters used for warpmac_startPacketGeneration
unsigned int Time;
unsigned short packetlength;

///@brief Send ackCommand Packet over the Ethernet
///
///This function is when certain node commands request acks. This packages
///the ack package as a specific type of command struct, with the current rxSeqNum
///It then sends this ackCommand Packet over the Ethernet
///@param rxSeqNum, sequence number of packet ethernet header
void sendAck(char rxSeqNum)
{
    //xil_printf("sent ack packet w sequence number: %d\r\n",rxSeqNum);
    
    //Initialize Payload Address
    int pktBuf_payloadAddr;
    
    //Fill in a node command reply pkt
    myNodeCmd.structType = STRUCTID_NODECOMMAND;
    myNodeCmd.nodeID = myID;
    myNodeCmd.cmdID = NODECMD_NODEACK;
    myNodeCmd.cmdParam = rxSeqNum;
                            
    //Fill in the ethernet packet's header info
    txEthPktHeader.ethType = WARPNET_ETHTYPE_NODE2SVR;
    memset(&(txEthPktHeader.srcAddr), (unsigned char)myID, 6); //Generic source address; ignored throughout
    memset(&(txEthPktHeader.dstAddr), (unsigned char)0xFF, 6); //Broadcast destination address
                        
    //Fill in the ethernet packet's header info
    txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnodeCommand);
    txEthPktHeader.numStructs = 1;
    txEthPktHeader.seqNum = rxSeqNum;
    
    //Copy over data for transmission over wire
    pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_PKTBUFFINDEX);
    memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
    memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), &(myNodeCmd), sizeof(warpnodeCommand));
    
    //Send the packet over the wire 
    warpmac_sendRawEthernetPacket((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
    return;
}


///@brief Send the current Stat Packet over the Ethernet
///
///This function is when certain node commands request stat packets to be send over the ethernet.
///The stat package ethernet header increases the seqNum sent over wire
///@param rxSeqNum, sequence number of packet ethernet header
void sendStatsPacket(char rxSeqNum)
{
    //xil_printf("\r\nSent Stats PACKET \r\n");
    //Send a new stats packet over the wire

    //Initialize Payload Address
    unsigned int pktBuf_payloadAddr;
    
    //De-assert the flag (it will be re-asserted in the main while loop later)
// sendStats = 0;
    
    //Fill in the ethernet packet's header info
    txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + (WARPNET_NUMNODES*sizeof(warpnodeStats));
    txEthPktHeader.numStructs = WARPNET_NUMNODES;
    txEthPktHeader.seqNum = rxSeqNum;
    
//  xil_printf("\r\nSent pktLength: %d \r\n", txEthPktHeader.pktLength);
//  xil_printf("\r\nSent numStructs: %d \r\n", txEthPktHeader.numStructs);
//  xil_printf("\r\nSent seqNum: %d \r\n", txEthPktHeader.seqNum);

    
    //Copy the ethernet header and stats payload to a memroy buffer
    pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_PKTBUFFINDEX);
    memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
    
    //Copy each stats struct (one per partner)
    memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), &myStats, WARPNET_NUMNODES*sizeof(warpnodeStats));
    
//  for(int i=0; i<(sizeof(warpnetEthernetPktHeader)+sizeof(warpnodeStats)); i++)
//  {
//      xil_printf("[%d] 0x%x\r\n", i, *((char *)pktBuf_payloadAddr+i));
//  }
    //xil_printf("Size of ethernetpktheader %d\r\n",  sizeof(warpnetEthernetPktHeader));
    
    //Send the packet over the wire
    warpmac_sendRawEthernetPacket((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
    
    sendStats = 0;
    return;
}

///@brief Update node parameters with warpnodeControl Struct values
///
///This function process the warpnodeControl struct by taking the specific
///paramters and modifying the current board configuration
///@param packet Pointer to warpnodeControl
void processControlStruct(warpnodeControl* ctrlStruct)
{
    //xil_printf("Processing Control Struct\r\n");
    //Interpret the control struct's modulation rate value
    switch(ctrlStruct->modOrder)
    {
            /*** Non-autorate version ***/
        case 0x1: //BPSK
            pktFullRate = HDR_FULLRATE_BPSK;
            warp_userio_lcd_printline("Mod order: BPSK ", 16, 8, 1);
            break;
        case 0x2: //QPSK
            pktFullRate = HDR_FULLRATE_QPSK;
            warp_userio_lcd_printline("Mod order: QPSK ", 16, 8, 1);
            break;
        case 0x4: //16-QAM
            pktFullRate = HDR_FULLRATE_QAM_16;
            warp_userio_lcd_printline("Mod order: 16QAM", 16, 8, 1);
            break;
        case 0x6: //64-QAM
            pktFullRate = HDR_FULLRATE_QAM_64;
            warp_userio_lcd_printline("Mod order: 64QAM", 16, 8, 1);
            break;
            /*******************/
        default:
            //xil_printf("Invalid Mod order - Recieved: %x\r\n", ctrlStruct->modOrder);
            //Invalid value, so ignore it and leave txFulrate as-is
            break;
    }
    
    //Set the transmit power for both radios
    warpphy_setTxPower(ctrlStruct->txPower);
    //xil_printf("Power changed to %d\r\n", ctrlStruct->txPower);
    
    //Use the circle of LEDs as a Tx power indicator
    warp_userioboard_set_leds((1<<((8+(ctrlStruct->txPower))/8))-1);
    unsigned char charsToPrint[16];
    snprintf(charsToPrint, 16, "Tx Pwr: 0x%2x    ", (ctrlStruct->txPower));
    
    //xil_printf("Tx Power changed to: %x\r\n", ctrlStruct->txPower);
    
    warp_userio_lcd_printline(charsToPrint, 16, 6, 1);
    
    //Set the center frequency of both radios
    // Values 1-14 are 2.4GHz channels
    // Values 15-37 are 5GHz channels
    // Any other value is invalid and ignored
    if( ((ctrlStruct->channel) > 0) && ((ctrlStruct->channel) <= 37) )
    {
        if((ctrlStruct->channel) <= 14)
            warpphy_setChannel(GHZ_2, ctrlStruct->channel);
        else
            warpphy_setChannel(GHZ_5, (ctrlStruct->channel) - 14);
    }
    
    snprintf(charsToPrint, 16, "Channel: %2d     ", (ctrlStruct->channel));
    warp_userio_lcd_printline(charsToPrint, 16, 7, 1);
    
    return;
}

///@brief Callback for the depression of the left push button
///
///This function is empty by default
void left(){
}

///@brief Callback for the depression of the right push button
///
///This button switched between different fullrate modulation orders
void right(){
    switch(pktFullRate){
        case HDR_FULLRATE_BPSK:
            pktFullRate = HDR_FULLRATE_QPSK;
            xil_printf("QPSK\r\n");
            break;
        case HDR_FULLRATE_QPSK:
            pktFullRate = HDR_FULLRATE_QAM_16;
            xil_printf("16-QAM\r\n");
            break;
        case HDR_FULLRATE_QAM_16:
            pktFullRate = HDR_FULLRATE_QAM_64;
            xil_printf("64-QAM\r\n");
            break;
        case HDR_FULLRATE_QAM_64:
            pktFullRate = HDR_FULLRATE_BPSK;
            xil_printf("BPSK\r\n");
            break;
    }
}

///@brief Callback for the depression of the up push button
///
///This button increments the 2.4GHz channel being used; only valid channels (in [1,14]) will be used
void up(){
    unsigned int newFreq;
    
    chan = (chan > 13) ? 14 : chan+1;
    newFreq = warpphy_setChannel(GHZ_2, chan);
    warpmac_leftHex(chan);
    
    xil_printf("New Frequency %d\r\n", newFreq);
}

///@brief Callback for the depression of the middle push button
///
///This button decrements the 2.4GHz channel being used; only valid channels (in [1,14]) will be used
void middle(){
    unsigned int newFreq;
    
    chan = (chan < 2) ? 1 : chan-1;
    newFreq = warpphy_setChannel(GHZ_2, chan);
    warpmac_leftHex(chan);
    
    xil_printf("New Frequency %d\r\n", newFreq);
}


///@brief Updates the LCD
///
///Updates the LCD. This is only for the User I/O daughtercard. If you do not have this card,
///this function will do nothing. This operation takes some time, so it can only be called in a few places so as to
///not adversely affect the performance of the MAC.
void updateLCD()
{

    warp_userioboard_set_leds(userIOBoard_LEDs);
    
    snprintf(charsToPrint, 16, "Good: %09d ", pktCount_good);
    warp_userio_lcd_printline(charsToPrint, 16, 4, 1);
    
    snprintf(charsToPrint, 16, "Bad:  %09d ", (pktCount_bad)>>1);//bad pkts get counted twice
    warp_userio_lcd_printline(charsToPrint, 16, 5, 1);
    
    return;
}

///@brief Callback for the reception of UART bytes
///@param uartByte ASCII byte received from UART
///
///Provides the user with the bytes that was received over the serial port. This is useful for configuring
///PHY and MAC parameters in real time on a board.
void uartRecv_callback(unsigned char uartByte)
{
    if(uartByte != 0x0)
    {
        xil_printf("(%c)\t", uartByte);
        
        switch(uartByte)
        {
            case ASCII_1:
                pktFullRate = HDR_FULLRATE_BPSK;
                xil_printf("Tx Full Rate = BPSK\r\n");
                break;
                
            case ASCII_2:
                pktFullRate = HDR_FULLRATE_QPSK;
                xil_printf("Tx Full Rate = QPSK\r\n");
                break;
                
            case ASCII_4:
                pktFullRate = HDR_FULLRATE_QAM_16;
                xil_printf("Tx Full Rate = 16-QAM\r\n");
                break;
                
            case ASCII_6:
                pktFullRate = HDR_FULLRATE_QAM_64;
                xil_printf("Tx Full Rate = 64-QAM\r\n");
                break;
            case ASCII_D:
                warpphy_setPktDetMinDurPlus(1);
                break;
            case ASCII_d:
                warpphy_setPktDetMinDurMinus(1);
                break;
            case ASCII_P:
                warpphy_setPktDetPlus(100);
                break;
            case ASCII_p:
                warpphy_setPktDetMinus(100);
                break;
            case ASCII_F:
                if(chan<14) chan++;
                warpphy_setChannel(GHZ_2, chan);
                xil_printf("Current channel: %d\r\n",chan);
                break;
            case ASCII_f:
                if(chan>1) chan--;
                warpphy_setChannel(GHZ_2, chan);
                xil_printf("Current channel: %d\r\n",chan);
                break;
            case ASCII_C:
                warpphy_setCSMAPlus(100);
                break;
            case ASCII_c:
                warpphy_setCSMAMinus(100);
                break;
            case ASCII_A:
                warpphy_setAntennaMode(TX_ANTMODE_SISO_ANTB, RX_ANTMODE_SISO_ANTB);
                xil_printf("Radio 3 Selected\r\n");
                break;
            case ASCII_a:
                warpphy_setAntennaMode(TX_ANTMODE_SISO_ANTA, RX_ANTMODE_SISO_ANTA);
                xil_printf("Radio 2 Selected\r\n");
                break;
            case ASCII_G:
                warpphy_setTargetPlus();
                break;
            case ASCII_g:
                warpphy_setTargetMinus();
                break;

            case ASCII_W:
                autoResp_delay++;
                autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_ACK, 1, (PHY_AUTORESPONSE_REQ_MATCH0 | PHY_AUTORESPONSE_REQ_MATCH1), autoResp_delay, 0, 0, 1, 0, 1);
                mimo_ofdmTxRx_setAction0(autoResp_action);
                xil_printf("ACK delay: %d\r\n", autoResp_delay);
                break;
                
            case ASCII_w:
                autoResp_delay--;
                autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_ACK, 1, (PHY_AUTORESPONSE_REQ_MATCH0 | PHY_AUTORESPONSE_REQ_MATCH1), autoResp_delay, 0, 0, 1, 0, 1);
                mimo_ofdmTxRx_setAction0(autoResp_action);
                xil_printf("ACK delay: %d\r\n", autoResp_delay);
                break;
                
            default:
                xil_printf("Undefined command\r\n");
                break;
        }
    }
    
    return;
}
///@brief Callback for the expiration of timers
///
///This function is responsible for handling #TIMEOUT and #BACKOFF.
///The job responsibilities of this function are to:
///-increase the contention window upon the expiration of a #TIMEOUT
///-initiate a #BACKOFF timer upon the expiration of a #TIMEOUT
///-retransmit a packet upon the expiration of a #BACKOFF
///@param timerType #TIMEOUT or #BACKOFF
void timer_callback(unsigned char timerType){
    int status;
    switch(timerType){
        case TIMEOUT_TIMER:
            //warpmac_incrementLEDHigh();
            warpmac_setTimer(BACKOFF_TIMER);
            break;
        case BACKOFF_TIMER:
            //xil_printf("Remaining Retransmissions = %d\r\n",txBuffer.header.remainingReSend&0xF);
            if(txBuffer.header.remainingReSend&0xF){
                //Copy the header over to packet buffer 1
                warpmac_prepPhyForXmit(&txBuffer, pktBuf_tx_DATA);
                //warpmac_setDebugGPIO(0xF);
                //Send packet buffer 1
                warpmac_startPhyXmit(pktBuf_tx_DATA);
                //Wait for it to finish and enable the receiver
                warpmac_finishPhyXmit();
                //warpmac_setDebugGPIO(0);
                //Start a timeout timer
                //xil_printf("----SET TIMEOUT\r\n");
                warpmac_setTimer(TIMEOUT_TIMER);
                warpmac_decrementRemainingReSend(&txBuffer);
                //Copy the header over to packet buffer 1
                //warpmac_prepPhyForXmit(&txBuffer,1);
            }
            else{
            //warpmac_setDebugGPIO(0x0);
            warpmac_enableDataFromNetwork();
            }
            break;
    }
}

///@brief Callback for the reception of Ethernet packets
///
///This function is called by the ethernet MAC drivers
///and process the packet to perform specific operations
///depending on the packet struct
///@param length Length, in bytes, of received Ethernet frame
void mgmtFromNetworkLayer_callback(Xuint32 length, char* payload){
    
    #if PrintDebug >0
    xil_printf("\r\nReceived A mgmt PACKET with following paramters: \r\n");
    #endif
    
    //warpmac_disableDataFromNetwork();
    unsigned char rxSeqNum=0;
    unsigned char destNode;

    warpmac_resetCurrentCW();
    //warpmac_disableDataFromNetwork();

    //Flag that will allow some WARPnet packets to be forwarded wirelessly in future revisions

    //Bookkeeping variables
    int pktBuf_payloadAddr;
    unsigned char numRxStructs, thisStructType;
    unsigned int rxPktOffset;
    int i;

    //Re-interpret the Rx packet as a WARPnet Ethernet frame
    warpnetEthernetPktHeader* receivedPkt;
    
    //Pointers to each kind of possible received struct; used to re-interpret the packet below
    warpnodeID* idStruct;
    warpnodeControl* controlStruct;
    warpnodeCommand* cmdStruct;
    warpnodeTraffic* trafficStruct;
    
    receivedPkt = (warpnetEthernetPktHeader*) payload;

    //xil_printf("Ethernet type is: %x but should be %x\r\n", receivedPkt->ethType,WARPNET_ETHTYPE_SVR2NODE);
    
    //Ack packet from the other nodes
    if ((receivedPkt->ethType) == WARPNET_ETHTYPE_NODE2SVR)
    {
        //xil_printf("Ack packet size: %d\r\n", sizeof(payload));
        return;
    }
    

    //If this is a WARPnet Ethernet packet, process it immediately
    if((receivedPkt->ethType) == WARPNET_ETHTYPE_SVR2NODE)
    {
        rxPktOffset = sizeof(warpnetEthernetPktHeader);
        numRxStructs = receivedPkt->numStructs;
        rxSeqNum = receivedPkt->seqNum;
    //  xil_printf("Number of structs %d\r\n", numRxStructs);
    // xil_printf("Number of rxSeqNum %d and my ID is %d\r\n", rxSeqNum, myID);
    
    /*
    for(i=0; i<30; i++)
    {
        xil_printf("[%d] %x\r\n", i, payload[i]);
    }*/
    
        for(i=0; i<numRxStructs; i++)
        {
            thisStructType = *(unsigned char*)(payload+rxPktOffset);
            switch(thisStructType)
            {
                case STRUCTID_NODEID:
                //  xil_printf("Got a node ID request\r\n");
                    
                    //Initialize my nodeID struct (to respond to server inquiries)
                    myNodeID.structType = STRUCTID_NODEID;
                    myNodeID.nodeID = myID;
                    
                    //Fill in the ethernet packet's header info
                    txEthPktHeader.ethType = WARPNET_ETHTYPE_NODE2SVR;
                    memset(&(txEthPktHeader.srcAddr), (unsigned char)myID, 6); //Generic source address; ignored throughout
                    memset(&(txEthPktHeader.dstAddr), (unsigned char)0xFF, 6); //Broadcast destination address
                    
                    txEthPktHeader.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnodeID);
                    txEthPktHeader.numStructs = 1;
                    
                    pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_PKTBUFFINDEX);
                    memcpy((void *)pktBuf_payloadAddr, &(txEthPktHeader), sizeof(warpnetEthernetPktHeader));
                    memcpy((void *)pktBuf_payloadAddr+sizeof(warpnetEthernetPktHeader), &(myNodeID), sizeof(warpnodeID));
                    
                    warpmac_sendRawEthernetPacket((void *)pktBuf_payloadAddr, txEthPktHeader.pktLength);
                    
                    rxPktOffset += sizeof(warpnodeID);
                    break;
                    
                case STRUCTID_NODECONTROL:
                    controlStruct = (warpnodeControl*)(payload+rxPktOffset);
                    
                    #if PrintDebug >0
                    xil_printf("Got a node control pkt and the ID param is : %d \r\n", controlStruct->nodeID);  
                    xil_printf("My nodeID: %d\r\n", controlStruct->nodeID);
                    #endif
                        
                    if( (controlStruct->nodeID) == myID)
                    {
//                      xil_printf("Got a node control: structType: %d\r\n", controlStruct->structType);
//                      xil_printf("Got a node control: nodeID: %d\r\n", controlStruct->nodeID);
//                      xil_printf("Got a node control: modOrder: %d\r\n", controlStruct->modOrder);
//                      xil_printf("Got a node control: txPower: %d\r\n", controlStruct->txPower);
//                      xil_printf("Got a node control: coding: %d\r\n", controlStruct->coding);
//                      xil_printf("Got a node control: channel: %d\r\n", controlStruct->channel);
                        
//                      for(i=0; i<(sizeof(warpnodeControl)); i++)
//                      {
//                      xil_printf("[%d] 0x%x\r\n", i, *((char *)controlStruct+i));
//                      }   
//      
                        processControlStruct((warpnodeControl*)(payload+rxPktOffset));
                        
                        //xil_printf("Number of Control rxSeqNum %d and received %d\r\n", rxSeqNum, receivedPkt->seqNum);
                        
                        //Send Ack Packet
                        if(rxSeqNum > 0) sendAck(rxSeqNum);
                    }
                    
                    rxPktOffset += sizeof(warpnodeControl);
                    break;
                    
                case STRUCTID_NODECOMMAND:
                    cmdStruct = (warpnodeCommand*)(payload+rxPktOffset);
                //xil_printf("Got a node command pkt and ID param is %d\r\n", cmdStruct->nodeID);
        
                    switch(cmdStruct->cmdID)
                    {
                    case NODECMD_RESETSTATS:
                    
                        if( (cmdStruct->nodeID) == myID)
                        {   
                            //xil_printf("NODECMC_RESETSTATS \r\n");
                            //Reset the good/bad pkt counts and Rx/Tx byte counts
                            for(i=0; i<WARPNET_NUMNODES; i++)
                            {
                                myStats[i].goodPackets = 0;
                                myStats[i].partnerBadPackets = 0;
                                myStats[i].otherBadPackets = 0;
                                myStats[i].txBytes = 0;
                                myStats[i].rxBytes = 0;
                                myStats[i].time = 0;
                            }
                            
                            //Send Ack Packet
                            if(rxSeqNum > 0) sendAck(rxSeqNum);

                    }
                        break;
                        
                    case NODECMD_REBOOT:
                        //Restart the software from scratch
                        main();
                        break;
                        
                    case NODECMD_SENDPACKET:
                        sendcallbackpacket=1;
                        break;
                        
                    case NODECMD_SENDSTATS:
                        sendStats = 1;
                        break;
                        
                    case NODECMD_START:
                    
                        XTime_GetTime(&start_timer);
                        //xil_printf("NODECMC_Start mode is: %x \r\n", Mode);
                        
                        //warpmac_enableDummyPacketMode();
                        if (Mode==TRANSMIT){
                        
                        warpmac_startPacketGeneration(packetlength,Time); //second argument is interval in usec
    
                        }
                        
                        break;
                        
                    case NODECMD_STOP:
                
                        //xil_printf("NODECMC_STOP \r\n");
                        warpmac_stopPacketGeneration();
                        //warpmac_disableDummyPacketMode();
                    
                        XTime_GetTime(&end_timer);
                        long long xtime=0;
                        xtime=end_timer-start_timer;
                            
                        for(i=0; i<WARPNET_NUMNODES; i++)
                        {
                            myStats[i].time = xtime;
                                
                        }
                        
                        if((cmdStruct->nodeID) == myID)
                        {   
                            //Send Ack Packet
                            if(rxSeqNum > 0) sendAck(rxSeqNum);
                        }
                    
                        break;
                        
                    case NODECMC_REQUESTSTATS:
                    
                        if( (cmdStruct->nodeID) == myID)
                        {   
                            //xil_printf("NODECMC_REQUESTSTATS \r\n");  
                            sendStatsPacket(rxSeqNum);
                            
                            //Send Ack Packet
                            if(rxSeqNum > 0) sendAck(rxSeqNum);
                        }
                        break;
                        
                    default:
        
                        for(i=0; i<sizeof(warpnodeCommand); i++)
                        {
                            xil_printf("%x ", *(unsigned char*)(payload+rxPktOffset+i));
                        }
                    
                        break;
                    }
            
                    rxPktOffset += sizeof(warpnodeCommand);
                    break;
                    
                    
                case STRUCTID_NODETRAFFIC: //This is for a traffic struct
                    trafficStruct = (warpnodeTraffic*)(payload+rxPktOffset);
                    
                    //xil_printf("Got a node traffic pkt with ID param as : %d \r\n",trafficStruct->nodeID);
                    if((trafficStruct->nodeID) == myID)
                    {
                        //xil_printf("MY ID: %x \r\n", myID);

                        //Send Ack Packet
                        if(rxSeqNum > 0) sendAck(rxSeqNum);
                        
                        Time=trafficStruct->txInterval;
                        Mode=trafficStruct->trafficMode;
                        packetlength=trafficStruct->txPacketLen;
                
                    }
                    
                    rxPktOffset += sizeof(warpnodeTraffic);
                    
                    //xil_printf("The mode is : %x\r\n", Mode);
                    
                break;
                            
                default:
                    //Didn't understand the WARPnet packet; discard it
                    //xil_printf("Got unknown WARPnet pkt - type: %d\r\n", thisStructType);
                    break;
            }
        
        }
    }
    

     //xil_printf("Unknown packet size: %d\r\n", sizeof(payload));
    // warpmac_enableDataFromNetwork();
    return;
}

///@brief Callback for the reception of data packets
///
///This function is called by the MAC drivers
///when a data packet is received and an ack packet is available to send. 
///This function fills the Macframe transmit buffer with the packet and sends
///it over the OFDM link
///@param length Length, in bytes, of received Ethernet frame
void dataFromNetworkLayer_callback(Xuint32 length, char* payload){
    
    unsigned char rxSeqNum;
    unsigned char destNode;
    
    warpmac_resetCurrentCW();
//  warpmac_setDebugGPIO(0xF);
    warpmac_disableDataFromNetwork();
    txBuffer.header.length = length;
    txBuffer.header.pktType = DATAPACKET;
    
    //Set the modulation scheme for the packet's full-rate symbols
    txBuffer.header.fullRate = pktFullRate;
    
    //Set the code rate for the packet's payload
    txBuffer.header.codeRate = pktCodeRate;
    
    //For now, assume our destination is our opposite ID (all traffic is 0 <-> 1)
    destNode = (myID+1)%2;
    
    //Use the next sequence number for this node (top four bits) and resend count of 0 (bottom four bits)
    txSequences[destNode] = txSequences[destNode]+0x10;
    txBuffer.header.remainingReSend = (((txSequences[destNode])&0xF0)|((maximumReSend+1)&0x0F));
    
    //Copy in the packet's destination MAC address
    //Hard-coded as this node's partner node
    //memcpy(txBuffer.header.destAddr,nodeTable[destNode].addr,6);
    txBuffer.header.destAddr = ((destNode + 2)<<8 | destNode);
    
    //If the medium is idle
    if(warpmac_carrierSense()){
        
        //Copy the header over to packet buffer 1
        warpmac_prepPhyForXmit(&txBuffer, pktBuf_tx_DATA);
        //Send packet buffer 1
        
        warpmac_startPhyXmit(pktBuf_tx_DATA);
        //Wait for it to finish and enable the receiver
        warpmac_finishPhyXmit();
        //warpmac_setDebugGPIO(0x0);
        //Start a timeout timer
        warpmac_setTimer(TIMEOUT_TIMER);
        warpmac_decrementRemainingReSend(&txBuffer);
        //Copy the header over to packet buffer 1
        //warpmac_prepPhyForXmit(&txBuffer,1);
    }
    else{
        //Start a backoff timer
        warpmac_setTimer(BACKOFF_TIMER);
    }
    
    return;
}

///@brief Callback for the reception of bad wireless headers
///
///@param packet Pointer to received Macframe
void phyRx_badHeader_callback() {
    //warpmac_incrementLEDLow();
    
    userIOBoard_LEDs = userIOBoard_LEDs==0x80 ? 0x01 : (userIOBoard_LEDs << 1);

    pktCount_bad++;
    
    int i;
    //Update every stats struct, since we don't know where this bad pkt came from
    for(i=0; i<WARPNET_NUMNODES; i++)
    {
        myStats[i].otherBadPackets++;
    }
    
    return;
}

///@brief Callback for the reception of good wireless headers
///
///This function then polls the PHY to determine if the entire packet passes checksum
///thereby triggering the transmission of the ACK and the transmission of the received
///data over Ethernet.
///@param packet Pointer to received Macframe
void phyRx_goodHeader_callback(Macframe* packet){
    
    unsigned char state=INCOMPLETE;
    unsigned char srcNode;
    unsigned char shouldSend = 0;
    int i;
    
//  xil_printf("Destination: %d\r\n",packet->header.destAddr);
//  xil_printf("Type: %d\r\n",packet->header.pktType);
    
    
    //Exploit the fact that our MAC addresses use our ID as the last octet
    // This should be replaced by something more sophisticated at some point
    srcNode = packet->header.srcAddr;
    
    //Rotate the ring of LEDs clockwise
    userIOBoard_LEDs = userIOBoard_LEDs==0x1 ? 0x80 : (userIOBoard_LEDs >> 1);
    
    //Print the header (debug only- completely ruins actual performance)
#if 0
    for(i=0; i<24; i++)
    {
        xil_printf("%d.",*((unsigned char*)&(packet->header) + i));
    }
    xil_printf("\r\n");
#endif
    
    //If the packet is addressed to this node
    if(packet->header.destAddr == ( ((myID+2)<<8) | myID) ){
        
        
        switch(packet->header.pktType){
                //If received packet is data
            case DATAPACKET:
                //Acknowledge the reception of the data
                

                //At this point, we have pre-loaded the PHY transmitter with the ACK in hoping that
                //the packet passes checksum. Now we wait for the state of the received to packet
                //to move from INCOMPLETE to either GOODPACKET or BADPACKET

                //Copy the header over to packet buffer 2
                //warpmac_prepPhyForXmit(&ackPacket,2);

                if((packet->header.length)>1400){
                    updateLCD();
                }
                
                state = warpmac_finishPhyRecv();
                
                //warpmac_setDebugGPIO(0x0);
                if(state&GOOD){
                    
                    //Send packet buffer 2 containing the ACK
                    
                    //warpmac_startPhyXmit(2);
                    warpmac_incrementLEDHigh();
                    
                    //Update the right-hex display with the current resend count for the received pkt
                    warpmac_leftHex(0xF & (packet->header.remainingReSend));
                    
                    //Check if the last received seq number for this partner node matches this received pkt
                    // If not, record the new number and allow the pkt to be forwarded over the wire
                    if(srcNode<16 && rxSequences[srcNode] != ((packet->header.remainingReSend)&0xF0)){
                        rxSequences[srcNode] = (packet->header.remainingReSend)&0xF0;
                        shouldSend = 1; 
                    } 
                    
                    //Starts the DMA transfer of the payload into the EMAC
                    if(shouldSend) warpmac_prepDataToNetwork(packet);
                    
                    //Blocks until the PHY is finished sending and enables the receiver
                    warpmac_finishPhyXmit();
                    
                    //Waits until the DMA transfer is complete, then starts the EMAC
                    if(shouldSend) warpmac_startDataToNetwork(packet);
                    
                    pktCount_good++;
                
                
                myStats[srcNode].goodPackets++;
                myStats[srcNode].rxBytes += (packet->header.length + NUM_HEADER_BYTES + NUM_PAYLOAD_CRC_BYTES + NUM_PAYLOAD_TAIL_BYTES);
                myStats[srcNode].txBytes += NUM_HEADER_BYTES; 
                }
                
                if(state&BAD){
                    //warpmac_incrementLEDLow();
                    pktCount_bad++;
                    
                    //Rotate the ring of LEDs counter-clockwise
                    userIOBoard_LEDs = userIOBoard_LEDs==0x80 ? 0x01 : (userIOBoard_LEDs << 1);


                myStats[srcNode].partnerBadPackets++;
                }
                
                break;
                
            case ACKPACKET:
                //Clear the TIMEOUT and enable Ethernet
                //if(warpmac_inTimeout()){
                    warpmac_incrementLEDHigh();

                    warpmac_clearTimer(TIMEOUT_TIMER);
                    txBuffer.header.remainingReSend = 0;
                    warpmac_resetCurrentCW();
                    warpmac_setTimer(BACKOFF_TIMER);
            //  }
                
                pktCount_good++;
                
                break;
        }
    }
}

///@brief Main function
///
///This function configures MAC parameters, enables the underlying frameworks, and then loops forever.
int main(){     

    
    
    //Initialize global variables
    currentAnt = 0;
    chan = 9;
    
    pktCount_good = 0;
    pktCount_bad = 0;
    
    pktBuf_rx = 0;
    pktBuf_tx_DATA = 1;
    pktBuf_tx_ACK = 2;
    autoResp_delay = 0;

    //Set the full-rate modulation to QPSK by default
    pktFullRate = HDR_FULLRATE_QPSK;
    
    //Set the payload coding rate to NONE by default
    //Note: Reference Design 12 PHY does not provide channel coding
    pktCodeRate = CODE_RATE_NONE;
    
    //Initialize the framework
    warpmac_init();
    maximumReSend = 8;
    warpmac_setMaxResend(maximumReSend);
    warpmac_setMaxCW(5);

    warpphy_setAntennaMode(TX_ANTMODE_SISO_ANTA, RX_ANTMODE_SISO_ANTA);
//  warpphy_setAntennaMode(TX_ANTMODE_ALAMOUTI, RX_ANTMODE_ALAMOUTI_ANTA);

    warpmac_setTimeout(100); //400
    warpmac_setSlotTime(22); //9 20
    
    //Read Dip Switch value from FPGA board.
    //This value will be used as an index into the routing table for other nodes
    myID = (unsigned short int)warpmac_getMyId();
            
    //Rx buffer is where the EMAC will DMA Wireless payloads from
    warpmac_setRxBuffer(&rxBuffer, pktBuf_rx);
    //Tx buffer is where the EMAC will DMA Ethernet payloads to
    warpmac_setTxBuffer(pktBuf_tx_DATA);
    
    //Copy this node's MAC address into the Tx buffer's source address field
    txBuffer.header.srcAddr = (unsigned short int)( ((myID + 2)<<8) | myID);
    
    //Register callbacks
    warpmac_setBadHeaderCallback((void *)phyRx_badHeader_callback);
    warpmac_setGoodHeaderCallback((void *)phyRx_goodHeader_callback);
    warpmac_setTimerCallback((void *)timer_callback);
    warpmac_setDataFromNetworkCallback((void *)dataFromNetworkLayer_callback);
    warpmac_setMgmtFromNetworkCallback((void *)mgmtFromNetworkLayer_callback);
    warpmac_setUpButtonCallback((void *)up);
    warpmac_setMiddleButtonCallback((void *)middle);
    warpmac_setLeftButtonCallback((void *)left);
    warpmac_setRightButtonCallback((void *)right);
    warpmac_setUartRecvCallback((void *)uartRecv_callback);
    

    userIOBoard_LEDs = 1;
    //Initialize the LCD for slot 1 (i.e. don't flip the image) and non-inverted colors
    warp_userioboard_lcd_init(1, 0);
    warp_userioboard_set_lcd_charbuf(1);
    
    //Print some static lines to the user I/O board LCD
    //snprintf(charsToPrint, 16, " Ref Design v12 ", pktCount_bad);
    warp_userio_lcd_printline("RefDesign v13.10", 16, 1, 1);
    //snprintf(charsToPrint, 16, "  Rx Pkt Counts", pktCount_bad);
    warp_userio_lcd_printline("  Rx Pkt Counts ", 16, 3, 1);
    
    //Set the user I/O board LEDs
    warp_userioboard_set_leds(userIOBoard_LEDs);
    
    //Set the default center frequency
    warpphy_setChannel(GHZ_2, chan);
    
    //Enable carrier sensing
    warpmac_enableCSMA();
    
    /******** START autoResponse setup *******/
    //Setup the PHY's autoResponse system
    // For CSMA, it is configured to send pktBuf 2 when a good DATA packet
    //  is received with myID as the destination address
    
    //Match condition 0: received header's destination address is myID
    autoResp_matchCond = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_DSTADDR, 2, htons( ((myID+2)<<8 | myID)) );
    mimo_ofdmTxRx_setMatch0(autoResp_matchCond);

    //Match condition 1: received header's type is DATA 
    autoResp_matchCond = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_TYPE, 1, DATAPACKET);
    mimo_ofdmTxRx_setMatch1(autoResp_matchCond);
    
    //Configure the header translator to use the Rx pkt's src address as the outgoing pkt's dst address
    // Addresses are two bytes, so two entries in the header translator need to be overridden
    // Except for these bytes, the ACK pktBuf's contents will be sent unaltered
    // PHY_HEADERTRANSLATE_SET(templatePktBuf, byteAddrToOverwrite, srcPktBuf, srcByteAddr)
    PHY_HEADERTRANSLATE_SET(pktBuf_tx_ACK, (PKTHEADER_INDX_DSTADDR+0), pktBuf_rx, (PKTHEADER_INDX_SRCADDR+0));
    PHY_HEADERTRANSLATE_SET(pktBuf_tx_ACK, (PKTHEADER_INDX_DSTADDR+1), pktBuf_rx, (PKTHEADER_INDX_SRCADDR+1));

    //Create a template ACK packet
    templatePkt.header.fullRate = pktFullRate;
    templatePkt.header.codeRate = pktCodeRate;
    templatePkt.header.length = 0;
    templatePkt.header.srcAddr = (unsigned short)(((myID+2)<<8) | myID);
    templatePkt.header.pktType = ACKPACKET;
    
    //Copy the header down to the PHY's packet buffer
    // This doesn't actually send anything; the autoResponse system will use this template when sending ACKs
    warpmac_prepPhyForXmit(&templatePkt, pktBuf_tx_ACK);

    //Action defitions come last; bad things might happen if an action is enabled before the template pkt is ready

    //Action 0: send pkt from buf 2 when match0 & match1 & goodPkt, using header translation
    autoResp_action = PHY_AUTORESPONSE_TXACTION_CONFIG(pktBuf_tx_ACK, 1, (PHY_AUTORESPONSE_REQ_MATCH0 | PHY_AUTORESPONSE_REQ_MATCH1), autoResp_delay, 0, 0, 1, 0, 1);
    mimo_ofdmTxRx_setAction0(autoResp_action);
    
    /******* END autoResponse setup ******/
    
    //Listen for new packets to send (either from Ethernet or local dummy packets)
    //warpmac_setDebugGPIO(0);
    warpmac_enableDataFromNetwork();
    
    //Set the modulation scheme use for base rate (header) symbols
    warpmac_setBaseRate(QPSK);
    
    //Enable local packet generation (ignoring Ethernet)
    warpmac_enableDummyPacketMode();
    
    
    
    
    warpmac_stopPacketGeneration(); //second argument is interval in usec
    
    //Intialize Global variables for csmaMac extension
    Mode=0;
    sendStats=0;
    Time=0;
   packetlength=1470;
    
    int i;  
    //Initialize the Stats Structs
    for(i=0; i<WARPNET_NUMNODES; i++)
    {
        txSequences[i] = 1;
        rxSequences[i] = 0;
        myStats[i].structType = STRUCTID_NODESTATS;
        myStats[i].nodeID = myID;
        myStats[i].partnerID = i;
        myStats[i].goodPackets = 0;
        myStats[i].otherBadPackets = 0;
        myStats[i].partnerBadPackets = 0;
        myStats[i].rxBytes = 0;
        myStats[i].txBytes = 0;
        myStats[i].reserved0 = 0;
    }
    
    //CRH
//  if(myID==1) warpmac_startPacketGeneration(1470,0);
//  warpphy_setTxPower(63);
    //CRH
    
#define PKTHEADER_INDX_DSTADDR  6//6
#define PKTHEADER_INDX_SRCADDR  4
#define PKTHEADER_INDX_TYPE     10
/*  
    //Setting up auto ACK responder
    unsigned int matchCond_toMe     = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_DSTADDR, 1, myID);
    unsigned int matchCond_dataType = PHY_AUTORESPONSE_MATCH_CONFIG(PKTHEADER_INDX_TYPE, 1, DATAPACKET);
    //  unsigned int action_ackPktBuf   = PHY_AUTORESPONSE_TXACTION_CONFIG(2, 0, (PHY_AUTORESPONSE_REQ_MATCH0 | PHY_AUTORESPONSE_REQ_MATCH1), 50, 0, 0, 1, 0, 0);
    unsigned int action_ackPktBuf   = PHY_AUTORESPONSE_TXACTION_CONFIG(2, 1, (PHY_AUTORESPONSE_REQ_MATCH0 | PHY_AUTORESPONSE_REQ_MATCH1), 0, 0, 0, 1, 0, 1);
    mimo_ofdmTxRx_setMatch0(matchCond_toMe);
    mimo_ofdmTxRx_setMatch1(matchCond_dataType);
    mimo_ofdmTxRx_setAction0(action_ackPktBuf);
    
    xil_printf("Set match0=%x, match1=%x, act0=%x\r\n", mimo_ofdmTxRx_getMatch0(), mimo_ofdmTxRx_getMatch1(), mimo_ofdmTxRx_getAction0());
    
    unsigned char ack_template = 2;
    unsigned char pktBuf_HdrSrc = 0;
    
    //Fill in the header translator with non-translating values by default
    // This will "overwrite" the header with the same pktbuf/byte combos as the template buffer
    unsigned char i;
    for(i=0; i<32; i++)
    {
        XIo_Out32(XPAR_OFDM_TXRX_MIMO_PLBW_0_MEMMAP_TXHEADERTRANSLATE+i*sizeof(int), ( (ack_template<<5) | i));
    }
    
    XIo_Out32(XPAR_OFDM_TXRX_MIMO_PLBW_0_MEMMAP_TXHEADERTRANSLATE+PKTHEADER_INDX_DSTADDR*sizeof(int), ( (1<<5) | PKTHEADER_INDX_SRCADDR));
    XIo_Out32(XPAR_OFDM_TXRX_MIMO_PLBW_0_MEMMAP_TXHEADERTRANSLATE+(PKTHEADER_INDX_DSTADDR+1)*sizeof(int), ( (1<<5) | (PKTHEADER_INDX_SRCADDR+1)));
    */
    
    //WarpRadio_v1_TxEnSoftControlDisable(RADIO2_ADDR);
    
    //Get Clock input
    XTime rand_time;
    XTime_GetTime(&end_timer);
    
    //Seed Random Number Generator
    srand(myID ^ ((int) rand_time) ^ WarpRadio_v1_RSSIData(RADIO2_ADDR));
    
    //Initialize Ethernet Packet Header
    txEthPktHeader.ethType = WARPNET_ETHTYPE_NODE2SVR;
    memset(&(txEthPktHeader.srcAddr), (unsigned char)myID, 6); //Generic source address; ignored throughout
    memset(&(txEthPktHeader.dstAddr), (unsigned char)0xFF, 6); //Broadcast destination address
        
    /*
    unsigned int pktBuf_payloadAddr;
    pktBuf_payloadAddr = warpphy_getBuffAddr(WARPNET_CHANEST_PKTBUFFINDEX);
    
    chanEst_AA[0].structType = STRUCTID_CHANEST;
    chanEst_AA[0].nodeID = myID;
    chanEst_AA[0].partnerID = 0;
    chanEst_AA[0].antennaID = 0;
    chanEst_AA[0].agc_RF = 0;
    chanEst_AA[0].agc_BB = 0;
    chanEst_AA[0].pktStatus = 0;
    chanEst_AA[0].reserved0 = 0;

    memcpy((void *)(pktBuf_payloadAddr+sizeof(txEthPktHeader) ),
            (void *)&(chanEst_AA[0]), sizeof(warpnodeChanEst));
    */

    xil_printf("Reference Design v12.1\r\n");
    xil_printf("Beginning main loop\r\n");
    
    while(1){
        warpmac_pollPeripherals();
    }
    
    return;
}