Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
TXRX_magic.h
- Committer:
- MatteoT
- Date:
- 2014-05-24
- Revision:
- 7:cda17cffec3c
- Parent:
- 6:4ddd68260f76
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)); }