iCub-main
Loading...
Searching...
No Matches
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;
80extern const eODeb_eoProtoParser_cfg_t *deb_eoParserCfg_ptr;
81FILE *file;
82
83
84
85// --------------------------------------------------------------------------------------------------------------------
86// - typedef with internal scope
87// --------------------------------------------------------------------------------------------------------------------
88// empty-section
89
90
91// --------------------------------------------------------------------------------------------------------------------
92// - declaration of static functions
93// --------------------------------------------------------------------------------------------------------------------
94static void s_process_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet);
95static void s_ethLowLevelPraser_configure(void);
96static 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// --------------------------------------------------------------------------------------------------------------------
108int 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// --------------------------------------------------------------------------------------------------------------------
290static 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
392static 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
459static 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
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
main
int main()
Definition main.cpp:67
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 Fri Sep 26 2025 14:18:33 for iCub-main by doxygen 1.9.8