File content as of revision 7:cda17cffec3c:

////////////// Information about the frame format: //////////////
#define TXRX_BUFFER 600
//Ethernet:
#define ETH_PROTO_OFFSET (12)
#define ETH_PAYLOAD_OFFSET (14)
//IP/UDP:
#define MSG_LEN (TXRX_BUFFER-14-20-8-10)
#define IP_LEN_OFFSET (2)
#define IP_PROTO_OFFSET (9)
#define IP_CKS_OFFSET (10)
#define IP_SRC_ADDR_OFFSET (12)
#define IP_DEST_ADDR_OFFSET (16)
#define IP_PAYLOAD_OFFSET (20)
#define IP_UDP_DESTPORT_OFFSET (20+2)
#define IP_UDP_LEN_OFFSET (20+2+2)
#define IP_UDP_CKS_OFFSET (20+2+2+2)
#define IP_UDP_PAYLOAD_OFFSET (20+8)
//ARP:
#define ARP_SENDER_HW_ADDR_OFFSET (8)
#define ARP_TARGET_HW_ADDR_OFFSET (8+6+4)
#define ARP_LEN (28)

/////////////////////////////////////////////////////////////////
struct QuadStateTXRXTimes{
    double    target_tx_dt;
    double    actual_tx_dt;
    double    average_tx_dt;
    double    average_tx_dt_k;

    double    target_rx_dt;
    double    actual_rx_dt;
    double    average_rx_dt;
    double    average_rx_dt_k;
    
    void reset(){
        target_tx_dt = TARGET_TX_DT;
        actual_tx_dt = TARGET_TX_DT;
        average_tx_dt = TARGET_TX_DT;
        average_tx_dt_k = AVERAGE_DT_K_GAIN;

        target_rx_dt = TARGET_RX_DT;
        actual_rx_dt = TARGET_RX_DT;
        average_rx_dt = TARGET_RX_DT;
        average_rx_dt_k = AVERAGE_DT_K_GAIN;
    }
};
QuadStateTXRXTimes TXRX_times;
Mutex         TXRX_mutex;
QuadState     TXRX_txQuadState;
QuadState     TXRX_rxQuadState;
Timer         TXRX_tx_dt_timer;
Timer         TXRX_rx_dt_timer;
char          TXRX_buffer [TXRX_BUFFER];

//Addresses
#define DEST_MAC_ADDR  0xF8,0xD1,0x11,0xA0,0x51,0xD0 //MAC address of my TL-MR3020 router
#define MY_IP_ADDR     0xc0,0xa8,0x02,0x10
#define DEST_IP_ADDR   0xc0,0xa8,0x02,0x01
#define UDP_PORT       0x08,0xb1
const char dest_mac_addr [6] = { 
    DEST_MAC_ADDR
};

//dumped frames:
char ethernet_frame [14] = { //Ethernetv2 packet header
    0xff,0xff,0xff,0xff,0xff,0xff,   //DEST ADDR
    0x00,0x02,0xf7,0xf1,0xa4,0xdb,   //SRC  ADDR
    0x08,0x00                        //PROTO
};
const char ethernet_frame_arp_proto [2] = {
    0x08,0x06
};
char ipudp_frame [28] = { //IPv4/UDP packet header (message must be MSG_LEN bytes)
    0x45,                            //IP: VER/HEADER LEN
    0x00,                            //DIFFERENTIAL SERVICE
    0x02,0x40,                       //TOTAL LEN
    0x01,0xa0,                       //ID
    0x00,                            //FLAGS
    0x00,                            //FRAG OFFSET
    0xff,                            //TTL
    0x11,                            //PROTO=udp
    0x32,0x99,                       //HEADER CKS
    MY_IP_ADDR,                      //SOURCE ADDR
    DEST_IP_ADDR,                    //DEST ADDR
                                     //UDP:
    UDP_PORT,                        //SOURCE PORT
    UDP_PORT,                        //DEST   PORT
    0x02,0x2c,                       //TOTAL LEN
    0x27,0xe2                        //CKS
};
char arp_frame [28] = { //Address Resolution Protocol packet
    0x00,0x01,                       //HTYPE ethernet
    0x08,0x00,                       //PTYPE IP
    0x06,0x04,                       //HLEN, PLEN
    0x00,0x02,                       //OPER   reply
    0x00,0x02,0xf7,0xf1,0xa4,0xdb,   //Sender hw addr
    MY_IP_ADDR,                      //Sender proto addr
    DEST_MAC_ADDR,                   //Target hw addr
    DEST_IP_ADDR                     //Target proto addr
};

#define htons(n)  ( (((n) & 0xFF00) >> 8) | (((n) & 0x00FF) << 8) )
uint16_t ip_checksum (const char * buf, size_t hdr_len);
uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr);

