SHARE
TWEET

Untitled

a guest Jul 16th, 2017 85 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define _GNU_SOURCE
  2. #include <fcntl.h>
  3. #include <pthread.h>
  4. #include <dirent.h>
  5. #include <errno.h>
  6. #include <stdarg.h>
  7. #include <stdio.h>
  8. #include <stdlib.h>
  9. #include <string.h>
  10. #include <time.h>
  11. #include <unistd.h>
  12. #include <arpa/inet.h>
  13. #include <linux/if_packet.h>
  14. #include <linux/ip.h>
  15. #include <net/if.h>
  16. #include <netinet/ether.h>
  17. #include <signal.h>
  18. #include <sys/ioctl.h>
  19. #include <sys/socket.h>
  20. #include <sys/stat.h>
  21. #include <sys/types.h>
  22. #include <sys/syscall.h>
  23.  
  24.  
  25. #include <mfe_control.h>
  26. #include <mfe_control_linux_uland.h>
  27. #include <mfe_config.h>
  28. #include "libarg.h"
  29.  
  30. #define ETHERTYPE_BER           0xa5a5
  31.  
  32. #define FUNC_SENDTO         0
  33. #define FUNC_SENDMSG        1
  34. #define FUNC_SENDMMSG       2
  35.  
  36. #ifndef PACKET_QDISC_BYPASS
  37.     #define PACKET_QDISC_BYPASS 20
  38. #endif
  39.  
  40. typedef struct func_ctx func_ctx_t;
  41. typedef unsigned long long (*func_t)(struct func_ctx *);
  42.  
  43. typedef struct ftable_entry {
  44.     char *name;
  45.     func_t func;
  46.     func_t rfunc;
  47. } ftable_entry_t;
  48.  
  49. unsigned long long do_sendto(struct func_ctx *ctx);
  50. unsigned long long do_recvfrom(struct func_ctx *ctx);
  51.  
  52. #define LOG_BASE            0
  53. #define LOG_META            1
  54. #define LOG_EXTENDED        2
  55.  
  56. static int log_level;
  57.  
  58. typedef struct thread_arg {
  59.     uint8_t suppress_all;
  60.     uint8_t func_no;
  61.     char *if_name;
  62.     ssize_t fsize;
  63.     struct supp_adrs* supp_head;
  64.     func_t rfunc;
  65.     func_ctx_t *ctx;
  66.     pthread_spinlock_t lock;
  67. } thread_arg_t;
  68.  
  69. typedef struct supp_adrs {
  70.     struct ether_addr *eaddr;
  71.     struct supp_adrs *next;
  72. } supp_adrs_t;
  73.  
  74. typedef struct mac_addresses {
  75.     char *if_name;
  76.     char *if_mac;
  77.     struct mac_addresses *next;
  78. } mac_addresses_t;
  79.  
  80. struct ber_proto {
  81.     unsigned long long num;
  82.     unsigned short csum;
  83.     unsigned short tail;
  84. } __attribute__((packed));
  85.  
  86. static volatile sig_atomic_t g_stop = 0, g_alarm = 0;
  87. void sighandler(int n)
  88. {
  89.     static char buf[32];
  90.     memset(buf, 0, 32);
  91.     switch (n) {
  92.         case SIGINT:
  93.         case SIGPIPE:
  94.         case SIGHUP:
  95.         case SIGTERM:
  96.             g_stop = 1;
  97.             snprintf(buf, 32, "\nINTERRUPT SIGNAL CATCHED\n");
  98.             break;
  99.         case SIGALRM:
  100.             snprintf(buf, 32, "ALARM SIGNAL CATCHED\n");
  101.             g_alarm = 1;
  102.             break;
  103.         default:
  104.             snprintf(buf, 32, "\nsignal %d arrived\npipe", n);
  105.     };
  106.     write(0, buf, 32);
  107. }
  108.  
  109. int print_log(int level, const char *format, ...)
  110. {
  111.     int ret = 0;
  112.     va_list args;
  113.  
  114.     if (level <= log_level) {
  115.         va_start(args, format);
  116.         ret = vprintf(format, args);
  117.         va_end(args);
  118.         fflush(stdout);
  119.     }
  120.  
  121.     return ret;
  122. }
  123.  
  124. struct mac_addresses* getmactable()
  125. {
  126.     DIR *dir;
  127.     struct dirent *entry;
  128.     char *if_name, *if_mac, *p;
  129.     int fd, ret;
  130.     struct stat st;
  131.     struct mac_addresses *head = NULL, *t = NULL;
  132.  
  133.     if (!(dir = opendir("/sys/class/net/")))
  134.         return NULL;
  135.     if (!(entry = readdir(dir)))
  136.         return NULL;
  137.  
  138.     do {
  139.         if (entry->d_type == DT_LNK) {
  140.             char fname[strlen("/sys/class/net/") + strlen(entry->d_name) + strlen("/address") + 1];
  141.  
  142.             if_name = (char *)malloc(sizeof(char) * strlen(entry->d_name) + 1);
  143.             memset(if_name, 0, sizeof(char) * strlen(entry->d_name) + 1);
  144.             strcpy(if_name, entry->d_name);
  145.  
  146.             sprintf(fname, "/sys/class/net/%s/address", entry->d_name);
  147.             fd = open(fname, 0);
  148.             if (fd < 0) {
  149.                 free(if_name);
  150.                 continue;
  151.             }
  152.             ret = stat(fname, &st);
  153.             if (ret < 0) {
  154.                 free(if_name);
  155.                 continue;
  156.             }
  157.  
  158.             if_mac = (char*)malloc(sizeof(char) * st.st_size + 1);
  159.             memset(if_mac, 0, sizeof(char) * st.st_size + 1);
  160.             ret = read(fd, if_mac, st.st_size);
  161.             if (ret < 0) {
  162.                 free(if_name);
  163.                 free(if_mac);
  164.                 continue;
  165.             }
  166.             p = strchr(if_mac, '\n');
  167.             if (p)
  168.                 *p = '\0';
  169.  
  170.             if (head == NULL) {
  171.                 head = (struct mac_addresses *)malloc(sizeof(struct mac_addresses));
  172.                 t = head;
  173.             } else {
  174.                 t->next = (struct mac_addresses *)malloc(sizeof(struct mac_addresses));
  175.                 t = t->next;
  176.             }
  177.             t->if_name = if_name;
  178.             t->if_mac = if_mac;
  179.             t->next = NULL;
  180.         }
  181.     } while ((entry = readdir(dir)));
  182.     closedir(dir);
  183.  
  184.     return head;
  185. }
  186.  
  187. char* getifnamebymac(char *mac, struct mac_addresses* table)
  188. {
  189.     char *ret = NULL;
  190.  
  191.     while (table) {
  192. //      printf("'%s' ~ '%s'\n", mac, table->if_mac);
  193.         if (!strncmp(mac, table->if_mac, strlen(mac))) {
  194.             ret = table->if_name;
  195.             break;
  196.         }
  197.         table = table->next;
  198.     }
  199.  
  200.     return ret;
  201. }
  202.  
  203. int is_suppressed(supp_adrs_t *supp_head, uint8_t *addr)
  204. {
  205.     supp_adrs_t *t = supp_head;
  206.     int ret = 0;
  207.  
  208.     while (t) {
  209. #if 0
  210.     printf("%x:%x:%x:%x:%x:%x ~ %x:%x:%x:%x:%x:%x\n", t->eaddr->ether_addr_octet[0], t->eaddr->ether_addr_octet[1], t->eaddr->ether_addr_octet[2],
  211.         t->eaddr->ether_addr_octet[3], t->eaddr->ether_addr_octet[4], t->eaddr->ether_addr_octet[5], addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
  212. #endif
  213.         if (!memcmp(t->eaddr->ether_addr_octet, addr, ETH_ALEN)) {
  214.             ret = 1;
  215.             break;
  216.         }
  217.         t = t->next;
  218.     }
  219.  
  220.     return ret;
  221. }
  222.  
  223. unsigned short csum(unsigned short *buf, unsigned int n)
  224. {
  225.     unsigned short byte;
  226.     short ret;
  227.     register unsigned long sum = 0;
  228.  
  229.     while (n > 1) {
  230.         sum += *buf++;
  231.         n -= 2;
  232.     }
  233.     if (n == 1) {
  234.         byte = 0;
  235.         *((u_char*)&byte) = *(u_char*)buf;
  236.         sum += byte;
  237.  
  238.         sum += htons(*(u_char *)buf << 8);
  239.     }
  240.  
  241.     sum = (sum >> 16) + (sum & 0xffff);
  242.     sum = sum + (sum >> 16);
  243.     ret = (unsigned short)~sum;
  244.  
  245.     return ret;
  246. }
  247.  
  248. struct func_ctx {
  249.     int sockfd;
  250.     int rsockfd;
  251.     struct ether_header *ether_hdr;
  252.     int tx_len;
  253.     unsigned long long fcount;
  254.     ssize_t fsize;
  255.     struct sockaddr_ll *sockaddr;
  256.     thread_arg_t *th_arg;
  257.     int msgs_cnt;
  258.     struct iovec *iov;
  259.     unsigned char *dest_mac;
  260.     union {
  261.         uint8_t *buf;
  262.         uint8_t **msgbuf;
  263.     } b;
  264.     uint32_t time;
  265.     uint8_t type;
  266. };
  267.  
  268. int wait_snd(int sock)
  269. {
  270.     fd_set RD, WR, EXP;
  271.     struct timespec ts;
  272.     int R;
  273.  
  274.     FD_ZERO(&RD);
  275.     FD_ZERO(&WR);
  276.     FD_ZERO(&EXP);
  277.  
  278.     FD_SET(sock, &RD);
  279.  
  280.     ts.tv_sec = 0;
  281.     ts.tv_nsec = 1;
  282.  
  283. reselect:
  284.     // We assume that tmo recalculated by Linux kernel
  285.     R = pselect(sock, &RD, &WR, &EXP, &ts, NULL);
  286.     switch (R)
  287.     {
  288.     case 0:
  289.     case 1:
  290.         return R;
  291.     case -1:
  292.     default:
  293.         switch(errno)
  294.         {
  295.         case EINTR:
  296.             goto reselect;
  297.         default:
  298.             return -errno;
  299.         }
  300.     }
  301. }
  302.  
  303. inline unsigned long long do_sendto(struct func_ctx *ctx)
  304. {
  305.     unsigned long long i = 0, sent_bytes = 0, fcount = ctx->fcount;
  306.     int sockfd = ctx->sockfd, tx_len = ctx->tx_len;
  307.     ssize_t ret, fsize = ctx->fsize;
  308.     struct sockaddr_ll *sockaddr = ctx->sockaddr;
  309.     uint8_t *buf = ctx->b.buf;
  310.     struct ether_header *ether_hdr = (struct ether_header *)buf;
  311.     struct ber_proto *ber = (struct ber_proto *) &buf[sizeof(struct ether_header)];
  312. //  thread_arg_t *th_arg = ctx->th_arg;
  313.  
  314.     for (i = 0; ((i < fcount - (fcount > 2 ? 2 : 0))|| (ctx->time && !g_alarm)) && !g_stop; i++) {
  315.         ber->num = i + 1;
  316.         ber->csum = 0;
  317.         ber->tail = 0;
  318.         ber->csum = csum((unsigned short *)buf, fsize);
  319. do_send1:
  320.         ether_hdr->ether_type = ETHERTYPE_BER;
  321.         if (g_stop)
  322.             return ber->num;
  323.         ret = sendto(sockfd, buf, tx_len, /*MSG_DONTWAIT*/0, (struct sockaddr *)sockaddr, sizeof(struct sockaddr_ll));
  324.         if (ret < 0) {
  325.             if (errno == ENOBUFS || errno == EAGAIN) {
  326.                 goto do_send1;
  327.             }
  328.             fprintf(stderr, "sendto[%llu]: %s (%d)\n", i, sys_errlist[errno], errno);
  329.             ret = 1;
  330.             break;
  331.         }
  332.         sent_bytes += ret;
  333.     }
  334.  
  335.     ber->num = i + 1;
  336.     ber->csum = 0;
  337.     ber->tail = 0xE4ED;
  338.     ber->csum = csum((unsigned short *)buf, fsize);
  339. do_send2:
  340.     ret = sendto(sockfd, buf, tx_len, /*MSG_DONTWAIT*/0, (struct sockaddr *)sockaddr, sizeof(struct sockaddr_ll));
  341.     if (ret < 0) {
  342.         if (errno == ENOBUFS || errno == EAGAIN) {
  343.             usleep(300000);
  344.             goto do_send2;
  345.         } else
  346.             fprintf(stderr, "sendto[%llu]: %s (%d)\n", i, sys_errlist[errno], errno);
  347.     }
  348.     ber->num = i + 2;
  349.     ber->csum = 0;
  350.     ber->tail = 0xe6ed;
  351.     ber->csum = csum((unsigned short *)buf, fsize);
  352. do_send3:
  353.     ret = sendto(sockfd, buf, tx_len, /*MSG_DONTWAIT*/0, (struct sockaddr *)sockaddr, sizeof(struct sockaddr_ll));
  354.     print_log(LOG_EXTENDED, "-> packet[%d]: ", fsize);
  355.     for (i = 0; (ssize_t)i < fsize; i++)
  356.         print_log(LOG_EXTENDED, "%02x%c", buf[i], (ssize_t)i == fsize - 1 ? '\0' : ':');
  357.     print_log(LOG_EXTENDED, "\n");
  358.     if (ret < 0) {
  359.         if (errno == ENOBUFS || errno == EAGAIN) {
  360.             usleep(300000);
  361.             goto do_send3;
  362.         } else
  363.             fprintf(stderr, "sendto[%llu]: %s (%d)\n", i, sys_errlist[errno], errno);
  364.     }
  365.  
  366.     return ber->num;
  367. }
  368.  
  369. void* receiver(void *arg)
  370. {
  371.     int sockfd, sockopt = 1, i;
  372.     struct ifreq ifopts;
  373.     struct ifreq ifr;
  374.     uint8_t *buf = NULL;
  375.     unsigned char dest_mac[6];
  376.     unsigned long long pkt_received = 0;
  377.     struct thread_arg *th_arg = (struct thread_arg *)arg;
  378.     func_t rfunc = th_arg->rfunc;
  379.     struct func_ctx *ctx = th_arg->ctx;
  380.     sigset_t set;
  381.  
  382.     sigemptyset(&set);
  383.     sigaddset(&set, SIGINT);
  384.     if (pthread_sigmask(SIG_BLOCK, &set, NULL)) {
  385.         printf("error: cannot block SIGINT\n");
  386.         return (void *)-1;
  387.     }
  388.  
  389.     memset(&ifr, 0, sizeof(struct ifreq));
  390.     if ((sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) == -1) {
  391.         fprintf(stderr, "socket: %s\n", sys_errlist[errno]);
  392.         return (void *)-1;
  393.     }
  394.  
  395.     strncpy(ifopts.ifr_name, th_arg->if_name, IFNAMSIZ - 1);
  396.     ioctl(sockfd, SIOCGIFFLAGS, &ifopts);
  397.     ifopts.ifr_flags |= IFF_PROMISC;
  398.     if (ioctl(sockfd, SIOCSIFFLAGS, &ifopts)) {
  399.         fprintf(stderr, "ioctl(SIOCSIFFLAGS): %s: %s\n", th_arg->if_name, sys_errlist[errno]);
  400.         close(sockfd);
  401.         return (void *)-1;
  402.     }
  403.  
  404.     if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof(sockopt)) == -1) {
  405.         fprintf(stderr, "setcockopt(SO_REUSEADDR): %s: %s\n", th_arg->if_name, sys_errlist[errno]);
  406.         close(sockfd);
  407.         return (void *)-1;
  408.     }
  409.  
  410.     if (setsockopt(sockfd, SOL_SOCKET, SO_BINDTODEVICE, ifopts.ifr_name, IFNAMSIZ - 1) == -1) {
  411.         fprintf(stderr, "setsockopt(SO_BINDTODEVICE): %s: %s\n", th_arg->if_name, sys_errlist[errno]);
  412.         close(sockfd);
  413.         return (void *)-1;
  414.     }
  415.  
  416.     strncpy(ifr.ifr_name, th_arg->if_name, IFNAMSIZ - 1);
  417.     ioctl(sockfd, SIOCGIFHWADDR, &ifr);
  418.     print_log(LOG_BASE, "receiving on %s ", ifr.ifr_name);
  419.  
  420.     for(i = 0; i < 6; i++) {
  421.         dest_mac[i] = ifr.ifr_hwaddr.sa_data[i] & 0xff;
  422.         print_log(LOG_BASE, "%02x%s", dest_mac[i], i < 5 ? ":" : "");
  423.     }
  424.     print_log(LOG_BASE, "\n");
  425.    
  426.     ctx->rsockfd = sockfd;
  427.     if (ctx->dest_mac == NULL)
  428.         ctx->dest_mac = dest_mac;
  429.     ctx->th_arg = th_arg;
  430.  
  431.     pkt_received = rfunc(ctx);
  432.  
  433.     strncpy(ifopts.ifr_name, th_arg->if_name, IFNAMSIZ - 1);
  434.     ifopts.ifr_flags &= ~IFF_PROMISC;
  435.     if (ioctl(sockfd, SIOCGIFFLAGS, &ifopts))
  436.         fprintf(stderr, "ioctl(SIOCSIFFLAGS): %s: %s\n", th_arg->if_name, sys_errlist[errno]);
  437.  
  438.     close(sockfd);
  439.     free(buf);
  440.  
  441.     return (void *)pkt_received;
  442. }
  443.  
  444. int wait_recv(int sock)
  445. {
  446.     fd_set RD, WR, EXP;
  447.     struct timeval tmo;
  448.     int R;
  449.  
  450.     FD_ZERO(&RD);
  451.     FD_ZERO(&WR);
  452.     FD_ZERO(&EXP);
  453.  
  454.     FD_SET(sock, &RD);
  455.  
  456.     tmo.tv_sec = 0;
  457.     tmo.tv_usec = 100000;
  458.  
  459. reselect:
  460.     // We assume that tmo recalculated by Linux kernel
  461.     R = select(sock + 1, &RD, &WR, &EXP, &tmo);
  462.     switch (R)
  463.     {
  464.     case 0:
  465.     case 1:
  466.         return R;
  467.     case -1:
  468.     default:
  469.         switch(errno)
  470.         {
  471.         case EINTR:
  472.             goto reselect;
  473.         default:
  474.             return -errno;
  475.         }
  476.     }
  477. }
  478.  
  479. inline unsigned long long do_recvfrom(struct func_ctx *ctx)
  480. {
  481.     int sockfd = ctx->rsockfd, i, j;
  482.     thread_arg_t *th_arg = (thread_arg_t *)ctx->th_arg;
  483.     ssize_t fsize = th_arg->fsize;
  484.     uint8_t xor, *buf = (uint8_t *)malloc(sizeof(uint8_t) * ctx->fsize); //ctx->b.buf;
  485.     unsigned char *dest_mac = ctx->dest_mac;
  486.     struct ether_header *ether_hdr = (struct ether_header *)buf;
  487.     struct ber_proto *ber = (struct ber_proto *) &buf[sizeof(struct ether_header)];
  488.     unsigned short pcsum;
  489.     unsigned long long bit_err = 0, pkt_num = 0, out_of_order = 0, pkt_received = 0, pkt_csum_err = 0;
  490.     int tail = 0;//, pl = sizeof(struct ether_header) + sizeof(struct ber_proto);
  491.  
  492.     pthread_spin_unlock(&th_arg->lock);
  493.     while (g_stop != 1) {
  494.         if (wait_recv(sockfd) < 0) {
  495.             fprintf(stderr, "error: wait_recv: %s\n", sys_errlist[errno]);
  496.             g_stop = 1;
  497.             break;
  498.         }
  499.         fsize = recvfrom(sockfd, buf, th_arg->fsize, 0, NULL, NULL);
  500.         if (ntohs(ether_hdr->ether_type) == ETHERTYPE_BER && ether_hdr->ether_dhost[0] == dest_mac[0] && ether_hdr->ether_dhost[1] == dest_mac[1]
  501.             && ether_hdr->ether_dhost[2] == dest_mac[2] && ether_hdr->ether_dhost[3] == dest_mac[3]
  502.             && ether_hdr->ether_dhost[4] == dest_mac[4] && ether_hdr->ether_dhost[5] == dest_mac[5]) {
  503.             pkt_num++;
  504.             if (!ber->tail && ber->num != pkt_num) {
  505.                 out_of_order++;
  506.                 pkt_num = ber->num;
  507.                 bit_err++;
  508.             }
  509.  
  510.             pkt_received++;
  511.             pcsum = ber->csum;
  512.             ber->csum = 0;
  513.             if (pcsum != csum((unsigned short *)buf, fsize)) {
  514.                 pkt_csum_err++;
  515.                 print_log(LOG_META, "checksum error %X != %X\n", pcsum, csum((unsigned short *)buf, fsize));
  516.                 for (i = 0; i < fsize; i++) {
  517.                     if (buf[i] != ctx->b.buf[i]) {
  518.                         xor = buf[i] ^ ctx->b.buf[i];
  519.                         for (j = 0; j < 8; j++) {
  520.                             if ((xor >> j) == 1)
  521.                                 bit_err++;
  522.                         }
  523. //                      printf("%llu-%d) buf[i] = %x, ctx->b.buf[i] = %x, buf[i] ^ ctx->b.buf[i] = %x\n", pkt_num, i, buf[i], ctx->b.buf[i], buf[i] ^ ctx->b.buf[i]);
  524.                     }
  525.                 }
  526.                 ber->csum = pcsum;
  527.             }
  528. //          printf("pkt_num = %llu/%llu\n", ber->num, ctx->fcount);
  529.             if (ber->tail == 0xE4ED || ber->tail == 0xE6ED) {
  530. //              printf("tail packet %x captured\n", ber->tail);
  531.                 tail++;
  532.             }
  533.             print_log(LOG_EXTENDED,
  534.                     "%02x:%02x:%02x:%02x:%02x:%02x -> %02x:%02x:%02x:%02x:%02x:%02x, %d bytes received\n",
  535.                     ether_hdr->ether_shost[0], ether_hdr->ether_shost[1], ether_hdr->ether_shost[2],
  536.                     ether_hdr->ether_shost[3], ether_hdr->ether_shost[4], ether_hdr->ether_shost[5],
  537.                     ether_hdr->ether_dhost[0], ether_hdr->ether_dhost[1], ether_hdr->ether_dhost[2],
  538.                     ether_hdr->ether_dhost[3], ether_hdr->ether_dhost[4], ether_hdr->ether_dhost[5], fsize);
  539.  
  540.             print_log(LOG_EXTENDED, "packet[%d]: ", fsize);
  541.             ber->csum = pcsum;
  542.             for (i = 0; i < fsize; i++)
  543.                 print_log(LOG_EXTENDED, "%02x%c", buf[i], i == fsize - 1 ? '\0' : ':');
  544.             print_log(LOG_EXTENDED, "\n");
  545.         } else {
  546.             if (ntohs(ether_hdr->ether_type) == ETHERTYPE_ARP) {
  547.                 print_log(LOG_META, "ARP destination MAC (0x%04x) %02x:%02x:%02x:%02x:%02x:%02x\n", ntohs(ether_hdr->ether_type),
  548.                     ether_hdr->ether_dhost[0], ether_hdr->ether_dhost[1], ether_hdr->ether_dhost[2],
  549.                     ether_hdr->ether_dhost[3], ether_hdr->ether_dhost[4], ether_hdr->ether_dhost[5]);
  550.                 continue;
  551.             }
  552.  
  553.             if (!th_arg->suppress_all && !is_suppressed(th_arg->supp_head, ether_hdr->ether_dhost)) {
  554.                 print_log(LOG_BASE, "wrong destination MAC (0x%04x) %02x:%02x:%02x:%02x:%02x:%02x != %02x:%02x:%02x:%02x:%02x:%02x\n", ntohs(ether_hdr->ether_type),
  555.                     ether_hdr->ether_dhost[0], ether_hdr->ether_dhost[1], ether_hdr->ether_dhost[2],
  556.                     ether_hdr->ether_dhost[3], ether_hdr->ether_dhost[4], ether_hdr->ether_dhost[5],
  557.                     dest_mac[0], dest_mac[1], dest_mac[2], dest_mac[3], dest_mac[4], dest_mac[5]);
  558.             }
  559.         }
  560.     }
  561.     free(buf);
  562.  
  563.     printf("number of received packets = %llu\n", pkt_received);
  564.     printf("bit error rate             = %.12Lf\n", (long double)bit_err / (ctx->fsize * 8) / (pkt_received + out_of_order));
  565.     printf("test of resistance to load: %s\n", tail > 0 ? "SUCCESS" : "FAIL");
  566.  
  567.     return pkt_received;
  568. }
  569.  
  570. int fill_suppressed(supp_adrs_t *supp_head, const char *supp_str, uint8_t *suppress_all)
  571. {
  572.     supp_adrs_t *t, *suppressed;
  573.     struct ether_addr *eaddr;
  574.     int i;
  575.  
  576.     if (supp_str) {
  577.         char *p = NULL;
  578.         int s = 0, len = strlen(supp_str), n;
  579.  
  580.         if (!strcmp(supp_str, "all")) {
  581.             *suppress_all = 1;
  582.         } else {
  583.             do {
  584.                 p = strchr(supp_str + s, ',');
  585.                 if(p)
  586.                     *p = 0;
  587.                 eaddr = (struct ether_addr *)malloc(sizeof(struct  ether_addr));
  588.                 n = sscanf(supp_str + s, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
  589.                     &eaddr->ether_addr_octet[0], &eaddr->ether_addr_octet[1], &eaddr->ether_addr_octet[2],
  590.                     &eaddr->ether_addr_octet[3], &eaddr->ether_addr_octet[4], &eaddr->ether_addr_octet[5]);
  591.                 if (n != 6) {
  592.                     if (s < len + 1) {
  593.                         p = strchr(supp_str + s, ',');
  594.                         if (p)
  595.                             *p = '\0';
  596.                         fprintf(stderr, "error in address '%s'\n", supp_str);
  597.                         return 1;
  598.                     }
  599.                     if (p)
  600.                         *p = ',';
  601.                     break;
  602.                 }
  603.                 suppressed = (supp_adrs_t *) malloc(sizeof(supp_adrs_t));
  604.                 suppressed->next = NULL;
  605.                 suppressed->eaddr = eaddr;
  606.                 if (supp_head->next) {
  607.                     t = supp_head;
  608.                     while (t->next)
  609.                         t = t->next;
  610.                     t->next = suppressed;
  611.                 } else {
  612.                     supp_head->next = suppressed;
  613.                 }
  614.                 s += strlen(supp_str + s) + 1;
  615.                 if (p)
  616.                     *p = ',';
  617.             } while (p != NULL);
  618.             t = supp_head->next;
  619.             printf ("suppressed list:\n");
  620.             while (t) {
  621.                 printf("    ");
  622.                 for (i = 0; i < ETH_ALEN; i++)
  623.                     printf("%02x%c", t->eaddr->ether_addr_octet[i], i == ETH_ALEN - 1 ? '\0' : ':');
  624.                 printf("\n");
  625.                 t = t->next;
  626.             }
  627.         }
  628.     }
  629.  
  630.     return 0;
  631. }
  632.  
  633. #define SEND_MODE 0x0001
  634. #define RECV_MODE 0x0010
  635. #define LBCK_MODE 0x0100
  636.  
  637. #define CHK(Expr) \
  638.     if ((err = (Expr)) != 0) \
  639.     { \
  640.     printf(#Expr " failed with error %d\n", -err); \
  641.     }
  642.  
  643. int get_speed_capability(struct MFE *dev)
  644. {
  645.     int caps = 0;
  646.     uint16_t reg = 0;
  647.  
  648.     if (dev == NULL)
  649.         return (uint32_t)-1;
  650.  
  651.     reg = mfe_reg_read(dev, MFE_BASIC_STATUS_REG);
  652.  
  653.     if (reg & _10BASE_HD_ST) caps |= MFE_SPEED_10BASE_T_HD;
  654.     if (reg & _10BASE_FD_ST) caps |= MFE_SPEED_10BASE_T_FD;
  655.     if (reg & _100BASE_TX_HD_ST) caps |= MFE_SPEED_100BASE_T_HD;
  656.     if (reg & _100BASE_TX_FD_ST) caps |= MFE_SPEED_100BASE_T_FD;
  657.  
  658.     if (reg & EXTENDED_STATUS) {
  659.         reg = mfe_reg_read(dev, MFE_EXTENDED_STATUS_REG);
  660.         if (reg & XS_1000BASE_T_HD) caps |= MFE_SPEED_1000BASE_T_HD;
  661.         if (reg & XS_1000BASE_T_FD) caps |= MFE_SPEED_1000BASE_T_FD;
  662.     }
  663.  
  664.     return caps;
  665. }
  666.  
  667. int configure_device(struct MFE *mfe, int speed, char *name)
  668. {
  669.     int ret;
  670.     unsigned int id;
  671.  
  672.     printf("configuring %s with speed mask %x\n", name, speed);
  673.     id = mfe_device_type_id(mfe);
  674. /*
  675.     if (id != MFE_DEV_TYPE_ID_MICREL && id != MFE_DEV_TYPE_ID_MFE && mfe_check_bit_state(mfe, MFE_BASIC_CONTROL_REG, POWER_DOWN)) {
  676.         mfe_power_down(mfe, 0);
  677.         usleep(1000000);
  678.     } else {
  679.         mfe_reset(mfe);
  680.         do {
  681.             ret = mfe_reg_read(mfe, 0x0);
  682.         } while (ret & RESET);
  683.     }
  684. */
  685.     if (mfe_check_bit_state(mfe, MFE_BASIC_CONTROL_REG, POWER_DOWN)) {
  686.         printf("configure_device %s  wakeup\n", name);
  687.         mfe_power_down(mfe, 0);
  688.         usleep(2000000);
  689.     }
  690.  
  691.     ret = mfe_reg_read(mfe, 0);
  692.     print_log(LOG_META, "%s[0] = 0x%x\n", name, ret);
  693. #if 1
  694.     if ((ret = mfe_reg_write(mfe, 0, ret & ~AUTO_NEG_EN)) < 0) {
  695.         printf("configure_device - 1\n");
  696. //      goto out;
  697.     }
  698. //  mfe_reg_write(mfe, MFE_AN_ADVERTISEMENT_REG, 0);
  699. //  mfe_set_autoneg_speeds(mfe, speed);
  700.     if ((ret = mfe_set_fixed_speed(mfe, speed)) < 0) {
  701.         printf("configure_device - 2\n");
  702. //      goto out;
  703.     }
  704. #else
  705.     if ((mode == LBCK_MODE) && selftest) {
  706.         mfe_reg_write(mfe, 0, ret & ~AUTO_NEG_EN);
  707.         mfe_set_fixed_speed(mfe, speed);
  708.     } else
  709.         mfe_set_autoneg_speeds(mfe, speed);
  710. #endif
  711.     ret = mfe_reg_read(mfe, 0);
  712.     print_log(LOG_META, "%s[0] = %x\n", name, ret);
  713.  
  714.     ret = 0;
  715. //out:
  716.     return ret;
  717. }
  718.  
  719. int init_mfe_devices(struct MFE **ptm, struct MFE **pam, int mode, char *dev, char *rdev, int speed, int selftest)
  720. {
  721.     int ret = 0, err = 0;
  722.     struct MFE_IDENT mfeid;
  723.     struct MFE *tm = NULL, *am = NULL;
  724.     uint16_t tm_caps, am_caps;
  725.  
  726.  
  727.     mfeid.ifname = dev;
  728.     print_log(LOG_META, "dev = %s; rdev = %s\n", dev, rdev);
  729.     if (!(mode == RECV_MODE) && dev && ((err = mfe_open(ptm, &mfeid)) != 0)) {
  730.         printf("failed mfe_open() for '%s' with error %d\n", mfeid.ifname, err);
  731.         ret = -1;
  732.         goto out;
  733.     }
  734.     if (mode == LBCK_MODE) {
  735.         am = tm;
  736.         *pam = *ptm;
  737.     } else {
  738.         mfeid.ifname = rdev;
  739.         if (!(mode == SEND_MODE) && rdev && ((err = mfe_open(pam, &mfeid)) != 0)) {
  740.             printf("failed mfe_open() for '%s' with error %d\n", mfeid.ifname, err);
  741.             ret = -1;
  742.             goto out;
  743.         }
  744.     }
  745.  
  746.     if (ptm) tm = *ptm;
  747.     if (pam) am = *pam;
  748.  
  749.     if (tm) {
  750.         printf("TM type ID = 0x%x\n", mfe_device_type_id(tm));
  751.         tm_caps = get_speed_capability(tm);
  752.         printf("tm_caps = 0x%0x\n", tm_caps);
  753.     } else
  754.         tm_caps = 0xffff;
  755.  
  756.     if (am) {
  757.         printf("AM type ID = 0x%x\n", mfe_device_type_id(am));
  758.         am_caps = get_speed_capability(am);
  759.         printf("am_caps = 0x%0x\n", am_caps);
  760.     } else
  761.         am_caps = 0xffff;
  762.  
  763.     print_log(LOG_META, "tm speed = %x\n", tm_caps);
  764.     print_log(LOG_META, "am speed = %x\n", am_caps);
  765.  
  766.     if (tm) {
  767.         print_log(LOG_META, "speed = %dMbps\n", speed);
  768.  
  769.         switch (speed) {
  770.             case 10:
  771.                 if (tm_caps & MFE_SPEED_10BASE_T_FD && am_caps & MFE_SPEED_10BASE_T_FD)
  772.                     speed = MFE_SPEED_10BASE_T_FD;
  773.                 else
  774.                     if (tm_caps & MFE_SPEED_10BASE_T_HD && am_caps & MFE_SPEED_10BASE_T_HD)
  775.                         speed = MFE_SPEED_10BASE_T_HD;
  776.                     else
  777.                         ret = -1;
  778.                 break;
  779.             case 100:
  780.                 if (tm_caps & MFE_SPEED_100BASE_T_FD && am_caps & MFE_SPEED_100BASE_T_FD)
  781.                     speed = MFE_SPEED_100BASE_T_FD;
  782.                 else
  783.                     if (tm_caps & MFE_SPEED_100BASE_T_HD && am_caps & MFE_SPEED_100BASE_T_HD)
  784.                         speed = MFE_SPEED_100BASE_T_HD;
  785.                     else
  786.                         ret = -1;
  787.                 break;
  788.             case 1000:
  789.                 if (tm_caps & MFE_SPEED_1000BASE_T_FD && am_caps & MFE_SPEED_1000BASE_T_FD)
  790.                     speed = MFE_SPEED_1000BASE_T_FD;
  791.                 else
  792.                     if (tm_caps & MFE_SPEED_1000BASE_T_HD && am_caps & MFE_SPEED_1000BASE_T_HD)
  793.                         speed = MFE_SPEED_1000BASE_T_HD;
  794.                     else
  795.                         ret = -1;
  796.                 break;
  797.             default:
  798.                 printf("unknown speed value %d\n", speed);
  799.                 ret = -1;
  800.                 goto out;
  801.         };
  802.     }
  803.     if (ret == -1) {
  804.         printf ("%s device can not operate at speed %d\n", am_caps < tm_caps ? "am" : "tm", speed);
  805.         goto out;
  806.     }
  807.  
  808.     if (tm) {
  809.         if ((ret = configure_device(tm, speed, "tm")) < 0) {
  810.             printf("configure_device error %d\n", ret);
  811.             goto out;
  812.         }
  813.         if (speed > MFE_SPEED_100BASE_T_FD) {
  814.             ret = mfe_reg_read(tm, 9);
  815.             mfe_reg_write(tm, 9, ret | (1 << 12));
  816.             ret = mfe_reg_read(tm, 9);
  817.             mfe_reg_write(tm, 9, ret & ~(1 << 11));
  818.         }
  819.     }
  820.  
  821.     if ((mode == LBCK_MODE) && selftest) {
  822.         printf("loopback selftest mode\n");
  823.         ret = mfe_reg_read(tm, 0);
  824.         print_log(LOG_META, "writing %x to [0]\n", (ret | 0x4000) & ~0x1000);
  825.         do {
  826.             ret = mfe_reg_write(tm, 0, (ret | LOOPBACK) & ~AUTO_NEG_EN);
  827.             usleep(300000);
  828.             ret = mfe_reg_read(tm, 0);
  829.             print_log(LOG_META, "turning loopback (0x%0x) on [0] = 0x%x\n", (ret | LOOPBACK) & ~0x1000, ret);
  830.         } while(!g_stop && ((ret & 0x4000) != 0x4000));
  831.         print_log(LOG_META, "ret = 0x%x\n", ret);
  832.     }
  833.  
  834.     if ((mode & RECV_MODE) == RECV_MODE) {
  835.         print_log(LOG_META, "init am...\n");
  836.         if (configure_device(am, speed, "am") < 0)
  837.             goto out;
  838.         if (speed > MFE_SPEED_100BASE_T_FD) {
  839.             ret = mfe_reg_read(am, 9);
  840.             mfe_reg_write(am, 9, ret | (1 << 12));
  841.             ret = mfe_reg_read(am, 9);
  842.             mfe_reg_write(am, 9, ret | (1 << 11));
  843.         }
  844.     }
  845.  
  846.     usleep(500000);
  847.     printf("SPEED = 0x%0x\n", speed);
  848.     if (tm) {
  849.         printf("waiting for tm link... ");
  850.         fflush(stdout);
  851. //      ret = mfe_reg_read(tm, 0x1f);
  852. //      mfe_reg_write(tm, 0x1f, ret & ~((1 << 9) | (1 << 2)));
  853. //usleep(1000000);
  854.         if (mfe_check_bit_state(tm, MFE_BASIC_CONTROL_REG, POWER_DOWN)) {
  855.             printf("do tm wakeup loop\n");
  856.             mfe_power_down(tm, 0);
  857.             usleep(1000000);
  858.             do {
  859.                 usleep(300000);
  860.                 ret = mfe_reg_read(tm, 0x01);
  861.                 print_log(LOG_META, "tm waiting ret = 0x%x\n", ret);
  862.             } while (!mfe_link_status(tm) && !g_stop);
  863.         }
  864.         printf("done.\n");
  865.         printf("tm speed = %x\n", mfe_get_speed(tm));
  866.     }
  867.  
  868.     if (am && am != tm && !g_stop) {
  869.         ret = mfe_reg_read(am, 1);
  870.         print_log(LOG_META, "am[1] = 0x%x\n", ret);
  871.         printf("waiting for am link... ");
  872.         fflush(stdout);
  873.         if (mfe_check_bit_state(am, MFE_BASIC_CONTROL_REG, POWER_DOWN)) {
  874.             printf("do am wakeup loop\n");
  875.             mfe_power_down(am, 0);
  876. //          usleep(1000000);
  877.             do {
  878.                 usleep(300000);
  879.                 ret = mfe_reg_read(am, 0x01);
  880.                 print_log(LOG_META, "am waiting ret = 0x%x\n", ret);
  881.             } while (!mfe_link_status(am) && !g_stop);
  882.         }
  883.         printf("done.\n");
  884.         printf("am speed = %x\n", mfe_get_speed(am));
  885.     }
  886.  
  887.     ret = 0;
  888.     if (g_stop)
  889.         ret = -1;
  890. out:
  891.     return ret;
  892. }
  893.  
  894. void show_usage(char *arg)
  895. {
  896.     printf("usage: %s [ options... ]\n"
  897.         "options:\n"
  898.         "    [-l eth]\n"
  899.         "        loopback mode on interface eth\n"
  900.         "    [-s eth]\n"
  901.         "        transmitting mode on interface eth\n"
  902.         "    [-r eth]\n"
  903.         "         receiving mode on interface eth\n"
  904.         "    [-smac <MAC address>]\n"
  905.         "        force source MAC address\n"
  906.         "    [-dmac <MAC address>]\n"
  907.         "        force destination MAC address\n"
  908.         "    [-f size]\n"
  909.         "        size of frame in bytes (1518 by default)\n"
  910.         "    [-n number]\n"
  911.         "        number of frames\n"
  912.         "    [-t time]\n"
  913.         "        suggested time of transmitting\n"
  914.         "    [-p all|mac1,...,macN]\n"
  915.         "        suppess MAC-address(es)\n"
  916.         "    [-v number]\n"
  917.         "        level of verbosity (0 - 2)\n", arg);
  918. }
  919.  
  920. int main(int argc, char *argv[])
  921. {
  922.     char *endptr, *dev = NULL, *rdev = NULL, *lo_dev = NULL, *supp_str = NULL, *smac = NULL, *dmac = NULL;
  923.     int sockfd, speed = 0, j = 0, if_src_idx = 0;
  924.     uint8_t suppress_all = 0, err;
  925.     ssize_t ret = 0, fsize;
  926.     struct ifreq if_src, if_dst;
  927.     struct ether_header *ether_hdr;
  928.     struct sockaddr_ll sockaddr;
  929.     uint8_t *buf = NULL, dest_mac[6];
  930.     unsigned long long pkt_sent = 0, i;
  931.     long long fcount;
  932.     unsigned int time;
  933.     struct timespec start_time, stop_time;
  934.     time_t sec = 0;
  935.     long long nsec = 0;
  936.     pthread_t rthr;
  937.     thread_arg_t *th_arg = NULL;
  938.     supp_adrs_t supp_head;
  939.     libarg_ctx *actx = NULL;
  940.     struct mac_addresses *addrs = NULL, *ma_tmp;
  941.     struct func_ctx fctx;
  942.     const int pl = sizeof(struct ether_header) + sizeof(struct ber_proto);
  943. //  uint8_t remote_tm = 0, remote_am = 0;
  944.     int mode = 0, selftest;
  945.     struct MFE *tm = NULL, *am = NULL;
  946.  
  947.     supp_head.eaddr = NULL;
  948.     supp_head.next = NULL;
  949.  
  950.     memset(&fctx, 0, sizeof(struct func_ctx));
  951.  
  952.     if (signal(SIGALRM, sighandler) == SIG_ERR)
  953.         printf("warning: cannot catch SIGALRM\n");
  954.     if (signal(SIGINT, sighandler) == SIG_ERR)
  955.         printf("warning: cannot catch SIGINT\n");
  956.     if (signal(SIGTERM, sighandler) == SIG_ERR)
  957.         printf("warning: cannot catch SIGTERM\n");
  958. /*
  959.     if (signal(SIGHUP, sighandler) == SIG_ERR)
  960.         printf("warning: cannot catch SIGHUP\n");
  961.     if (signal(SIGPIPE, sighandler) == SIG_ERR)
  962.         printf("warning: cannot catch SIGPIPE\n");
  963. */
  964.  
  965.     addrs = getmactable();
  966.     if (addrs == NULL) {
  967.         printf("error: cannot open devices list via sysfs\n");
  968.         return 1;
  969.     }
  970.  
  971.     actx = libarg_init(show_usage);
  972.     libarg_add_str(actx, 0, &lo_dev, 'l', "--loopback", NULL);
  973.     libarg_add_flag(actx, &selftest, '\0', "--selftest");
  974.     libarg_add_str(actx, 0, &dev, 's', "--send", NULL);
  975.     libarg_add_str(actx, 0, &rdev, 'r', "--recv", NULL);
  976.     libarg_add_longlong(actx, 0, &fcount, 'n', "--fcount", 0);
  977.     libarg_add_uint(actx, 0, &time, 't', "--time", 0);
  978.     libarg_add_ssize(actx, 0, &fsize, 'f', "--fsize", 1518);
  979.     libarg_add_int(actx, 0, &speed, 'c', "--speed", 0);
  980.     libarg_add_int(actx, 0, &log_level, 'v', "--verbosity", 0);
  981.     libarg_add_str(actx, 0, &supp_str, 'p', "--suppress", NULL);
  982.     libarg_add_str(actx, 0, &smac, '\0', "-smac", NULL);
  983.     libarg_add_str(actx, 0, &dmac, '\0', "-dmac", NULL);
  984.  
  985.     if ((ret = libarg_settle(actx, 1, argc, argv, 0, ' ')) != 0) {
  986.         printf("error with %s\n", ret > 0 ? argv[ret] : argv[0]);
  987.         goto out;
  988.     }
  989.  
  990.     if (dev)
  991.         mode += SEND_MODE;
  992.     if (rdev)
  993.         mode += RECV_MODE;
  994.     if (lo_dev)
  995.         mode += LBCK_MODE;
  996.  
  997.     if (mode > LBCK_MODE) {
  998.         printf ("error: loopback mode can not be mixed with another mode\n");
  999.         show_usage(argv[0]);
  1000.         goto out;
  1001.     }
  1002.  
  1003.     if (selftest && (mode != LBCK_MODE)) {
  1004.         printf("error: selftest allowed only in loopback mode\n");
  1005.         show_usage(argv[0]);
  1006.         goto out;
  1007.     }
  1008.  
  1009.     if (mode == LBCK_MODE) {
  1010.         if (dev || rdev) {
  1011.             show_usage(argv[0]);
  1012.             goto out;
  1013.         }
  1014.         dev = rdev = lo_dev;
  1015.     }
  1016.  
  1017.     if ((mode == SEND_MODE)/* && !rdev*/) {
  1018.         if (dmac == NULL) {
  1019.             printf ("error: dmac either receiving device are needed in transmit mode\n");
  1020.             goto out;
  1021.         }
  1022. //      printf("dmac = %s\n", dmac);
  1023.     }
  1024. /*
  1025.     if (mode == RECV_MODE) {
  1026.         remote_tm = 1;
  1027.     }
  1028. */
  1029.     if ((mode != RECV_MODE) && fcount <= 0 && time <= 0) {
  1030.         printf("error: frame count or time should be setled more than 0\n");
  1031.         goto out;
  1032.     }
  1033.  
  1034.     if (fsize < pl) {
  1035.         fsize = pl;
  1036.         printf("fsize forced to be minimal %d\n", pl);
  1037.     }
  1038.  
  1039.     buf = (uint8_t *) malloc(sizeof(uint8_t) * fsize);
  1040.  
  1041.     if(fill_suppressed(&supp_head, supp_str, &suppress_all))
  1042.         goto out;
  1043.  
  1044.     ether_hdr = (struct ether_header *)buf;
  1045.  
  1046.     if (dmac) {
  1047.         unsigned int a1, a2, a3, a4, a5, a6;
  1048.         if (sscanf(dmac, "%x:%x:%x:%x:%x:%x", &a1, &a2, &a3, &a4, &a5, &a6) == 6) {
  1049.             dest_mac[0] = a1;
  1050.             dest_mac[1] = a2;
  1051.             dest_mac[2] = a3;
  1052.             dest_mac[3] = a4;
  1053.             dest_mac[4] = a5;
  1054.             dest_mac[5] = a6;
  1055.         } else {
  1056.             printf("error: bad dmac address '%s'\n", dmac);
  1057.             goto out;
  1058.         }
  1059.     }
  1060.  
  1061.     if (init_mfe_devices(&tm, &am, mode, dev, rdev, speed, selftest) == -1)
  1062.         goto out;
  1063.  
  1064.     if ((sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) == -1) {
  1065.         fprintf(stderr, "socket: %s\n", sys_errlist[errno]);
  1066.         goto out;
  1067.     }
  1068.  
  1069.     j = 1;
  1070.     if (setsockopt(sockfd, SOL_PACKET, PACKET_QDISC_BYPASS, &j, sizeof(j)) == -1) {
  1071.         fprintf(stderr, "setsockopt(PACKET_QDISC_BYPASS): %s: %s\n", dev, sys_errlist[errno]);
  1072.         goto out;
  1073.     }
  1074.  
  1075.     fctx.sockfd = sockfd;
  1076.     fctx.tx_len = sizeof(struct ether_header) + fsize - 14;
  1077.     fctx.fcount = fcount;
  1078.     fctx.fsize = fsize;
  1079.     fctx.sockaddr = &sockaddr;
  1080.     fctx.th_arg = th_arg;
  1081.     fctx.ether_hdr = ether_hdr;
  1082.     fctx.time = time;
  1083.     if (dmac)
  1084.         fctx.dest_mac = dest_mac;
  1085.  
  1086.     for (j = 0; j < fsize - pl; j++)
  1087.         buf[pl + j] = j % 256;
  1088.     fctx.b.buf = buf;
  1089.  
  1090.     if (dev) {
  1091.         memset(&if_src, 0, sizeof(struct ifreq));
  1092.         printf("// tm[0] = %x\n", mfe_reg_read(tm, 0));
  1093.         CHK(mfe_ifindex(tm, &if_src_idx));
  1094.         printf("// tm[0] = %x\n", mfe_reg_read(tm, 0));
  1095.         CHK(mfe_macaddr(tm, if_src.ifr_hwaddr.sa_data));
  1096.         printf("// tm[0] = %x\n", mfe_reg_read(tm, 0));
  1097.  
  1098.         for (i = 0; i < 6; i++)
  1099.             ether_hdr->ether_shost[i] = ((uint8_t *)&if_src.ifr_hwaddr.sa_data)[i];
  1100.     }
  1101.  
  1102.     if (smac) {
  1103.         for (i = 0; i < 6; i++) {
  1104.             ether_hdr->ether_shost[i] = strtoll(smac + 3 * i, &endptr, 16);
  1105.             sockaddr.sll_addr[i] = ether_hdr->ether_shost[i];
  1106.             if (!((*endptr == ':' && i < 5) || (*endptr == '\0' && i == 5))) {
  1107.                 break;
  1108.             }
  1109.         }
  1110.         print_log(LOG_EXTENDED, "smac = %s\n", smac);
  1111.     }
  1112.     if (dmac) {
  1113.         for (i = 0; i < 6; i++) {
  1114.             ether_hdr->ether_dhost[i] = strtoll(dmac + 3 * i, &endptr, 16);
  1115.             sockaddr.sll_addr[i] = ether_hdr->ether_dhost[i];
  1116.             if (!((*endptr == ':' && i < 5) || (*endptr == '\0' && i == 5))) {
  1117.                 break;
  1118.             }
  1119.         }
  1120.         print_log(LOG_EXTENDED, "dmac = %s\n", dmac);
  1121.     }
  1122.  
  1123.     ether_hdr->ether_type = htons(ETHERTYPE_BER);
  1124.  
  1125.     memset(&sockaddr, 0, sizeof(sockaddr));
  1126.     sockaddr.sll_family = AF_PACKET;
  1127.     sockaddr.sll_protocol = htons(ETHERTYPE_BER);
  1128.     sockaddr.sll_ifindex = if_src_idx;
  1129.     sockaddr.sll_hatype = ARPHRD_ETHER;
  1130.     sockaddr.sll_pkttype = PACKET_OTHERHOST;
  1131.     sockaddr.sll_halen = ETH_ALEN;
  1132.  
  1133.     if (rdev || (mode == LBCK_MODE)) {
  1134.         if (am) {
  1135.             printf("creating receiving thread on %s...\n", rdev);
  1136.             CHK(mfe_macaddr(am, if_dst.ifr_hwaddr.sa_data));
  1137.             for (i = 0; i < 6; i++) {
  1138.                 ether_hdr->ether_dhost[i] = ((uint8_t *) & if_dst.ifr_hwaddr.sa_data)[i];
  1139.                 sockaddr.sll_addr[i] = ether_hdr->ether_dhost[i];
  1140.             }
  1141.             sockaddr.sll_addr[6] = 0x00;
  1142.             sockaddr.sll_addr[7] = 0x00;
  1143.  
  1144.             th_arg = (struct thread_arg *)malloc(sizeof(struct thread_arg));
  1145.             th_arg->if_name = (char *)malloc(strlen(rdev) + 1);
  1146.             strcpy(th_arg->if_name, rdev);
  1147.             th_arg->supp_head = supp_head.next;
  1148.             th_arg->suppress_all = suppress_all;
  1149.             th_arg->fsize = fsize;
  1150.             th_arg->ctx = &fctx;
  1151.             th_arg->rfunc = do_recvfrom;
  1152.  
  1153.             pthread_spin_init(&th_arg->lock, PTHREAD_PROCESS_SHARED);
  1154.             pthread_spin_lock(&th_arg->lock);
  1155.             pthread_create(&rthr, NULL, receiver, th_arg);
  1156.             pthread_spin_lock(&th_arg->lock);
  1157.             if (!dev) {
  1158.                 goto out;
  1159.             }
  1160.         }
  1161.     } else
  1162.         printf("receiving thread off\n");
  1163.  
  1164.     printf("src MAC = ");
  1165.     for (i = 0; i < 6; i++)
  1166.         printf("%02x%c", ether_hdr->ether_shost[i], i < 5 ? ':' : ' ');
  1167.     printf("\n");
  1168.     printf("dst MAC = ");
  1169.     for (i = 0; i < 6; i++)
  1170.         printf("%02x%c", ether_hdr->ether_dhost[i], i < 5 ? ':' : ' ');
  1171.     printf("\n");
  1172.     print_log(LOG_EXTENDED, "fcount = %lld, fsize = %ld\n", fcount, fsize);
  1173.  
  1174.     printf("time = %d\n", time);
  1175.     if (time > 0) {
  1176. //      sigaction(SIGALRM, &sa, 0);
  1177.         printf("alarm time = %d seconds\n", time);
  1178.         alarm(time);
  1179.     }
  1180.  
  1181.     pkt_sent = 0;
  1182.     clock_gettime(CLOCK_REALTIME, &start_time);
  1183.     pkt_sent = do_sendto(&fctx);
  1184.     clock_gettime(CLOCK_REALTIME, &stop_time);
  1185.     if (th_arg)
  1186.         th_arg->ctx->fcount = pkt_sent;
  1187.     sleep(1); // wait for last packet
  1188.     printf("number of sent packets     = %llu\n", pkt_sent);
  1189.  
  1190.  
  1191.     sec = stop_time.tv_sec - start_time.tv_sec;
  1192.     nsec = stop_time.tv_nsec - start_time.tv_nsec;
  1193.     if (nsec < 0) {
  1194.         sec -= 1;
  1195.         nsec = 1e9 - -nsec;
  1196.     }
  1197.  
  1198. out:
  1199.     if (th_arg) {
  1200.         void *th_ret = NULL;
  1201.         if(dev)
  1202.             g_stop = 1;
  1203.         pthread_join(rthr, &th_ret);
  1204.         fflush(stdout);
  1205.         pthread_spin_unlock(&th_arg->lock);
  1206.         pthread_spin_destroy(&th_arg->lock);
  1207.         free(th_arg->if_name);
  1208.         free(th_arg);
  1209.     }
  1210.     if (pkt_sent > 0) {
  1211.         printf("average speed: %.4lf MB/s\n", fsize * pkt_sent / (sec + (double)nsec / 1e9) / 1024 / 1024);
  1212.     }
  1213.     if (tm) {
  1214.         if (mode == LBCK_MODE) {
  1215.             ret = mfe_reg_read(tm, 0);
  1216. //          printf ("// tm at exit  = 0x%x\n", ret);
  1217.             mfe_reg_write(tm, 0, ret & ~0x4000);
  1218.             ret = mfe_reg_read(tm, 0);
  1219. //          printf ("// reverted tm = 0x%x\n", ret);
  1220.         }
  1221.         mfe_reset(tm);
  1222. //      mfe_power_down(tm, 1);
  1223.         mfe_close(tm);
  1224.     }
  1225.     if (am && !(mode == LBCK_MODE)) {
  1226.         mfe_reset(am);
  1227. //      mfe_power_down(am, 1);
  1228.         mfe_close(am);
  1229.     }
  1230.  
  1231.     while(addrs) {
  1232.         ma_tmp = addrs->next;
  1233.         free(addrs->if_name);
  1234.         free(addrs->if_mac);
  1235.         free(addrs);
  1236.         addrs = ma_tmp;
  1237.     }
  1238.  
  1239.     if (supp_head.next) {
  1240.         supp_adrs_t *t = supp_head.next, *tmp;
  1241.         while(t) {
  1242.             free(t->eaddr);
  1243.             tmp = t;
  1244.             t = t->next;
  1245.             free(tmp);
  1246.         }
  1247.     }
  1248.  
  1249.     libarg_destroy(actx);
  1250.     free(buf);
  1251.  
  1252.     return 0;
  1253. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top