[1567] | 1 | #include <assert.h> |
---|
| 2 | #include <unistd.h> |
---|
| 3 | #include <signal.h> |
---|
| 4 | #include <string.h> |
---|
| 5 | #include <stdlib.h> |
---|
| 6 | #include <pcap.h> |
---|
| 7 | #include "ber_processor.h" |
---|
| 8 | //#include <CoreFoundation/CoreFoundation.h> |
---|
| 9 | #include <sys/time.h> |
---|
| 10 | |
---|
| 11 | //Global variables |
---|
| 12 | |
---|
| 13 | //Lookup table of numbers of ones in binary representations of each 8-bit value |
---|
| 14 | const unsigned char ones_in_chars[256] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4, 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8}; |
---|
| 15 | |
---|
| 16 | //PCAP session handle (used by pcap Rx and Tx threads) |
---|
| 17 | pcap_t *pcap_handle; |
---|
| 18 | |
---|
| 19 | //Network device for PCAP (en0 = Ethernet; en5 = USB Ethernet) |
---|
| 20 | char* pcap_device; |
---|
| 21 | |
---|
| 22 | const char debug = 0; |
---|
| 23 | |
---|
| 24 | //Number of WARP nodes |
---|
| 25 | unsigned int numWARPnodes; |
---|
| 26 | |
---|
| 27 | //WARPnet structs |
---|
| 28 | warpnetObserveBER berStruct; |
---|
| 29 | warpnetEthernetPktHeader txEthPktHdr; |
---|
| 30 | warpnetControllerGroup groupStruct; |
---|
| 31 | unsigned char txEthPktBuf[MAX_ETHPKTSIZE]; |
---|
| 32 | |
---|
| 33 | //This "node's" ID (for talking to the warpnet server as if it were a node) |
---|
| 34 | unsigned char myID; |
---|
| 35 | unsigned short berSeq; |
---|
| 36 | |
---|
| 37 | int phypkt_seq_nums[MAX_NUMWARPNODES]; |
---|
| 38 | |
---|
| 39 | unsigned char* pktBufs[MAX_NUMWARPNODES]; |
---|
| 40 | |
---|
| 41 | //Global sequence number incrememnted every time a packet is sent to any node |
---|
| 42 | unsigned char ethPkt_seqNumTx, ethPkt_seqNumRx; |
---|
| 43 | |
---|
| 44 | //Time structs for scheduled node updates |
---|
| 45 | struct timeval currentTime; |
---|
| 46 | struct timezone theTimezone; |
---|
| 47 | |
---|
| 48 | void ProcessWARPnodeMessage(u_char *args, const struct pcap_pkthdr *header, const u_char *packet) |
---|
| 49 | { |
---|
| 50 | //printf("ProcessWARPnodeMessage: Got a WARP node update\n"); |
---|
| 51 | |
---|
| 52 | int i; |
---|
| 53 | int rv; |
---|
| 54 | |
---|
| 55 | //Packets from the WARP node must have this sturcture; indicies are bytes: |
---|
| 56 | // warpnetEthernetPktHeader struct first: |
---|
| 57 | // 0-5: Source MAC address (last nibble assumed to be nodeID) |
---|
| 58 | // 6-11: Destination MAC address |
---|
[1573] | 59 | // 12-13: Ethertype = WARPNET_ETHTYPE_NODE2BER |
---|
[1567] | 60 | // 14-15: Number of bytes in full packet, including this header |
---|
| 61 | // 16: Number of WARPNet structs in packet |
---|
| 62 | // 17: seqNum (for what?) |
---|
| 63 | // 18:... Raw wireless header and payload as Tx/Rx by the PHY |
---|
| 64 | |
---|
| 65 | warpnetEthernetPktHeader* ethpkt_hdr; |
---|
| 66 | unsigned char ethpkt_nodeID; |
---|
| 67 | unsigned int ethpkt_len; |
---|
| 68 | |
---|
| 69 | phyHeader* phypkt_hdr; |
---|
| 70 | unsigned short phypkt_seqNum; |
---|
| 71 | unsigned short phypkt_srcAddr, phypkt_dstAddr; |
---|
| 72 | unsigned short phypkt_len; |
---|
| 73 | |
---|
| 74 | char isPktFromTx; |
---|
| 75 | char isPktFromRx; |
---|
| 76 | char alreadyHavePktFromTx; |
---|
| 77 | char alreadyHavePktFromRx; |
---|
| 78 | |
---|
| 79 | isPktFromRx = 0; |
---|
| 80 | alreadyHavePktFromTx = 0; |
---|
| 81 | |
---|
| 82 | if( (header->len) < (sizeof(warpnetEthernetPktHeader) + sizeof(phyHeader))) { |
---|
| 83 | printf("ProcessWARPnodeMessage: pcap captured too short a packet! (%d bytes)\n", (header->len)); |
---|
| 84 | return; |
---|
| 85 | } |
---|
| 86 | |
---|
| 87 | //Extract fields from the MAC/PHY header |
---|
| 88 | phypkt_hdr = (phyHeader*)(packet + sizeof(warpnetEthernetPktHeader)); |
---|
| 89 | phypkt_seqNum = htons(phypkt_hdr->seqNum); |
---|
| 90 | phypkt_srcAddr = htons(phypkt_hdr->srcAddr); |
---|
| 91 | phypkt_dstAddr = htons(phypkt_hdr->destAddr); |
---|
| 92 | phypkt_len = htons(phypkt_hdr->length); |
---|
| 93 | |
---|
| 94 | //Intrepret the received packet's header |
---|
| 95 | ethpkt_hdr = (warpnetEthernetPktHeader*)packet; |
---|
| 96 | ethpkt_len = htons(ethpkt_hdr->pktLength); |
---|
| 97 | ethpkt_nodeID = ((ethpkt_hdr->srcAddr[5]) & 0xF); |
---|
| 98 | |
---|
| 99 | |
---|
| 100 | if( (header->len) < ethpkt_len) { |
---|
| 101 | printf("ProcessWARPnodeMessage: pcap captured too short a packet! (%d bytes captured, ethPktHdr.len claimed %d)\n", (header->len), ethpkt_len); |
---|
| 102 | return; |
---|
| 103 | } |
---|
| 104 | |
---|
| 105 | if( ethpkt_len < (phypkt_len + sizeof(phyHeader))) { |
---|
| 106 | printf("ProcessWARPnodeMessage: WARP Node Pkt was too short! (%d vs. %d)\n", ethpkt_len, (int)(phypkt_len + sizeof(phyHeader))); |
---|
| 107 | return; |
---|
| 108 | } |
---|
| 109 | |
---|
| 110 | if((ethpkt_nodeID >= numWARPnodes) | (phypkt_srcAddr > numWARPnodes) | (phypkt_dstAddr > numWARPnodes)) { |
---|
| 111 | printf("ProcessWARPnodeMessage: invalid nodeID or addr! nodeID=%d, dstAddr=%d, srcAddr=%d\n", ethpkt_nodeID, phypkt_dstAddr, phypkt_srcAddr); |
---|
| 112 | for(i=0; i<48; i++) { |
---|
| 113 | printf("%02x ", *((unsigned char *)(packet+i))); |
---|
| 114 | } |
---|
| 115 | printf("\n\n"); |
---|
| 116 | |
---|
| 117 | return; |
---|
| 118 | } |
---|
| 119 | |
---|
| 120 | if(debug > 1) { |
---|
| 121 | printf("Got BER pkt: (PHY: Seq=%d, Src=%d, Dst=%d, Len=%d) (Eth: Len=%d, srcID=%d)\n", \ |
---|
| 122 | phypkt_seqNum, phypkt_srcAddr, phypkt_dstAddr, phypkt_len, \ |
---|
| 123 | ethpkt_len, ethpkt_nodeID); |
---|
| 124 | } |
---|
| 125 | |
---|
| 126 | |
---|
| 127 | //Copy this packet to the local buffers |
---|
| 128 | memcpy(pktBufs[ethpkt_nodeID], packet, (header->len)); |
---|
| 129 | |
---|
| 130 | //Record this wireless packet's sequence number as the reporting node's most recent |
---|
| 131 | phypkt_seq_nums[ethpkt_nodeID] = (int)phypkt_seqNum; |
---|
| 132 | |
---|
| 133 | isPktFromTx = ( ethpkt_nodeID == phypkt_srcAddr); |
---|
| 134 | isPktFromRx = ( ethpkt_nodeID == phypkt_dstAddr); |
---|
| 135 | |
---|
| 136 | alreadyHavePktFromTx = ((int)phypkt_seqNum == phypkt_seq_nums[phypkt_srcAddr]); |
---|
| 137 | alreadyHavePktFromRx = ((int)phypkt_seqNum == phypkt_seq_nums[phypkt_dstAddr]); |
---|
| 138 | |
---|
| 139 | //Check if we have enough to run a BER calculation |
---|
| 140 | if( (isPktFromRx && alreadyHavePktFromTx) || (isPktFromTx && alreadyHavePktFromRx)) |
---|
| 141 | { |
---|
| 142 | // rv = CalculateBER(pktBufs[phypkt_srcAddr]+sizeof(phyHeader)+sizeof(warpnetEthernetPktHeader), pktBufs[phypkt_dstAddr]+sizeof(phyHeader)+sizeof(warpnetEthernetPktHeader), phypkt_len, &berStruct); |
---|
| 143 | //Ignoring last two bytes; somehow a checksum is leaking into the test, maybe via the TEMAC xmit. I'll track it down later... |
---|
| 144 | rv = CalculateBER(pktBufs[phypkt_srcAddr]+sizeof(phyHeader)+sizeof(warpnetEthernetPktHeader), pktBufs[phypkt_dstAddr]+sizeof(phyHeader)+sizeof(warpnetEthernetPktHeader), (phypkt_len-2), &berStruct); |
---|
| 145 | |
---|
| 146 | if(rv == 0) { |
---|
| 147 | //BER calculation was successful; update the berStruct and send it to the server |
---|
| 148 | berSeq++; |
---|
| 149 | berStruct.sequenceNumber = htons(berSeq); |
---|
| 150 | berStruct.nodeID_tx = (0xFF & phypkt_srcAddr); |
---|
| 151 | berStruct.nodeID_rx = (0xFF & phypkt_dstAddr); |
---|
| 152 | berStruct.mac_seqNum = htons(phypkt_seqNum); |
---|
| 153 | |
---|
| 154 | if(debug > 1) printf("BER Results: Seq=%d, TxNode=%d, RxNode=%d, BER: %d / %d = %f\n", phypkt_seqNum, berStruct.nodeID_tx, berStruct.nodeID_rx, berStruct.bits_errors, berStruct.bits_rx, (float)( (float)berStruct.bits_errors / (float)(berStruct.bits_rx))); |
---|
| 155 | SendStructToServer(&berStruct); |
---|
| 156 | |
---|
| 157 | //Invalidate the stored seqNums, to avoid bogus comparisons when sequence numbers wrap and Rx isn't receiving anything |
---|
| 158 | phypkt_seq_nums[phypkt_dstAddr] = -1; |
---|
| 159 | phypkt_seq_nums[phypkt_srcAddr] = -1; |
---|
| 160 | } |
---|
| 161 | else { |
---|
| 162 | printf("BER Calculation Failed! (rv=%d)\n", rv); |
---|
| 163 | return; |
---|
| 164 | } |
---|
| 165 | } |
---|
| 166 | |
---|
| 167 | return; |
---|
| 168 | } |
---|
| 169 | |
---|
| 170 | int CalculateBER(void *bufA, void *bufB, int length, warpnetObserveBER *berResult) |
---|
| 171 | { |
---|
| 172 | int i, totalErrors; |
---|
| 173 | unsigned char byteA, byteB, byteComp; |
---|
| 174 | |
---|
| 175 | totalErrors = 0; |
---|
| 176 | |
---|
| 177 | if(debug > 1) printf("CalculateBER: errors in bytes: "); |
---|
| 178 | for(i = 0; i<length; i++) |
---|
| 179 | { |
---|
| 180 | byteA = *( (unsigned char *)(bufA + i)); |
---|
| 181 | byteB = *( (unsigned char *)(bufB + i)); |
---|
| 182 | |
---|
| 183 | byteComp = (byteA ^ byteB); |
---|
| 184 | |
---|
| 185 | totalErrors += ones_in_chars[byteComp]; |
---|
| 186 | if(debug > 1) if(byteComp > 0) printf("%d ", i); |
---|
| 187 | } |
---|
| 188 | if(debug > 1) if(totalErrors == 0) printf("none."); |
---|
| 189 | if(debug > 1) printf("\n"); |
---|
| 190 | |
---|
| 191 | //Endian-swap the values before storing them in the struct (saves having to do it before sending the struct to the server) |
---|
| 192 | berResult->bits_rx = htonl(length*8); |
---|
| 193 | berResult->bits_errors = htonl(totalErrors); |
---|
| 194 | |
---|
| 195 | return 0; |
---|
| 196 | } |
---|
| 197 | |
---|
| 198 | void SendStructToServer(void *theStruct) { |
---|
| 199 | |
---|
| 200 | unsigned char structID; |
---|
| 201 | warpnetObserveBER* aBerStruct; |
---|
| 202 | |
---|
| 203 | int structLen = 0; |
---|
| 204 | int rv; |
---|
| 205 | |
---|
| 206 | structID = *( (unsigned char *)theStruct); |
---|
| 207 | void* txPktPtr; |
---|
| 208 | |
---|
| 209 | switch(structID) |
---|
| 210 | { |
---|
| 211 | case STRUCTID_OBSERVE_BER: |
---|
| 212 | aBerStruct = (warpnetObserveBER *)theStruct; |
---|
| 213 | txEthPktHdr.ethType = WARPNET_ETHTYPE_NODE2SVR; |
---|
| 214 | txEthPktHdr.pktLength = sizeof(warpnetEthernetPktHeader) + sizeof(warpnetControllerGroup) + sizeof(warpnetObserveBER); |
---|
| 215 | txEthPktHdr.numStructs = 1; |
---|
| 216 | txEthPktHdr.seqNum = (unsigned char)((aBerStruct->sequenceNumber)&0xFF); |
---|
| 217 | structLen = sizeof(warpnetObserveBER); |
---|
| 218 | break; |
---|
| 219 | |
---|
| 220 | default: |
---|
| 221 | printf("SendStructToServer: Unknown structID! (%d)\n", structID); |
---|
| 222 | break; |
---|
| 223 | } |
---|
| 224 | |
---|
| 225 | if(structLen > 0) |
---|
| 226 | { |
---|
| 227 | txPktPtr = &txEthPktBuf; |
---|
| 228 | memcpy(txPktPtr, &txEthPktHdr, sizeof(warpnetEthernetPktHeader)); |
---|
| 229 | memcpy(txPktPtr+sizeof(warpnetEthernetPktHeader), &groupStruct, sizeof(warpnetControllerGroup)); |
---|
| 230 | memcpy(txPktPtr+sizeof(warpnetEthernetPktHeader)+sizeof(warpnetControllerGroup), theStruct, structLen); |
---|
| 231 | |
---|
| 232 | rv = pcap_inject(pcap_handle, (void *)txPktPtr, (txEthPktHdr.pktLength) ); |
---|
| 233 | |
---|
| 234 | if(rv < 0){ |
---|
| 235 | pcap_perror(pcap_handle, ""); |
---|
| 236 | printf("Error on pcap_inject!\n"); |
---|
| 237 | } |
---|
| 238 | } |
---|
| 239 | |
---|
| 240 | return; |
---|
| 241 | } |
---|
| 242 | |
---|
| 243 | //This code is based on the excellent libpcap examples at http://www.tcpdump.org/pcap.htm |
---|
| 244 | int SetupPCAP() |
---|
| 245 | { |
---|
| 246 | char errbuf[PCAP_ERRBUF_SIZE]; // Error string |
---|
| 247 | struct bpf_program fp; // The compiled filter |
---|
| 248 | char filter_exp[19]; |
---|
| 249 | bpf_u_int32 mask; // Our netmask |
---|
| 250 | bpf_u_int32 net; // Our IP |
---|
| 251 | |
---|
| 252 | fprintf(stdout, "Attaching to device %s\n", pcap_device); |
---|
| 253 | |
---|
[1573] | 254 | sprintf((char *)&filter_exp, "ether proto 0x%04x", WARPNET_ETHTYPE_NODE2BER); |
---|
[1567] | 255 | |
---|
| 256 | //Lookup properties for the network device |
---|
| 257 | // pcap_device is a global variable, defined either by default or as a cmd line argument |
---|
| 258 | if (pcap_lookupnet(pcap_device, &net, &mask, errbuf) == -1) |
---|
| 259 | { |
---|
| 260 | fprintf(stderr, "PCAP: Couldn't get netmask for device %s: %s\n", pcap_device, errbuf); |
---|
| 261 | net = 0; |
---|
| 262 | mask = 0; |
---|
| 263 | } |
---|
| 264 | |
---|
| 265 | // Open the session in promiscuous mode with a big enough buffer |
---|
| 266 | pcap_handle = pcap_open_live(pcap_device, 65535, 1, 100, errbuf); |
---|
| 267 | |
---|
| 268 | if (pcap_handle == NULL) |
---|
| 269 | { |
---|
| 270 | fprintf(stderr, "PCAP: Couldn't open device %s: %s\n", pcap_device, errbuf); |
---|
| 271 | return -2; |
---|
| 272 | } |
---|
| 273 | |
---|
| 274 | // Compile and apply the filter |
---|
| 275 | if (pcap_compile(pcap_handle, &fp, filter_exp, 0, net) == -1) |
---|
| 276 | { |
---|
| 277 | fprintf(stderr, "PCAP: Couldn't parse filter %s: %s\n", filter_exp, pcap_geterr(pcap_handle)); |
---|
| 278 | return -2; |
---|
| 279 | } |
---|
| 280 | |
---|
| 281 | if (pcap_setfilter(pcap_handle, &fp) == -1) |
---|
| 282 | { |
---|
| 283 | fprintf(stderr, "PCAP: Couldn't install filter %s: %s\n", filter_exp, pcap_geterr(pcap_handle)); |
---|
| 284 | return -2; |
---|
| 285 | } |
---|
| 286 | |
---|
| 287 | fprintf(stdout, "Applied filter '%s'\n", filter_exp); |
---|
| 288 | |
---|
| 289 | //Loop forever on received Ethernet frames |
---|
| 290 | pcap_loop(pcap_handle, -1, (pcap_handler)ProcessWARPnodeMessage, NULL); |
---|
| 291 | |
---|
| 292 | return 0; |
---|
| 293 | } |
---|
| 294 | |
---|
| 295 | int main(int argc, char *argv[]) |
---|
| 296 | { |
---|
| 297 | printf("Starting WARPnet BER Processor\n"); |
---|
| 298 | |
---|
[1574] | 299 | int i; |
---|
| 300 | |
---|
[1567] | 301 | //Default number of WARP nodes; can be overridden on the command line |
---|
| 302 | numWARPnodes = 2; |
---|
| 303 | |
---|
| 304 | //Default Ethernet interface (enX on OS X/BSD, probably ethX on Linux) |
---|
| 305 | // User running this app must have read/write access for /dev/bpf* (OS X/BSD) or raw Ethernet (Linux) |
---|
| 306 | pcap_device = "en0"; |
---|
| 307 | |
---|
| 308 | //Use a big node ID, to avoid any conflict with actual WARP node IDs |
---|
| 309 | myID = 99; |
---|
| 310 | |
---|
| 311 | berSeq = 0; |
---|
| 312 | berStruct.structID = STRUCTID_OBSERVE_BER; |
---|
| 313 | berStruct.nodeID = myID; |
---|
| 314 | |
---|
| 315 | groupStruct.controllerID = 0; |
---|
| 316 | groupStruct.controllerGrp = 0; |
---|
| 317 | groupStruct.access = 1; |
---|
| 318 | groupStruct.reserved0 = 0; |
---|
| 319 | |
---|
| 320 | txEthPktHdr.ethType = WARPNET_ETHTYPE_NODE2SVR; |
---|
| 321 | txEthPktHdr.srcAddr[0]=0x00; |
---|
| 322 | txEthPktHdr.srcAddr[1]=0x50; |
---|
| 323 | txEthPktHdr.srcAddr[2]=0xC2; |
---|
| 324 | txEthPktHdr.srcAddr[3]=0x63; |
---|
| 325 | txEthPktHdr.srcAddr[4]=0x3F; |
---|
| 326 | txEthPktHdr.srcAddr[5]=0x80+myID; |
---|
| 327 | |
---|
[1575] | 328 | //Replace with MAC address of your WARPnet server! |
---|
| 329 | txEthPktHdr.dstAddr[0]=0xff; |
---|
| 330 | txEthPktHdr.dstAddr[1]=0xff; |
---|
| 331 | txEthPktHdr.dstAddr[2]=0xff; |
---|
| 332 | txEthPktHdr.dstAddr[3]=0xff; |
---|
| 333 | txEthPktHdr.dstAddr[4]=0xff; |
---|
| 334 | txEthPktHdr.dstAddr[5]=0xff; |
---|
[1574] | 335 | |
---|
| 336 | for(i=0; i<numWARPnodes; i++) |
---|
| 337 | { |
---|
| 338 | pktBufs[i] = (unsigned char *)malloc((size_t)MAX_ETHPKTSIZE); |
---|
| 339 | } |
---|
[1567] | 340 | |
---|
| 341 | //Sets up the PCAP capture interface |
---|
| 342 | // This function blocks, processing Ethernet packets forever |
---|
| 343 | SetupPCAP(); |
---|
| 344 | |
---|
| 345 | return 0; |
---|
| 346 | } |
---|