void TXRX_thread_routine (void const *args){ //New magic version
    DigitalOut led_rx(LED_RX); led_rx = 0;
    
    //Prepare state
    TXRX_times.reset();
    TXRX_txQuadState.reset();
    TXRX_rxQuadState.reset();
    
    //Setup the magic networking
    Thread::wait(2000);
    FAST_FLASH_OFF(led_rx,2);
    Ethernet eth0;
    FAST_FLASH_OFF(led_rx,2);
    eth0.address(ethernet_frame+6);
    FAST_FLASH_OFF(led_rx,2);
    Thread::wait(2000);
    
    //infinite loop: if disconnected, reconnects.
    while(1){
        
        //Wait eth0 link
        Thread::wait(10);
        while(!eth0.link()){
            SLOW_FLASH_ON(led_rx,1);
        };
        Thread::wait(5);
        FAST_FLASH_OFF(led_rx,2);
        
        //Perform 5 gratuitous ARP reply, with an interval of 10ms
        memset(ethernet_frame, 0xff, 6); //ethernet broadcast
        memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw addr = my mac; sender ip addr predefined.
        memcpy(arp_frame+ARP_TARGET_HW_ADDR_OFFSET, arp_frame+ARP_SENDER_HW_ADDR_OFFSET, 6+4); //target = sender (both hw and ip - gratuitous ARP reply as announcement).
        for(int i=0;i<5;++i){
            eth0.write(ethernet_frame,12); //ethernet dest and src addr
            eth0.write(ethernet_frame_arp_proto,2); //ethernet proto field = arp
            eth0.write(arp_frame,ARP_LEN); //arp packet
            eth0.send();
            Thread::wait(10);
        }
        FAST_FLASH_ON(led_rx,5);
        Thread::wait(5);
    
    
        //Prepare timers
        TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
        TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
    
        unsigned int step=0;
        bool accept=false, sent=false, connected=true;
        while(connected){
            
            led_rx = (step % 40 < 35 ? 1 : (TXRX_times.actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving.
            
            {   //RECEIVE
                accept = false;
                const int received = eth0.receive();
                if(received > ETH_PAYLOAD_OFFSET && received < TXRX_BUFFER){ //check ethernet frame packet
                    //ACCEPTANCE
                    accept = (eth0.read(TXRX_buffer,received) > ETH_PAYLOAD_OFFSET ? true : false);
                    //memcpy(ethernet_frame, TXRX_buffer+6, 6);//sent dest hw addr = received src hw addr
                    //CHECK ARP PACKET AND SEND REPLY
                    if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame_arp_proto,2)){
                        accept = false; //consider arp packets as not accepted and reply now.
                        memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw = my mac
                        memcpy(arp_frame+ARP_TARGET_HW_ADDR_OFFSET, TXRX_buffer+ETH_PAYLOAD_OFFSET+ARP_SENDER_HW_ADDR_OFFSET, 6+4); //target = sender (both hw and ip)
                        eth0.write(ethernet_frame,12);
                        eth0.write(ethernet_frame_arp_proto,2);
                        eth0.write(arp_frame,ARP_LEN);
                        eth0.send();
                    }
                    else if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET, ethernet_frame+ETH_PROTO_OFFSET,2)
                      && received > ETH_PAYLOAD_OFFSET + IP_UDP_PAYLOAD_OFFSET + 2){
                        //VALIDATE UDP PACKET
                        TXRX_buffer[received] = 0; //string termination
                        accept = true;/*
                        if(0 != memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame+ETH_PROTO_OFFSET, 2)) accept = false;
                        if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_PROTO_OFFSET,ipudp_frame+IP_PROTO_OFFSET, 2)) accept = false;
                        if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_DESTPORT_OFFSET,ipudp_frame+IP_UDP_DESTPORT_OFFSET, 2)) accept = false;*/
                    }
                    else accept = false;
                }
            }
TXRX_mutex.lock();
            //PRODUCE ACCEPTING DATA
            if(accept)
                accept = (QuadState::length() == TXRX_rxQuadState.setFromJSON(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_PAYLOAD_OFFSET));
                
            //Import tx/rx times config from main thread
            TXRX_times.target_rx_dt        =  TXRX_txQuadState.target_rx_dt;
            TXRX_times.average_rx_dt_k     =  TXRX_txQuadState.average_rx_dt_k;
            TXRX_times.target_tx_dt        =  TXRX_txQuadState.target_tx_dt;
            TXRX_times.average_tx_dt_k     =  TXRX_txQuadState.average_tx_dt_k;
            //Export tx/rx times measured to remote and main thread
            TXRX_txQuadState.actual_rx_dt  = (TXRX_rxQuadState.actual_rx_dt  = TXRX_times.actual_rx_dt);
            TXRX_txQuadState.average_rx_dt = (TXRX_rxQuadState.average_rx_dt = TXRX_times.average_rx_dt);
            TXRX_txQuadState.actual_tx_dt  = (TXRX_rxQuadState.actual_tx_dt  = TXRX_times.actual_tx_dt);
            TXRX_txQuadState.average_tx_dt = (TXRX_rxQuadState.average_tx_dt = TXRX_times.average_tx_dt);
            
            {   //SEND
                //TXRX_buffer will contain: IP header, UDP header, json message. (no ethernet header).
                //Why? the function udp_checksum needs the udp payload to be next to the udp header.
                memset(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET,' ',MSG_LEN);
                int setted = TXRX_txQuadState.getJSON(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET);
TXRX_mutex.unlock();
                //CALC CHECKSUMs
                memcpy(TXRX_buffer,ipudp_frame,IP_UDP_PAYLOAD_OFFSET);
                memset(TXRX_buffer+IP_CKS_OFFSET,0,2);
                memset(TXRX_buffer+IP_UDP_CKS_OFFSET,0,2);
                uint16_t ip_cks  = htons ( ip_checksum(TXRX_buffer,IP_PAYLOAD_OFFSET) );
                //uint16_t udp_cks = htons ( udp_checksum(TXRX_buffer+IP_PAYLOAD_OFFSET, 8+MSG_LEN, TXRX_buffer+IP_SRC_ADDR_OFFSET, TXRX_buffer+IP_DEST_ADDR_OFFSET) );
                memcpy(TXRX_buffer+IP_CKS_OFFSET,&ip_cks,2);
                //memcpy(TXRX_buffer+IP_UDP_CKS_OFFSET,&udp_cks,2);
                //WRITE PACKET AND SEND
                eth0.write(ethernet_frame,ETH_PAYLOAD_OFFSET);
                eth0.write(TXRX_buffer,IP_UDP_PAYLOAD_OFFSET+MSG_LEN);
                sent = (eth0.send() ? true : false);
            }
            
            //check rx/tx dt time
            if(accept)
              QUAD_STATE_UPDATE_DT (TXRX_times, rx, TXRX_rx_dt_timer)
            else
              QUAD_STATE_READ_ACTUAL_DT (TXRX_times, rx, TXRX_rx_dt_timer)
            if(sent)
              QUAD_STATE_UPDATE_DT (TXRX_times, tx, TXRX_tx_dt_timer)
            else
              QUAD_STATE_READ_ACTUAL_DT (TXRX_times, tx, TXRX_tx_dt_timer)
            
            
            if(TXRX_times.actual_rx_dt > 10.0*TXRX_times.target_rx_dt)//disconnect when receiving nothing for a long time
                connected = false;
            if(TXRX_times.actual_rx_dt > 3.00*TXRX_times.target_rx_dt)//check link when receiving nothing for just a while
                connected = eth0.link();
                
            led_rx = 0;
            
            ///REALX TIMINGS (always sleep at least 48ms)
            double to_sleep = (TXRX_times.target_tx_dt - TXRX_times.actual_tx_dt) - 0.048;
            if(to_sleep < 0)
                to_sleep=0;
            QUAD_STATE_WAIT_DT_TARGET(0, to_sleep)
            Thread::wait(48);
            
            ++step;
        }
        //end of while(connected)
    }
}


