TXRX_magic.h

00001 ////////////// Information about the frame format: //////////////
00002 #define TXRX_BUFFER 600
00003 //Ethernet:
00004 #define ETH_PROTO_OFFSET (12)
00005 #define ETH_PAYLOAD_OFFSET (14)
00006 //IP/UDP:
00007 #define MSG_LEN (TXRX_BUFFER-14-20-8-10)
00008 #define IP_LEN_OFFSET (2)
00009 #define IP_PROTO_OFFSET (9)
00010 #define IP_CKS_OFFSET (10)
00011 #define IP_SRC_ADDR_OFFSET (12)
00012 #define IP_DEST_ADDR_OFFSET (16)
00013 #define IP_PAYLOAD_OFFSET (20)
00014 #define IP_UDP_DESTPORT_OFFSET (20+2)
00015 #define IP_UDP_LEN_OFFSET (20+2+2)
00016 #define IP_UDP_CKS_OFFSET (20+2+2+2)
00017 #define IP_UDP_PAYLOAD_OFFSET (20+8)
00018 //ARP:
00019 #define ARP_SENDER_HW_ADDR_OFFSET (8)
00020 #define ARP_TARGET_HW_ADDR_OFFSET (8+6+4)
00021 #define ARP_LEN (28)
00022 
00023 /////////////////////////////////////////////////////////////////
00024 struct QuadStateTXRXTimes{
00025     double    target_tx_dt;
00026     double    actual_tx_dt;
00027     double    average_tx_dt;
00028     double    average_tx_dt_k;
00029 
00030     double    target_rx_dt;
00031     double    actual_rx_dt;
00032     double    average_rx_dt;
00033     double    average_rx_dt_k;
00034     
00035     void reset(){
00036         target_tx_dt = TARGET_TX_DT;
00037         actual_tx_dt = TARGET_TX_DT;
00038         average_tx_dt = TARGET_TX_DT;
00039         average_tx_dt_k = AVERAGE_DT_K_GAIN;
00040 
00041         target_rx_dt = TARGET_RX_DT;
00042         actual_rx_dt = TARGET_RX_DT;
00043         average_rx_dt = TARGET_RX_DT;
00044         average_rx_dt_k = AVERAGE_DT_K_GAIN;
00045     }
00046 };
00047 QuadStateTXRXTimes TXRX_times;
00048 Mutex         TXRX_mutex;
00049 QuadState     TXRX_txQuadState;
00050 QuadState     TXRX_rxQuadState;
00051 Timer         TXRX_tx_dt_timer;
00052 Timer         TXRX_rx_dt_timer;
00053 char          TXRX_buffer [TXRX_BUFFER];
00054 
00055 //Addresses
00056 #define DEST_MAC_ADDR  0xF8,0xD1,0x11,0xA0,0x51,0xD0 //MAC address of my TL-MR3020 router
00057 #define MY_IP_ADDR     0xc0,0xa8,0x02,0x10
00058 #define DEST_IP_ADDR   0xc0,0xa8,0x02,0x01
00059 #define UDP_PORT       0x08,0xb1
00060 const char dest_mac_addr [6] = { 
00061     DEST_MAC_ADDR
00062 };
00063 
00064 //dumped frames:
00065 char ethernet_frame [14] = { //Ethernetv2 packet header
00066     0xff,0xff,0xff,0xff,0xff,0xff,   //DEST ADDR
00067     0x00,0x02,0xf7,0xf1,0xa4,0xdb,   //SRC  ADDR
00068     0x08,0x00                        //PROTO
00069 };
00070 const char ethernet_frame_arp_proto [2] = {
00071     0x08,0x06
00072 };
00073 char ipudp_frame [28] = { //IPv4/UDP packet header (message must be MSG_LEN bytes)
00074     0x45,                            //IP: VER/HEADER LEN
00075     0x00,                            //DIFFERENTIAL SERVICE
00076     0x02,0x40,                       //TOTAL LEN
00077     0x01,0xa0,                       //ID
00078     0x00,                            //FLAGS
00079     0x00,                            //FRAG OFFSET
00080     0xff,                            //TTL
00081     0x11,                            //PROTO=udp
00082     0x32,0x99,                       //HEADER CKS
00083     MY_IP_ADDR,                      //SOURCE ADDR
00084     DEST_IP_ADDR,                    //DEST ADDR
00085                                      //UDP:
00086     UDP_PORT,                        //SOURCE PORT
00087     UDP_PORT,                        //DEST   PORT
00088     0x02,0x2c,                       //TOTAL LEN
00089     0x27,0xe2                        //CKS
00090 };
00091 char arp_frame [28] = { //Address Resolution Protocol packet
00092     0x00,0x01,                       //HTYPE ethernet
00093     0x08,0x00,                       //PTYPE IP
00094     0x06,0x04,                       //HLEN, PLEN
00095     0x00,0x02,                       //OPER   reply
00096     0x00,0x02,0xf7,0xf1,0xa4,0xdb,   //Sender hw addr
00097     MY_IP_ADDR,                      //Sender proto addr
00098     DEST_MAC_ADDR,                   //Target hw addr
00099     DEST_IP_ADDR                     //Target proto addr
00100 };
00101 
00102 #define htons(n)  ( (((n) & 0xFF00) >> 8) | (((n) & 0x00FF) << 8) )
00103 uint16_t ip_checksum (const char * buf, size_t hdr_len);
00104 uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr);
00105 
00106 void TXRX_thread_routine (void const *args){ //New magic version
00107     DigitalOut led_rx(LED_RX); led_rx = 0;
00108     
00109     //Prepare state
00110     TXRX_times.reset();
00111     TXRX_txQuadState.reset();
00112     TXRX_rxQuadState.reset();
00113     
00114     //Setup the magic networking
00115     Thread::wait(2000);
00116     FAST_FLASH_OFF(led_rx,2);
00117     Ethernet eth0;
00118     FAST_FLASH_OFF(led_rx,2);
00119     eth0.address(ethernet_frame+6);
00120     FAST_FLASH_OFF(led_rx,2);
00121     Thread::wait(2000);
00122     
00123     //infinite loop: if disconnected, reconnects.
00124     while(1){
00125         
00126         //Wait eth0 link
00127         Thread::wait(10);
00128         while(!eth0.link()){
00129             SLOW_FLASH_ON(led_rx,1);
00130         };
00131         Thread::wait(5);
00132         FAST_FLASH_OFF(led_rx,2);
00133         
00134         //Perform 5 gratuitous ARP reply, with an interval of 10ms
00135         memset(ethernet_frame, 0xff, 6); //ethernet broadcast
00136         memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw addr = my mac; sender ip addr predefined.
00137         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).
00138         for(int i=0;i<5;++i){
00139             eth0.write(ethernet_frame,12); //ethernet dest and src addr
00140             eth0.write(ethernet_frame_arp_proto,2); //ethernet proto field = arp
00141             eth0.write(arp_frame,ARP_LEN); //arp packet
00142             eth0.send();
00143             Thread::wait(10);
00144         }
00145         FAST_FLASH_ON(led_rx,5);
00146         Thread::wait(5);
00147     
00148     
00149         //Prepare timers
00150         TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
00151         TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
00152     
00153         unsigned int step=0;
00154         bool accept=false, sent=false, connected=true;
00155         while(connected){
00156             
00157             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.
00158             
00159             {   //RECEIVE
00160                 accept = false;
00161                 const int received = eth0.receive();
00162                 if(received > ETH_PAYLOAD_OFFSET && received < TXRX_BUFFER){ //check ethernet frame packet
00163                     //ACCEPTANCE
00164                     accept = (eth0.read(TXRX_buffer,received) > ETH_PAYLOAD_OFFSET ? true : false);
00165                     //memcpy(ethernet_frame, TXRX_buffer+6, 6);//sent dest hw addr = received src hw addr
00166                     //CHECK ARP PACKET AND SEND REPLY
00167                     if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame_arp_proto,2)){
00168                         accept = false; //consider arp packets as not accepted and reply now.
00169                         memcpy(arp_frame+ARP_SENDER_HW_ADDR_OFFSET, ethernet_frame+6, 6); //sender hw = my mac
00170                         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)
00171                         eth0.write(ethernet_frame,12);
00172                         eth0.write(ethernet_frame_arp_proto,2);
00173                         eth0.write(arp_frame,ARP_LEN);
00174                         eth0.send();
00175                     }
00176                     else if(accept && 0 == memcmp(TXRX_buffer+ETH_PROTO_OFFSET, ethernet_frame+ETH_PROTO_OFFSET,2)
00177                       && received > ETH_PAYLOAD_OFFSET + IP_UDP_PAYLOAD_OFFSET + 2){
00178                         //VALIDATE UDP PACKET
00179                         TXRX_buffer[received] = 0; //string termination
00180                         accept = true;/*
00181                         if(0 != memcmp(TXRX_buffer+ETH_PROTO_OFFSET,ethernet_frame+ETH_PROTO_OFFSET, 2)) accept = false;
00182                         if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_PROTO_OFFSET,ipudp_frame+IP_PROTO_OFFSET, 2)) accept = false;
00183                         if(0 != memcmp(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_DESTPORT_OFFSET,ipudp_frame+IP_UDP_DESTPORT_OFFSET, 2)) accept = false;*/
00184                     }
00185                     else accept = false;
00186                 }
00187             }
00188 TXRX_mutex.lock();
00189             //PRODUCE ACCEPTING DATA
00190             if(accept)
00191                 accept = (QuadState::length() == TXRX_rxQuadState.setFromJSON(TXRX_buffer+ETH_PAYLOAD_OFFSET+IP_UDP_PAYLOAD_OFFSET));
00192                 
00193             //Import tx/rx times config from main thread
00194             TXRX_times.target_rx_dt        =  TXRX_txQuadState.target_rx_dt;
00195             TXRX_times.average_rx_dt_k     =  TXRX_txQuadState.average_rx_dt_k;
00196             TXRX_times.target_tx_dt        =  TXRX_txQuadState.target_tx_dt;
00197             TXRX_times.average_tx_dt_k     =  TXRX_txQuadState.average_tx_dt_k;
00198             //Export tx/rx times measured to remote and main thread
00199             TXRX_txQuadState.actual_rx_dt  = (TXRX_rxQuadState.actual_rx_dt  = TXRX_times.actual_rx_dt);
00200             TXRX_txQuadState.average_rx_dt = (TXRX_rxQuadState.average_rx_dt = TXRX_times.average_rx_dt);
00201             TXRX_txQuadState.actual_tx_dt  = (TXRX_rxQuadState.actual_tx_dt  = TXRX_times.actual_tx_dt);
00202             TXRX_txQuadState.average_tx_dt = (TXRX_rxQuadState.average_tx_dt = TXRX_times.average_tx_dt);
00203             
00204             {   //SEND
00205                 //TXRX_buffer will contain: IP header, UDP header, json message. (no ethernet header).
00206                 //Why? the function udp_checksum needs the udp payload to be next to the udp header.
00207                 memset(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET,' ',MSG_LEN);
00208                 int setted = TXRX_txQuadState.getJSON(TXRX_buffer+IP_UDP_PAYLOAD_OFFSET);
00209 TXRX_mutex.unlock();
00210                 //CALC CHECKSUMs
00211                 memcpy(TXRX_buffer,ipudp_frame,IP_UDP_PAYLOAD_OFFSET);
00212                 memset(TXRX_buffer+IP_CKS_OFFSET,0,2);
00213                 memset(TXRX_buffer+IP_UDP_CKS_OFFSET,0,2);
00214                 uint16_t ip_cks  = htons ( ip_checksum(TXRX_buffer,IP_PAYLOAD_OFFSET) );
00215                 //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) );
00216                 memcpy(TXRX_buffer+IP_CKS_OFFSET,&ip_cks,2);
00217                 //memcpy(TXRX_buffer+IP_UDP_CKS_OFFSET,&udp_cks,2);
00218                 //WRITE PACKET AND SEND
00219                 eth0.write(ethernet_frame,ETH_PAYLOAD_OFFSET);
00220                 eth0.write(TXRX_buffer,IP_UDP_PAYLOAD_OFFSET+MSG_LEN);
00221                 sent = (eth0.send() ? true : false);
00222             }
00223             
00224             //check rx/tx dt time
00225             if(accept)
00226               QUAD_STATE_UPDATE_DT (TXRX_times, rx, TXRX_rx_dt_timer)
00227             else
00228               QUAD_STATE_READ_ACTUAL_DT (TXRX_times, rx, TXRX_rx_dt_timer)
00229             if(sent)
00230               QUAD_STATE_UPDATE_DT (TXRX_times, tx, TXRX_tx_dt_timer)
00231             else
00232               QUAD_STATE_READ_ACTUAL_DT (TXRX_times, tx, TXRX_tx_dt_timer)
00233             
00234             
00235             if(TXRX_times.actual_rx_dt > 10.0*TXRX_times.target_rx_dt)//disconnect when receiving nothing for a long time
00236                 connected = false;
00237             if(TXRX_times.actual_rx_dt > 3.00*TXRX_times.target_rx_dt)//check link when receiving nothing for just a while
00238                 connected = eth0.link();
00239                 
00240             led_rx = 0;
00241             
00242             ///REALX TIMINGS (always sleep at least 48ms)
00243             double to_sleep = (TXRX_times.target_tx_dt - TXRX_times.actual_tx_dt) - 0.048;
00244             if(to_sleep < 0)
00245                 to_sleep=0;
00246             QUAD_STATE_WAIT_DT_TARGET(0, to_sleep)
00247             Thread::wait(48);
00248             
00249             ++step;
00250         }
00251         //end of while(connected)
00252     }
00253 }
00254 
00255 
00256 //////////////////////////////////////////////////////
00257 
00258 
00259 bool TXRX_stateExchange (QuadState & tx, QuadState & rx){
00260   if(TXRX_mutex.trylock()){
00261       rx = TXRX_rxQuadState;
00262       TXRX_txQuadState = tx;
00263       TXRX_mutex.unlock();
00264       return true;
00265   }
00266   return false;
00267 }
00268 
00269 
00270 //////////////////////////////////////////////////////
00271 
00272 
00273 
00274 
00275 uint16_t ip_checksum (const char * buf, size_t hdr_len)
00276 {
00277         unsigned long sum = 0;
00278         const char *ip1 = buf;
00279         while (hdr_len > 1)
00280         {
00281                 sum += (*ip1++)<<8;  sum += (*ip1++);
00282                 if (sum & 0x80000000)
00283                         sum = (sum & 0xFFFF) + (sum >> 16);
00284                 hdr_len -= 2;
00285         }
00286         while (sum >> 16)
00287                 sum = (sum & 0xFFFF) + (sum >> 16);
00288         return(~sum);
00289 }
00290 
00291 uint16_t udp_checksum(const char * buff, size_t len, const char * src_addr, const char * dest_addr){
00292     const char *buf=buff;
00293     uint32_t sum;
00294     size_t length=len;
00295     sum = 0;
00296     while (len > 1){
00297         sum += (*buf++)<<8;  sum += (*buf++);
00298         if (sum & 0x80000000)
00299             sum = (sum & 0xFFFF) + (sum >> 16);
00300         len -= 2;
00301     }
00302     if ( len & 1 )
00303         sum += *((uint8_t *)buf);
00304     sum += (*src_addr++)<<8;   sum += (*src_addr++);
00305     sum += (*src_addr++)<<8;   sum += (*src_addr++);
00306     sum += (*dest_addr++)<<8;  sum += (*dest_addr++);
00307     sum += (*dest_addr++)<<8;  sum += (*dest_addr++);
00308     sum += htons(17); //IPPROTO_UDP
00309     sum += htons(length);
00310     while (sum >> 16)
00311             sum = (sum & 0xFFFF) + (sum >> 16);
00312     return ((uint16_t)(~sum));
00313 }