src/fwp/wme_test/wserver.c

   1 #include <stdlib.h>
   2 #include <errno.h>
   3
   4 #include <sys/types.h>
   5 #include <sys/socket.h>
   6 #include <netinet/in.h>
   7 #include <arpa/inet.h>
   8
   9 #include <signal.h>
  10 #include <sys/wait.h>
  11 #include <stdio.h>
  12 #include <unistd.h>
  13 #include <fcntl.h>
  14 #include <time.h>
  15 #include <string.h>
  16 #include <pthread.h>
  17 #include "common.h"
  18 #include <stdbool.h>
  19
  20 bool opt_same_interface = false;
  21
  22 static int ac_sockfd[AC_NUM];
  23
  24 struct receiver {
  25         unsigned received, last_received;
  26 } receivers[AC_NUM];
  27
  28
  29 void stopper()
  30 {
  31         int i;
  32
  33         for (i = 0; i < AC_NUM; i++)
  34                 close(ac_sockfd[i]);
  35
  36         exit(0);
  37 }
  38
  39 int create_ac_socket(unsigned int ac)
  40 {
  41         struct sockaddr_in my_addr;
  42         int sockfd;
  43         unsigned int yes=1, tos;
  44
  45
  46         if ((sockfd = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
  47         {
  48                 perror("Socket nelze otevrit");
  49                 return -1;
  50         }
  51
  52         if (setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,&yes,sizeof(int)) == -1) {
  53                 perror("Chyba v nastaveni soketu");
  54                 return -1;
  55         }
  56
  57         if (opt_same_interface) {
  58                 int receive = 1;
  59                 if (setsockopt(sockfd, SOL_IP, IP_PKTINFO, &receive, sizeof(receive)) == -1) {
  60                         perror("setsockopt: IP_PKTINFO");
  61                         exit(1);
  62                 }
  63         }
  64
  65
  66    //   bzero(&my_addr, sizeof(my_addr));
  67         memset(&my_addr,0, sizeof(my_addr));
  68         my_addr.sin_family = AF_INET;
  69         my_addr.sin_addr.s_addr = INADDR_ANY;
  70         my_addr.sin_port = htons(BASE_PORT + ac);
  71
  72         if (bind(sockfd, (struct sockaddr *)&my_addr, sizeof(my_addr)) == -1) {
  73                 perror("Chyba v bind");
  74                 close(sockfd);
  75                 return -1;
  76         }
  77
  78         //tos = ((AC_NUM - ac) *2 - 1)*32;
  79         tos = ac_to_tos[ac];
  80         if (setsockopt(sockfd, SOL_IP, IP_TOS, &tos, sizeof(tos))) {
  81                 perror("Unable to set TOS");
  82                 close(sockfd);
  83                 return -1;
  84         }
  85
  86         return sockfd;
  87 }
  88
  89 void* qhandler(void* queue)
  90 {
  91         union msg_buff buff;
  92         struct  sockaddr_in rem_addr;
  93         int     mlen;
  94         unsigned int rem_addr_length;
  95         intptr_t ac;
  96         char cbufrec[512], cbufsend[512];
  97         struct iovec  iov;
  98         struct msghdr msg;
  99         struct in_pktinfo *ipi = NULL;
 100         struct timespec ts;
 101
 102         ac = (intptr_t) queue;
 103         rem_addr_length=sizeof(rem_addr);
 104
 105         block_signals();
 106         set_rt_prio(90-ac);
 107
 108         while (1) {
 109                 struct cmsghdr *cmsg;
 110
 111                 iov.iov_base = &buff;
 112                 iov.iov_len = sizeof(buff);
 113                 msg.msg_name = (void*)&rem_addr;
 114                 msg.msg_namelen = sizeof(rem_addr);
 115                 msg.msg_iov = &iov;
 116                 msg.msg_iovlen = 1;
 117                 msg.msg_flags = 0;
 118                 msg.msg_control = cbufrec;
 119                 msg.msg_controllen = sizeof(cbufrec);
 120
 121                 while ((mlen = recvmsg(ac_sockfd[ac], &msg, 0)) < 0) {
 122                         if (errno == EINTR) continue;
 123                         perror("recvmsg");
 124                         return NULL;
 125                 }
 126                 clock_gettime(CLOCK_REALTIME, &ts);
 127                 buff.msg.sendback_timestamp = ts.tv_sec*1000000000 + ts.tv_nsec;
 128
 129
 130                 if (opt_same_interface) {
 131                         /* determine receiving interface */
 132                         for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 133                                 if (cmsg->cmsg_level == SOL_IP) {
 134                                         if (cmsg->cmsg_type == IP_PKTINFO) {
 135 /*                                              char spec_dst[20], addr[20]; */
 136                                                 ipi = (struct in_pktinfo*)CMSG_DATA(cmsg);
 137                                                 if (cmsg->cmsg_len <= sizeof(cbufsend)) {
 138                                                         struct in_pktinfo *ipi2;
 139                                                         msg.msg_control = cbufsend;
 140                                                         msg.msg_controllen = CMSG_LEN(sizeof(struct in_pktinfo));
 141                                                         cmsg = CMSG_FIRSTHDR(&msg);
 142                                                         cmsg->cmsg_level = SOL_IP;
 143                                                         cmsg->cmsg_type = IP_PKTINFO;
 144                                                         cmsg->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
 145                                                         /* Initialize the payload: */
 146                                                         ipi2 = (struct in_pktinfo*)CMSG_DATA(cmsg);
 147                                                         memset(ipi2, 0, sizeof(*ipi2));
 148                                                         ipi2->ipi_ifindex = ipi->ipi_ifindex;
 149
 150                                                 } else {
 151                                                         fprintf(stderr, "cbufsend too small\n");
 152                                                         msg.msg_control = NULL;
 153                                                         msg.msg_controllen = 0;
 154                                                 }
 155 /*                                              strncpy(spec_dst, inet_ntoa(ipi->ipi_spec_dst), sizeof(spec_dst)-1); */
 156 /*                                              strncpy(addr,     inet_ntoa(ipi->ipi_addr),     sizeof(addr)-1); */
 157 /*                                              printf("pktinfo if=%d %s %s\n", ipi->ipi_ifindex, spec_dst, addr); */
 158                                         }
 159                                 }
 160                         }
 161                 } else {
 162                         msg.msg_control = NULL;
 163                         msg.msg_controllen = 0;
 164                 }
 165 #ifdef DEBUG
 166                 printf("%d",ac);
 167                 fflush(stdout);
 168 #endif
 169                 receivers[ac].received++;
 170                 msg.msg_iov->iov_len = mlen;
 171 #ifdef WITH_FWP
 172                 /* resp_port is already stored in network order */
 173                 rem_addr.sin_port = buff.msg.resp_port;
 174 #endif
 175                 while (sendmsg(ac_sockfd[ac], &msg, 0) < 0) {
 176                             if (errno == EINTR) continue;
 177                             perror("sendmsg");
 178                             return NULL;
 179                 }
 180         }
 181 }
 182
 183
 184 int main(int argc, char *argv[])
 185 {
 186         int rc;
 187         intptr_t ac;
 188         pthread_attr_t attr;
 189         pthread_t thread;
 190
 191         char opt;
 192
 193
 194         while ((opt = getopt(argc, argv, "I")) != -1) {
 195                 switch (opt) {
 196                         case 'I':
 197                                 opt_same_interface = true;
 198                                 break;
 199                         default:
 200                                 fprintf(stderr, "Usage: %s [ options ]\n\n", argv[0]);
 201                                 fprintf(stderr, "Options:\n");
 202                                 fprintf(stderr, "    -I  send back through the same interface (bypass routing tables)\n");
 203                                 exit(1);
 204                 }
 205         }
 206         pthread_attr_init(&attr);
 207
 208         if (signal(SIGTERM, stopper) == SIG_ERR) {
 209                 perror("Signal handler registration error");
 210                 exit(1);
 211         }
 212
 213         if (signal(SIGINT, stopper) == SIG_ERR) {
 214                 perror("Signal handler registration error");
 215                 exit(1);
 216         }
 217
 218         for (ac = 0; ac < AC_NUM; ac++) {
 219                 ac_sockfd[ac] = create_ac_socket(ac);
 220                 rc = pthread_create(&thread, &attr, qhandler, (void*) ac);
 221
 222         }
 223
 224         while (1) {
 225                 printf("\r");
 226                 for (ac = 0; ac < AC_NUM; ac++) {
 227                         int delta = receivers[ac].received - receivers[ac].last_received;
 228                         receivers[ac].last_received = receivers[ac].received;
 229                         fprintf(stderr, "%s %5d %4d/s ", ac_to_text[ac], receivers[ac].received, delta);
 230                 }
 231                 fflush(stdout);
 232                 sleep(1);
 233         }
 234         printf("\n");
 235
 236         return 0;
 237
 238 }