//////////////////////////////////////////////////////


bool TXRX_stateExchange (QuadState & tx, QuadState & rx){
  if(TXRX_mutex.trylock()){
      rx = TXRX_rxQuadState;
      TXRX_txQuadState = tx;
      TXRX_mutex.unlock();
      return true;
  }
  return false;
}


//////////////////////////////////////////////////////




uint16_t ip_checksum (const char * buf, size_t hdr_len)
{
        unsigned long sum = 0;
        const char *ip1 = buf;
        while (hdr_len > 1)
        {
                sum += (*ip1++)<<8;  sum += (*ip1++);
                if (sum & 0x80000000)
                        sum = (sum & 0xFFFF) + (sum >> 16);
                hdr_len -= 2;
        }
        while (sum >> 16)
                sum = (sum & 0xFFFF) + (sum >> 16);
        return(~sum);
}

uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr){
    const char *buf=buff;
    uint32_t sum;
    size_t length=len;
    sum = 0;
    while (len > 1){
        sum += (*buf++)<<8;  sum += (*buf++);
        if (sum & 0x80000000)
            sum = (sum & 0xFFFF) + (sum >> 16);
        len -= 2;
    }
    if ( len & 1 )
        sum += *((uint8_t *)buf);
    sum += (*src_addr++)<<8;   sum += (*src_addr++);
    sum += (*src_addr++)<<8;   sum += (*src_addr++);
    sum += (*dest_addr++)<<8;  sum += (*dest_addr++);
    sum += (*dest_addr++)<<8;  sum += (*dest_addr++);
    sum += htons(17); //IPPROTO_UDP
    sum += htons(length);
    while (sum >> 16)
            sum = (sum & 0xFFFF) + (sum >> 16);
    return ((uint16_t)(~sum));
}