marte_layer.cc
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           WMPSNIFFER          --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V7.0B  11/05/10
00005  *
00006  *
00007  *  File: marte_layer.cc
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2012, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 
00037 #include <cstdio>
00038 #include <unistd.h>
00039 #include <pthread.h>
00040 #include <sys/types.h>
00041 #include <sys/stat.h>
00042 #include <sys/socket.h>
00043 #include <sys/ioctl.h>
00044 #include <netdb.h>
00045 #include <netinet/in.h>
00046 #include <netinet/ether.h>
00047 #include <sys/utsname.h>
00048 #include <net/if.h>
00049 #include <fcntl.h>
00050 #include <linux/if_packet.h>
00051 #include <linux/if_ether.h>
00052 #include <time.h>
00053 #include <cstring>
00054 #include <signal.h>
00055 #include <cstdlib>
00056 #include <map>
00057 #include "marte_layer.h"
00058 #if 0
00059 #define DEBUG(x,args...) printf("%s: " x, __func__ , ##args)
00060 #else
00061 #define DEBUG(x,args...)
00062 #endif
00063 
00064 #define LOGGER_PROTOCOL 0x1010
00065 #define OUTPUT_FILE "output_file.log"
00066 #define NIC_INTERFACE "eth1"
00067 
00074 #define ETH_MIN_PAYLOAD 46
00075 
00076 typedef struct {
00077         struct ethhdr header;
00078         unsigned char less_than_min;
00079         unsigned char data[ETH_DATA_LEN - 1 ];
00080 } __attribute__((__packed__)) eth_frame_t;
00081 
00082 static int sock;
00083 static bool marte_keep_running;
00084 static int pfd[2];
00085 
00086 void marte_layer_close() {
00087                 fprintf(stdout,"Finishing the logger ");
00088                 marte_keep_running = false;
00089                 /* to unlock the select */
00090                 char dsblq=0;
00091                 if (write(pfd[1],&dsblq,1)==-1){
00092                         fprintf(stdout,"Problem closing logger");
00093                 }
00094 }
00095 
00096 int err;
00097 char device[]= "eth1";//NIC_INTERFACE;
00098 struct sockaddr_ll host_addr;
00099 struct ifreq ifr;
00100 ssize_t nbytes;
00101 size_t written_bytes;
00102 eth_frame_t receive_buffer;
00103 
00104 int marte_layer_init(int nnodes) {
00105         fprintf(stderr,"Setting up MaRTE socket\n");
00106         sock=socket(PF_PACKET,SOCK_RAW,htons(LOGGER_PROTOCOL));
00107         if (sock < 0){
00108                 fprintf(stderr,"Error Creating raw socket\n");
00109                 return -1;
00110         }
00111         memset(&ifr, 0, sizeof(struct ifreq));
00112         fprintf(stderr,"Marte Sniffer Using interface %s\n",device);
00113 
00114         strcpy(ifr.ifr_name, device);
00115         err = ioctl(sock, SIOCGIFHWADDR, &ifr);
00116         if (err < 0){
00117                 fprintf(stderr,"Error SIOCGIFHWADDR\n");
00118                 return -1;
00119         }
00120         memcpy(host_addr.sll_addr, ifr.ifr_addr.sa_data, ETH_ALEN);
00121         err = ioctl(sock, SIOGIFINDEX, &ifr);
00122         if (err < 0) {
00123                 fprintf(stderr,"Error SIOGIFINDEX\n");
00124                 return -1;
00125         }
00126         host_addr.sll_ifindex = ifr.ifr_ifindex;
00127         host_addr.sll_family = AF_PACKET;
00128         host_addr.sll_protocol = htons(LOGGER_PROTOCOL);
00129         host_addr.sll_halen = ETH_ALEN;
00130         bind(sock, (struct sockaddr *)&host_addr, sizeof(host_addr));
00131 
00132         marte_keep_running = true;
00133         if (pipe(pfd) == -1) {
00134                 perror("pipe");
00135                 exit(EXIT_FAILURE);
00136         }
00137         return 1;
00138 }
00139 
00140 int marte_sniff_packet(char * data, simData_Hdr & sd, unsigned long long &time_us, std::map<int,robo_pose_t> & poses) {
00141         while (marte_keep_running) {
00142 
00143                 fd_set set1;
00144                 FD_ZERO(&set1);
00145                 FD_SET(sock, &set1);
00146                 FD_SET(pfd[0], &set1);
00147                 select(pfd[0] + 1, &set1, NULL, NULL, 0);
00148                 if (FD_ISSET(sock, &set1) ) {
00149                         nbytes = recvfrom(sock, &receive_buffer, sizeof(receive_buffer), 0,     NULL, 0);
00150                         if (nbytes < 0){
00151                                 fprintf(stderr," *** WARNING: Error reading from interface %s\n",device);
00152                         }
00153                 } else{
00154                         continue;
00155                 }
00156                 if (receive_buffer.less_than_min == 0) {
00157                         nbytes = nbytes - ETH_HLEN - 1;
00158                 }
00159 
00160                 remote_data_t * rd = (remote_data_t * ) receive_buffer.data;
00161                 char * payload = (char *) (&receive_buffer.data[0] + sizeof(remote_data_t));
00162                 sd.data_src = 32;
00163                 time_us = rd->time;
00164                 sd.len = rd->size;
00165                 sd.is_wmp = rd->type;
00166                 if (sd.is_wmp){
00167 
00168                         memcpy(data, payload, rd->size);
00169                         wmpFrame * p = (wmpFrame*) data;
00170                         sd.frame_type = SP_LUS_WMP_FRAME;
00171                         poses[p->hdr.from].reached=true;
00172                 }else{
00173                         sd.frame_type = SP_FOREIGN;
00174                 }
00175                 return rd->size;
00176         }
00177         close(sock);
00178         fprintf(stdout,"OK!\n");
00179         return 0;
00180 }
00181 


ros_rt_wmp_sniffer
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:57