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 |
---|
59 | // 12-13: Ethertype = WARPNET_ETHTYPE_NODE2BER |
---|
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 | |
---|
254 | sprintf((char *)&filter_exp, "ether proto 0x%04x", WARPNET_ETHTYPE_NODE2BER); |
---|
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 | |
---|
299 | int i; |
---|
300 | |
---|
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 | |
---|
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; |
---|
335 | |
---|
336 | for(i=0; i<numWARPnodes; i++) |
---|
337 | { |
---|
338 | pktBufs[i] = (unsigned char *)malloc((size_t)MAX_ETHPKTSIZE); |
---|
339 | } |
---|
340 | |
---|
341 | //Sets up the PCAP capture interface |
---|
342 | // This function blocks, processing Ethernet packets forever |
---|
343 | SetupPCAP(); |
---|
344 | |
---|
345 | return 0; |
---|
346 | } |
---|