faspplugin/faspsimul.cxx (r4864/r4189)

Go to the documentation of this file.

#include <arpa/inet.h>
#include <linux/if_packet.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <stdlib.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <net/if.h>
#include <netinet/ether.h>

#define BUF_SIZ 1024

uint8_t dest_mac[6] = { 0xb8, 0xac, 0x6f, 0x92, 0x2b, 0x34 };

const int FaspBlockSize = 44;
const int FaspSyncPos = 34;


double Gauss_Rnd(double mean, double sigma)
{
   double x, y, z;
   z = 1.* rand() / RAND_MAX;
   y = 1.* rand() / RAND_MAX;
   x = z * 6.28318530717958623;
   return mean + sigma*sin(x)*sqrt(-2*log(y));
}

int main(int argc, char *argv[])
{
   int sockfd;
   struct ifreq if_idx;
   struct ifreq if_mac;
   int tx_len = 0;
   bool debug = false;
   char sendbuf[BUF_SIZ], recvbuf[BUF_SIZ];

   struct sockaddr_ll socket_address;
   char ifName[IFNAMSIZ];

    if (argc<3) {
       printf("FASP simulatror. usage: faspsimul ifname mac\n");
       printf("example: faspsimul eth0 BC:30:5B:E8:9B:86\n");
       return 1;
    }

   // Get interface name
   strcpy(ifName, argv[1]);

   if ((argc > 2) && (strlen(argv[2])>=17)) {
      unsigned ddd[6];
      sscanf(argv[2], "%x:%x:%x:%x:%x:%x", ddd, ddd+1, ddd+2, ddd+3, ddd+4,ddd+5);
      for (int n=0;n<6;n++) dest_mac[n] = ddd[n];
   } else {
      printf("destination MAC not specified - fail\n");
      return 1;
   }

   // Open RAW socket to send on
   if ((sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL) /*IPPROTO_RAW*/)) == -1) {
      printf("Failure - cannot open raw socket\n");
      return 1;
   }

   // Get the index of the interface to send on
   memset(&if_idx, 0, sizeof(struct ifreq));
   strncpy(if_idx.ifr_name, ifName, IFNAMSIZ-1);
   if (ioctl(sockfd, SIOCGIFINDEX, &if_idx) < 0) {
      printf("SIOCGIFINDEX - cannot het if index\n");
      return 1;
   }
   // Get the MAC address of the interface to send on
   memset(&if_mac, 0, sizeof(struct ifreq));
   strncpy(if_mac.ifr_name, ifName, IFNAMSIZ-1);
   if (ioctl(sockfd, SIOCGIFHWADDR, &if_mac) < 0) {
      printf("SIOCGIFHWADDR - cannot get own MAC\n");
      return 1;
   }

   /* Index of the network device */
   socket_address.sll_family = PF_PACKET;
   socket_address.sll_protocol = htons(ETH_P_IP);
   socket_address.sll_ifindex = if_idx.ifr_ifindex;
   /* Address length*/
   socket_address.sll_halen = ETH_ALEN;
   /* Destination MAC */
   socket_address.sll_hatype = ARPHRD_ETHER;
   socket_address.sll_pkttype = PACKET_OTHERHOST;
   memcpy(socket_address.sll_addr, dest_mac, 6);

   printf("Start sniffing on iface: %s for packets from MAC: %s\n", argv[1], argv[2]);
  
   unsigned sync_tm(0);
   unsigned adc_tm(0);

   int cnt(100000000), cnt1(0);
   
   unsigned sync_num(1);
   int sync_repeat(3);
   bool first_sync_in_loop = true;

   while (cnt--> 0) {

      socklen_t addr_len = sizeof(struct sockaddr_ll);

//      size_t res = recvfrom(sockfd, recvbuf, BUF_SIZ, MSG_DONTWAIT,
//            (struct sockaddr*)&socket_address, &addr_len);

      size_t res = recvfrom(sockfd, recvbuf, BUF_SIZ, 0, 0, 0);

      if (res == -1) {
         if (errno != EAGAIN)
            printf("error %d  EAGAIN %d\n", errno, EAGAIN);
         else
            usleep(1000);
      } else
      if (res>0) {

         struct ether_header *eh2 = (struct ether_header *) recvbuf;

         bool is_my = (res==60);
         for (int cnt=0;cnt<6 && is_my;cnt++)
            if ((unsigned) eh2->ether_shost[cnt] != (unsigned) dest_mac[cnt]) is_my = false;

         if (is_my) {
            if (cnt1++ % 10000 == 0) printf("Recv %d packets\n", cnt1);
            if (debug)
               printf("Recv packet of size %3d  src: %02x:%02x:%02x:%02x:%02x:%02x    %x  %x  %x\n", (int) res,
                     eh2->ether_shost[0], eh2->ether_shost[1], eh2->ether_shost[2],
                     eh2->ether_shost[3], eh2->ether_shost[4], eh2->ether_shost[5]);
            memcpy(sendbuf, recvbuf+6, 6);
            memcpy(sendbuf+6, recvbuf, 6);
            memcpy(sendbuf+12, recvbuf+12, res-12);
            uint8_t* data = ((uint8_t*) sendbuf) + 14;
            
            
//            for (int n=0;n<46;n++) data[n] = n;
            // reset adc data
            for (int n=0;n<16;n++) {
               unsigned adc = (unsigned) Gauss_Rnd(230 + n*4, 50 - 2*n);
               data[n*2] = (adc >> 2) & 0xff;
               data[n*2+1] = adc & 0x3;
            }
            
            if (first_sync_in_loop) {
              adc_tm = (sync_tm - (unsigned) Gauss_Rnd(200, 30)/50) ;
            } else {
               adc_tm = sync_tm + 100 + (unsigned) (1000*rand()/RAND_MAX);
            }
            
//            printf("produce sync %u sync_tm %u adc_tm %u \n", sync_num, sync_tm, adc_tm);

            // set sync number
            data[FaspSyncPos] = (sync_num >> 16) & 0xff;
            data[FaspSyncPos+1] = (sync_num >> 8) & 0xff;
            data[FaspSyncPos+2] = sync_num & 0xff;

            data[FaspSyncPos+4] = (adc_tm >> 16) & 0xff;
            data[FaspSyncPos+5] = (adc_tm >> 8) & 0xff;
            data[FaspSyncPos+6] = adc_tm & 0xff;

            data[FaspSyncPos+7] = (sync_tm >> 16) & 0xff;
            data[FaspSyncPos+8] = (sync_tm >> 8) & 0xff;
            data[FaspSyncPos+9] = sync_tm & 0xff;

//            printf("produce sync %u \n", sync_num);

            // change sync id every third packet

            first_sync_in_loop = false;
            if (--sync_repeat <= 0) {
                do {
                   sync_num++;
                } while ((sync_repeat = (int) (30.* rand() / RAND_MAX)) > 20);
                sync_tm = (unsigned) rand() & 0xffffff;
                first_sync_in_loop = true;
             }

            // sync_num++;
            
            if (sendto(sockfd, sendbuf, 60, 0, (struct sockaddr*)&socket_address, sizeof(socket_address))<0)
               printf("Fail to reply\n");
            //else
            //   printf("Reply on the packet\n");
          }
      } else
         usleep(1000);
   }

   close(sockfd);

   return 0;
}