API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 16th, 2017
220
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 33.50 KB | None | 0 0
raw download clone embed print report
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!