iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3  * Author: Valentina Gaggero
4  * email: valentina.gaggero@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 /* @file main.cpp
20  @brief
21  @author valentina.gaggero@iit.it
22  @date 03/19/2013
23 **/
24 
25 // --------------------------------------------------------------------------------------------------------------------
26 // - external dependencies
27 // --------------------------------------------------------------------------------------------------------------------
28 
29 #include <unistd.h>
30 #include <stdio.h>
31 
32 //#include <pcap.h>
33 //#include "header.h"
34 //#include "debugFunctions.h"
35 
36 // ACE stuff
37 #include <ace/ACE.h>
38 #include "ace/SOCK_Dgram.h"
39 #include "ace/Addr.h"
40 
41 //yarp stuff
42 //#include <yarp/os/Time.h>
43 //using namespace yarp::os;
44 
45 //embody stuff
46 #include "EoCommon.h"
47 #include "EoMotionControl.h"
48 #include "eODeb_eoProtoParser.h"
49 #include "eOtheEthLowLevelParser.h"
50 //pcap stuff
51 #include "pcap_wrapper_linux.h"
52 #include "stdio.h"
53 
54 
55 
56 
57 // --------------------------------------------------------------------------------------------------------------------
58 // - declaration of extern public interface
59 // --------------------------------------------------------------------------------------------------------------------
60 
61 
62 
63 
64 // --------------------------------------------------------------------------------------------------------------------
65 // - declaration of extern hidden interface
66 // --------------------------------------------------------------------------------------------------------------------
67 
68 
69 
70 
71 // --------------------------------------------------------------------------------------------------------------------
72 // - #define with internal scope
73 // --------------------------------------------------------------------------------------------------------------------
74 #define MAX_ACQUISITION 1000000000L
75 
76 // --------------------------------------------------------------------------------------------------------------------
77 // - definition (and initialisation) of extern variables, but better using _get(), _set()
78 // --------------------------------------------------------------------------------------------------------------------
79 //extern const eODeb_eoProtoParser_cfg_t deb_eoParserCfg;
80 extern const eODeb_eoProtoParser_cfg_t *deb_eoParserCfg_ptr;
81 FILE *file;
82 
83 
84 
85 // --------------------------------------------------------------------------------------------------------------------
86 // - typedef with internal scope
87 // --------------------------------------------------------------------------------------------------------------------
88 // empty-section
89 
90 
91 // --------------------------------------------------------------------------------------------------------------------
92 // - declaration of static functions
93 // --------------------------------------------------------------------------------------------------------------------
94 static void s_process_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet);
95 static void s_ethLowLevelPraser_configure(void);
96 static void print_help(void);
97 
98 
99 
100 // --------------------------------------------------------------------------------------------------------------------
101 // - definition (and initialisation) of static variables
102 // --------------------------------------------------------------------------------------------------------------------
103 
104 
105 // --------------------------------------------------------------------------------------------------------------------
106 // - definition of extern public functions
107 // --------------------------------------------------------------------------------------------------------------------
108 int main(int argc, char *argv[])
109 {
110  // pcap_t *handle;
111  // char errbuf[PCAP_ERRBUF_SIZE];
112  // struct bpf_program fp; /* The compiled filter expression */
113  char filter_exp[260]; /* The filter expression */
114  char filtersrc[20];
115  bool filterOnSrc = false;
116  char filterdst[20];
117  bool filterOnDst = false;
118  char dev[10];
119  uint64_t maxacquisition = MAX_ACQUISITION;
120  char default_filter_exp[60];
121 
122 
123 
124  struct pcap_pkthdr header; /* The header that pcap gives us */
125  const u_char *packet; /* The actual packet */
126 
127 
128  //1) init data
129  sprintf(dev, "eth1");
130  sprintf(filter_exp, "src host 10.0.1.104");
131 // sprintf(filter_exp, " ");
132 
133  //2) parse arguments
134  if(argc>1)
135  {
136  for(uint8_t i = 0; i<argc; i++)
137  {
138  if(strcmp("--help", argv[i]) == 0)
139  {
140  print_help();
141  return(0);
142  }
143 
144  if(strcmp("--dev", argv[i]) == 0)
145  {
146  if(i<(argc-1))
147  {
148  sprintf(dev, "%s", argv[++i]);
149  }
150  continue;
151  }
152 
153  if(strcmp("--filterSrcAddr", argv[i]) == 0)
154  {
155  if(i<(argc-1))
156  {
157  filterOnSrc = true;
158  sprintf(filtersrc, "%s", argv[++i]);
159  }
160  continue;
161  }
162 
163 
164  if(strcmp("--filterDstAddr", argv[i]) == 0)
165  {
166  if(i<(argc-1))
167  {
168  filterOnDst = true;
169  sprintf(filterdst, "%s", argv[++i]);
170  }
171  continue;
172  }
173 
174  if(strcmp("--maxacquisition", argv[i]) == 0)
175  {
176  if(i<(argc-1))
177  {
178  maxacquisition = atoll(argv[++i]);
179  }
180  continue;
181  }
182 
183  }
184  }
185 
186  file = fopen("test.txt", "a+");
187  if(NULL == file)
188  {
189  printf("cannot open file \n");
190  return(0);
191 
192  }
193 
194  //set filter
195  if((filterOnSrc) &&(filterOnDst))
196  {
197  sprintf(filter_exp, "src host %s and dst host %s", filtersrc, filterdst);
198 
199  }
200  else if(filterOnSrc)
201  {
202  sprintf(filter_exp, "src host %s ", filtersrc);
203 
204  }
205  else if(filterOnDst)
206  {
207  sprintf(filter_exp, "dst host %s", filterdst);
208  }
209 
210  sprintf(default_filter_exp, " and not port 4444"); //emsbackdoor
211  strcat(filter_exp, default_filter_exp);
212 
213  printf("Debug-tool: Ethernet Low Level Praser start on interface %s\n", dev);
214  printf("\t\t Used filter: %s\n", filter_exp);
215 
216 
217  //3) init pcap
218 /*
219  handle = pcap_open_live(dev, BUFSIZ, 0, TIMEOUT_MS, errbuf); // 0 = non promiscuo
220  if (handle == NULL)
221  {
222  printf("Sorry, occured problem while opening pcap\n");
223  //fprintf(stderr, "Couldn't open device %s: %s\n", dev, errbuf);
224  return(2);
225  }
226 
227  // make sure we're capturing on an Ethernet device [2]
228  if (pcap_datalink(handle) != DLT_EN10MB)
229  {
230  printf("Sorry, pcap has been open on a NOT ethernet device\n");
231  //fprintf(stderr, "%s is not an Ethernet\n", dev);
232  exit(EXIT_FAILURE);
233  }
234 
235  if (pcap_compile(handle, &fp, filter_exp, 0, net) == -1)
236  {
237  printf("Sorry, pcap can't paser filter%s: %s\n", filter_exp, pcap_geterr(handle));
238  //fprintf(stderr, "Couldn't parse filter %s: %s\n", filter_exp, pcap_geterr(handle));
239  return(2);
240  }
241 
242  if (pcap_setfilter(handle, &fp) == -1)
243  {
244  printf("Sorry, pcap couldn't install filter %s: %s\n", filter_exp, pcap_geterr(handle));
245  // fprintf(stderr, "Couldn't install filter %s: %s\n", filter_exp, pcap_geterr(handle));
246  return(2);
247  }
248 */
249  if(wrapperPcap_init(dev, filter_exp)==0)
250  {
251  printf("error in inti libpacap\n");
252  return(1);
253  }
254 
255  //4)init low level parser objs
256  s_ethLowLevelPraser_configure();
257 
258  //5)start to acquire packets
259 /*
260  pcap_loop(handle, maxacquisition, s_process_packet, NULL); // Get into the loop
261 
262 */
263  printf("start\n");
264  wrapperPcap_loop(maxacquisition, s_process_packet, NULL);
265 
266  printf("\nCapture complete.\n");
267 
268  //6) close the session
269  wrapperPcap_close();
270  fclose(file);
271 
272  return(0); // everything ok
273 }
274 
275 
276 
277 
278 
279 
280 
281 // --------------------------------------------------------------------------------------------------------------------
282 // - definition of extern hidden functions
283 // --------------------------------------------------------------------------------------------------------------------
284 
285 
286 
287 // --------------------------------------------------------------------------------------------------------------------
288 // - definition of static functions
289 // --------------------------------------------------------------------------------------------------------------------
290 static void s_process_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet)
291 {
292 // printf("process pkt\n");
293  //eOTheEthLowLevParser_DissectPacket(eo_ethLowLevParser_GetHandle, packet);
294 
296  eOethLowLevParser_packetInfo_t pktInfo;
297  eOresult_t res;
298 
299  res = eOTheEthLowLevParser_GetUDPdatagramPayload(eo_ethLowLevParser_GetHandle(), (uint8_t*)packet, &pktInfo);
300  if(eores_OK != res)
301  {
302  return;
303  }
304  eODeb_eoProtoParser_RopFrameDissect(eODeb_eoProtoParser_GetHandle(), &pktInfo);
305 }
306 
307 
308 //static void my_cbk_onErrorSeqNum(eOethLowLevParser_packetInfo_t *pktInfo_ptr, uint32_t rec_seqNum, uint32_t expected_seqNum)
309 //{
310 //
311 // printf("ERR in SEQNUM; rec=%d expected=%d\n",rec_seqNum, expected_seqNum );
312 //return;
313 //}
314 //
315 //static void my_cbk_onNVfound(eOethLowLevParser_packetInfo_t *pktInfo_ptr, eODeb_eoProtoParser_ropAdditionalInfo_t *ropAddInfo_ptr)
316 //{
317 //
318 // printf("NV found!!: ep=%x id=%x\n", ropAddInfo_ptr->desc.ep, ropAddInfo_ptr->desc.id);
319 // return;
320 //}
321 //
322 //static void my_cbk_onNVsetpointFound(eOethLowLevParser_packetInfo_t *pktInfo_ptr, eODeb_eoProtoParser_ropAdditionalInfo_t *ropAddInfo_ptr)
323 //{
324 // eOmc_setpoint_t * setpoint_ptr = (eOmc_setpoint_t *)ropAddInfo_ptr->desc.data;
325 // uint8_t board = 0, j;
326 // float enc_factor, zero, enc_factor_6=182.044 , enc_factor_8=182.044, zero_6=180, zero_8=-180;
327 //
328 // //printf("NV found!!: ep=%x id=%x\n", ropAddInfo_ptr->desc.ep, ropAddInfo_ptr->desc.id);
329 //
330 // if(ropAddInfo_ptr->desc.ep == 0x18)
331 // {
332 // board = 8;
333 // enc_factor = enc_factor_8;
334 // zero = zero_8;
335 // }
336 // else if(ropAddInfo_ptr->desc.ep == 0x16)
337 // {
338 // board = 6;
339 // enc_factor = enc_factor_6;
340 // zero = zero_6;
341 // }
342 // else
343 // {
344 // printf("\n\n ERROR: un expected ep!!! %d \n ", ropAddInfo_ptr->desc.ep);
345 // return;
346 // }
347 //
348 // if(setpoint_ptr->type != eomc_setpoint_position)
349 // {
350 // printf("ERR: no setpoint position. typse= %d", setpoint_ptr->type);
351 // return;
352 // }
353 //
354 // switch(ropAddInfo_ptr->desc.id)
355 // {
356 // case 0xbc1a:
357 // {
358 // j = 0;
359 // }break;
360 //
361 // case 0xbc3a:
362 // {
363 // j = 1;
364 // }break;
365 //
366 // case 0xbc5a:
367 // {
368 // j = 2;
369 // }break;
370 //
371 // case 0xbc7a:
372 // {
373 // j = 3;
374 // }break;
375 // default:
376 // {
377 // printf("ERROR: receiv unexpected nvid %x", ropAddInfo_ptr->desc.id);
378 // return;
379 // }
380 // }
381 // float vel, pos;
382 //
383 // pos = (setpoint_ptr->to.position.value/enc_factor)-zero;
384 // vel = setpoint_ptr->to.position.withvelocity/fabs(enc_factor);
385 //
386 // printf("board %d j %d pos %f (%d) vel %f (%d) enc_factor=%f zero=%f\n", board, j, pos, setpoint_ptr->to.position.value, vel, setpoint_ptr->to.position.withvelocity,enc_factor,zero);
387 //
388 // return;
389 //}
390 
391 
392 static void s_ethLowLevelPraser_configure(void)
393 {
394 
395 // //4.1) init application parser: embObjParser
396 // const eODeb_eoProtoParser_cfg_t deb_eoParserCfg =
397 // {
398 // EO_INIT(.checks)
399 // {
400 // EO_INIT(.seqNum)
401 // {
402 // EO_INIT(.cbk_onErrSeqNum) NULL, //my_cbk_onErrorSeqNum,
403 // },
404 //
405 // EO_INIT(.nv)
406 // {
407 // EO_INIT(.NVs2searchArray)
408 // {
409 // EO_INIT(.head)
410 // {
411 // EO_INIT(.capacity) eODeb_eoProtoParser_maxNV2find,
412 // EO_INIT(.itemsize) sizeof(eODeb_eoProtoParser_nvidEp_couple_t),
413 // EO_INIT(.size) 8,
414 // },
415 // EO_INIT(.data)
416 // {
417 // {0x18, 0xbc1a},
418 // {0x18, 0xbc3a},
419 // {0x18, 0xbc5a},
420 // {0x18, 0xbc7a},
421 // {0x16, 0xbc1a},
422 // {0x16, 0xbc3a},
423 // {0x16, 0xbc5a},
424 // {0x16, 0xbc7a}
425 // }
426 //
427 // },
428 // EO_INIT(.cbk_onNVfound) my_cbk_onNVsetpointFound, //my_cbk_onNVfound
429 // },
430 //
431 // EO_INIT(.invalidRopFrame) {0}
432 // }
433 // };
434 
435  eODeb_eoProtoParser_Initialise(/*&deb_eoParserCfg*/deb_eoParserCfg_ptr);
436 
437 
438  //4.2) init low level parser: eOethLowLevParser
439 /* const eOethLowLevParser_cfg_t ethLowLevParserCfg =
440  {
441  EO_INIT(.conFiltersData)
442  {
443  EO_INIT(.filtersEnable) 0,
444  EO_INIT(.filters) {0}, //use pcap filter
445  },
446 
447  EO_INIT(.appParserData)
448  {
449  EO_INIT(.func) eODeb_eoProtoParser_RopFrameDissect
450  EO_INIT(.arg) eODeb_eoProtoParser_GetHandle(),
451  }
452  };
453 */
454  //currently use thelow level parser and appl paser separately
455  const eOethLowLevParser_cfg_t ethLowLevParserCfg = {0};
456  eo_ethLowLevParser_Initialise(&ethLowLevParserCfg);
457 }
458 
459 static void print_help(void)
460 {
461 
462  printf("option --help: print this help\n");
463  printf("option --dev: specify device where start to capture (default eth1)\n");
464  printf("option --filterSrcAddr: application will capture packet with source address specified here. Not mandatory\n");
465  printf("option --filterDstAddr: application will capture packet with dest address specified here. Not mandatory\n");
466  printf("option --maxacquisition: max num of captured packets. Not mandatory\n");
467 }
468 
469 // --------------------------------------------------------------------------------------------------------------------
470 // - end-of-file (leave a blank line after)
471 // --------------------------------------------------------------------------------------------------------------------
472 
473 
474 
475 
main
int main(int argc, char *argv[])
Definition: main.cpp:31
s_ethLowLevelPraser_configure
static void s_ethLowLevelPraser_configure(void)
Definition: main.cpp:392
file
FILE * file
Definition: main.cpp:81
s_process_packet
static void s_process_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet)
Definition: main.cpp:290
print_help
static void print_help(void)
Definition: main.cpp:459
MAX_ACQUISITION
#define MAX_ACQUISITION
Definition: main.cpp:74
deb_eoParserCfg_ptr
const eODeb_eoProtoParser_cfg_t * deb_eoParserCfg_ptr
Definition: parser_cfg_calibration.cpp:146
wrapperPcap_loop
uint8_t wrapperPcap_loop(int32_t cnt, pcap_handler callback, uint8_t *user)
Definition: pcap_wrapper_linux.cpp:147
wrapperPcap_close
void wrapperPcap_close(void)
Definition: pcap_wrapper_linux.cpp:156
wrapperPcap_init
uint8_t wrapperPcap_init(char *dev, char *filter_expr)
Definition: pcap_wrapper_linux.cpp:95
pcap_wrapper_linux.h
fclose
fclose(fileID)
Generated on Thu Apr 11 2024 13:50:58 for iCub-main by doxygen 1.9.1