comms.cpp

00001 #include "mbed.h"
00002 #include "comms.h"
00003 
00004 void le2be_u16(uint8_t* buffer, uint16_t in_word) {  
00005         *buffer++ = (uint8_t)((in_word & 0xFF00) >> 8);
00006         *buffer = (uint8_t)(in_word & 0x00FF);
00007 }
00008 
00009 /**
00010  * This function is used to calculate a one's complement based checksum
00011  * as is commonly used in IP based protocols
00012  */
00013 uint16_t chksum_u16(uint32_t in_word) {
00014     uint32_t temp = in_word & 0x0000FFFF;
00015     temp += (in_word & 0xFFFF0000)>>16;
00016     return (uint16_t)( (temp & 0x0000FFFF) + ((temp & 0xFFFF0000)>>16) );
00017 }
00018 
00019 /**
00020  * Transport constructor.
00021  * Set up the pseudo header and packet to be used in communications
00022  */
00023 Transport::Transport (uint32_t src_addr, uint32_t dst_addr, uint32_t ID) {
00024     // Set up IPv4 pseudo header for checksum calculation
00025     pseudo_header_.src_addr = src_addr;
00026     pseudo_header_.dst_addr = dst_addr;
00027     pseudo_header_.protocol = PROTOCOL;
00028     pseudo_header_.length = sizeof(header_);
00029     // Set up default ports
00030     header_.src_port = PORT_BASE + ID;
00031     header_.dst_port = PORT_BASE;
00032     header_.length = sizeof(header_);
00033     // Calculate checksum of the pseudo-header: this should save some future recalculation
00034     uint32_t w_checksum = chksum_u16(src_addr);
00035     w_checksum = chksum_u16(dst_addr+w_checksum);
00036     w_checksum = chksum_u16(pseudo_header_.protocol+w_checksum);
00037     pseudo_header_.checksum = chksum_u16(pseudo_header_.length+w_checksum);
00038     // Now calculate checksum of the UDP packet header + pseudo-header
00039     w_checksum = chksum_u16(header_.src_port+pseudo_header_.checksum);
00040     w_checksum = chksum_u16(header_.dst_port+w_checksum);
00041     header_.checksum = chksum_u16(header_.length+w_checksum);
00042     // Load up the packet
00043     le2be_u16((packet_+0),header_.src_port);
00044     le2be_u16((packet_+2),header_.dst_port);
00045     le2be_u16((packet_+4),header_.length);
00046     le2be_u16((packet_+6),header_.checksum);
00047 }
00048 
00049 /**
00050  * Provides a pointer to a buffer containing a formatted packet and
00051  * the total length of said packet.
00052  */
00053 void Transport::get_packet (uint8_t** buffer, uint16_t* length) {
00054     *buffer = packet_;
00055     *length = header_.length;
00056 }
00057 
00058 /**
00059  * Loads the transport packet with an array of 16bit samples.
00060  * This will require translating the words into big-endian.
00061  */
00062 void Transport::load_data (uint16_t* buffer, uint16_t length) {
00063     uint8_t* pkt_ptr = packet_ + sizeof(header_);
00064     for (uint16_t i=0; i<length; i++) {
00065         le2be_u16(pkt_ptr,*buffer++);
00066         pkt_ptr += 2;
00067     }
00068     // Set the header length (in octets/bytes)
00069     header_.length = sizeof(header_) + (length<<1);
00070     le2be_u16((packet_+4),header_.length);
00071     // For now set the checksum to IGNORE
00072     header_.checksum = 0x0000;
00073     le2be_u16((packet_+6),header_.checksum);
00074 }
00075 
00076 /**
00077  * Set the destination port.
00078  */
00079 void Transport::set_dst_port (uint16_t port) {
00080     // Store the port both in the local struct...
00081     header_.dst_port = port;
00082     // ...and in the transmission packet
00083     le2be_u16((packet_+2),port);
00084     //packet_[2] = (uint8_t)((port & 0xFF00) >> 8);
00085     //packet_[3] = (uint8_t)(port & 0x00FF);
00086 }