libeap/mac80211_hwsim/tools/hwsim_test.c

   1 /*
   2  * hwsim_test - Data connectivity test for mac80211_hwsim
   3  * Copyright (c) 2009, Atheros Communications
   4  *
   5  * This software may be distributed under the terms of the BSD license.
   6  * See README for more details.
   7  */
   8
   9 #include <stdlib.h>
  10 #include <stdio.h>
  11 #include <string.h>
  12 #include <unistd.h>
  13 #include <sys/ioctl.h>
  14 #include <sys/socket.h>
  15 #include <sys/select.h>
  16 #include <netpacket/packet.h>
  17 #include <net/ethernet.h>
  18 #include <net/if.h>
  19 #include <arpa/inet.h>
  20 #include <netinet/ip.h>
  21
  22 #define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
  23 #define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
  24
  25 #define HWSIM_ETHERTYPE ETHERTYPE_IP
  26 #define HWSIM_PACKETLEN 1500
  27
  28 static unsigned char addr1[ETH_ALEN], addr2[ETH_ALEN], bcast[ETH_ALEN];
  29
  30 static u_int16_t checksum(const void *buf, size_t len)
  31 {
  32         size_t i;
  33         u_int32_t sum = 0;
  34         const u_int16_t *pos = buf;
  35
  36         for (i = 0; i < len / 2; i++)
  37                 sum += *pos++;
  38
  39         while (sum >> 16)
  40                 sum = (sum & 0xffff) + (sum >> 16);
  41
  42         return sum ^ 0xffff;
  43 }
  44
  45
  46 static void tx(int s, const char *ifname, int ifindex,
  47                const unsigned char *src, const unsigned char *dst,
  48                u_int8_t tos)
  49 {
  50         char buf[HWSIM_PACKETLEN], *pos;
  51         struct ether_header *eth;
  52         struct iphdr *ip;
  53         int i;
  54
  55         printf("TX: %s(ifindex=%d) " MACSTR " -> " MACSTR "\n",
  56                ifname, ifindex, MAC2STR(src), MAC2STR(dst));
  57
  58         eth = (struct ether_header *) buf;
  59         memcpy(eth->ether_dhost, dst, ETH_ALEN);
  60         memcpy(eth->ether_shost, src, ETH_ALEN);
  61         eth->ether_type = htons(HWSIM_ETHERTYPE);
  62         ip = (struct iphdr *) (eth + 1);
  63         memset(ip, 0, sizeof(*ip));
  64         ip->ihl = 5;
  65         ip->version = 4;
  66         ip->ttl = 64;
  67         ip->tos = tos;
  68         ip->tot_len = htons(HWSIM_PACKETLEN - sizeof(*eth));
  69         ip->protocol = 1;
  70         ip->saddr = htonl(192 << 24 | 168 << 16 | 1 << 8 | 1);
  71         ip->daddr = htonl(192 << 24 | 168 << 16 | 1 << 8 | 2);
  72         ip->check = checksum(ip, sizeof(*ip));
  73         pos = (char *) (ip + 1);
  74         for (i = 0; i < sizeof(buf) - sizeof(*eth) - sizeof(*ip); i++)
  75                 *pos++ = i;
  76
  77         if (send(s, buf, sizeof(buf), 0) < 0)
  78                 perror("send");
  79 }
  80
  81
  82 struct rx_result {
  83         unsigned int rx_unicast1:1;
  84         unsigned int rx_broadcast1:1;
  85         unsigned int rx_unicast2:1;
  86         unsigned int rx_broadcast2:1;
  87 };
  88
  89
  90 static void rx(int s, int iface, const char *ifname, int ifindex,
  91                struct rx_result *res)
  92 {
  93         char buf[HWSIM_PACKETLEN + 1], *pos;
  94         struct ether_header *eth;
  95         struct iphdr *ip;
  96         int len, i;
  97
  98         len = recv(s, buf, sizeof(buf), 0);
  99         if (len < 0) {
 100                 perror("recv");
 101                 return;
 102         }
 103         eth = (struct ether_header *) buf;
 104
 105         printf("RX: %s(ifindex=%d) " MACSTR " -> " MACSTR " (len=%d)\n",
 106                ifname, ifindex,
 107                MAC2STR(eth->ether_shost), MAC2STR(eth->ether_dhost), len);
 108
 109         if (len != HWSIM_PACKETLEN) {
 110                 printf("Ignore frame with unexpected RX length (%d)\n", len);
 111                 return;
 112         }
 113
 114         ip = (struct iphdr *) (eth + 1);
 115         pos = (char *) (ip + 1);
 116         for (i = 0; i < sizeof(buf) - 1 - sizeof(*eth) - sizeof(*ip); i++) {
 117                 if ((unsigned char) *pos != (unsigned char) i) {
 118                         printf("Ignore frame with unexpected contents\n");
 119                         printf("i=%d received=0x%x expected=0x%x\n",
 120                                i, (unsigned char) *pos, (unsigned char) i);
 121                         return;
 122                 }
 123                 pos++;
 124         }
 125
 126         if (iface == 1 &&
 127                    memcmp(eth->ether_dhost, addr1, ETH_ALEN) == 0 &&
 128                    memcmp(eth->ether_shost, addr2, ETH_ALEN) == 0)
 129                 res->rx_unicast1 = 1;
 130         else if (iface == 1 &&
 131                    memcmp(eth->ether_dhost, bcast, ETH_ALEN) == 0 &&
 132                    memcmp(eth->ether_shost, addr2, ETH_ALEN) == 0)
 133                 res->rx_broadcast1 = 1;
 134         else if (iface == 2 &&
 135                    memcmp(eth->ether_dhost, addr2, ETH_ALEN) == 0 &&
 136                    memcmp(eth->ether_shost, addr1, ETH_ALEN) == 0)
 137                 res->rx_unicast2 = 1;
 138         else if (iface == 2 &&
 139                    memcmp(eth->ether_dhost, bcast, ETH_ALEN) == 0 &&
 140                    memcmp(eth->ether_shost, addr1, ETH_ALEN) == 0)
 141                 res->rx_broadcast2 = 1;
 142 }
 143
 144
 145 static void usage(void)
 146 {
 147         fprintf(stderr, "usage: hwsim_test [-D<DSCP>] [-t<tos>] <ifname1> <ifname2>\n");
 148 }
 149
 150
 151 int main(int argc, char *argv[])
 152 {
 153         int s1 = -1, s2 = -1, ret = -1, c;
 154         struct ifreq ifr;
 155         int ifindex1, ifindex2;
 156         struct sockaddr_ll ll;
 157         fd_set rfds;
 158         struct timeval tv;
 159         struct rx_result res;
 160         char *s_ifname, *d_ifname, *end;
 161         int tos = 0;
 162
 163         for (;;) {
 164                 c = getopt(argc, argv, "D:t:");
 165                 if (c < 0)
 166                         break;
 167                 switch (c) {
 168                 case 'D':
 169                         tos = strtol(optarg, &end, 0) << 2;
 170                         if (*end) {
 171                                 usage();
 172                                 return -1;
 173                         }
 174                         break;
 175                 case 't':
 176                         tos = strtol(optarg, &end, 0);
 177                         if (*end) {
 178                                 usage();
 179                                 return -1;
 180                         }
 181                         break;
 182                 default:
 183                         usage();
 184                         return -1;
 185                 }
 186         }
 187
 188         if (optind != argc - 2) {
 189                 usage();
 190                 return -1;
 191         }
 192
 193         s_ifname = argv[optind];
 194         d_ifname = argv[optind + 1];
 195
 196         memset(bcast, 0xff, ETH_ALEN);
 197
 198         s1 = socket(PF_PACKET, SOCK_RAW, htons(HWSIM_ETHERTYPE));
 199         if (s1 < 0) {
 200                 perror("socket");
 201                 goto fail;
 202         }
 203
 204         s2 = socket(PF_PACKET, SOCK_RAW, htons(HWSIM_ETHERTYPE));
 205         if (s2 < 0) {
 206                 perror("socket");
 207                 goto fail;
 208         }
 209
 210         memset(&ifr, 0, sizeof(ifr));
 211         strncpy(ifr.ifr_name, s_ifname, sizeof(ifr.ifr_name));
 212         if (ioctl(s1, SIOCGIFINDEX, &ifr) < 0) {
 213                 perror("ioctl[SIOCGIFINDEX]");
 214                 goto fail;
 215         }
 216         ifindex1 = ifr.ifr_ifindex;
 217         if (ioctl(s1, SIOCGIFHWADDR, &ifr) < 0) {
 218                 perror("ioctl[SIOCGIFHWADDR]");
 219                 goto fail;
 220         }
 221         memcpy(addr1, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
 222
 223         memset(&ifr, 0, sizeof(ifr));
 224         strncpy(ifr.ifr_name, d_ifname, sizeof(ifr.ifr_name));
 225         if (ioctl(s2, SIOCGIFINDEX, &ifr) < 0) {
 226                 perror("ioctl[SIOCGIFINDEX]");
 227                 goto fail;
 228         }
 229         ifindex2 = ifr.ifr_ifindex;
 230         if (ioctl(s2, SIOCGIFHWADDR, &ifr) < 0) {
 231                 perror("ioctl[SIOCGIFHWADDR]");
 232                 goto fail;
 233         }
 234         memcpy(addr2, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
 235
 236         memset(&ll, 0, sizeof(ll));
 237         ll.sll_family = PF_PACKET;
 238         ll.sll_ifindex = ifindex1;
 239         ll.sll_protocol = htons(HWSIM_ETHERTYPE);
 240         if (bind(s1, (struct sockaddr *) &ll, sizeof(ll)) < 0) {
 241                 perror("bind");
 242                 goto fail;
 243         }
 244
 245         memset(&ll, 0, sizeof(ll));
 246         ll.sll_family = PF_PACKET;
 247         ll.sll_ifindex = ifindex2;
 248         ll.sll_protocol = htons(HWSIM_ETHERTYPE);
 249         if (bind(s2, (struct sockaddr *) &ll, sizeof(ll)) < 0) {
 250                 perror("bind");
 251                 goto fail;
 252         }
 253
 254         tx(s1, s_ifname, ifindex1, addr1, addr2, tos);
 255         tx(s1, s_ifname, ifindex1, addr1, bcast, tos);
 256         tx(s2, d_ifname, ifindex2, addr2, addr1, tos);
 257         tx(s2, d_ifname, ifindex2, addr2, bcast, tos);
 258
 259         tv.tv_sec = 1;
 260         tv.tv_usec = 0;
 261
 262         memset(&res, 0, sizeof(res));
 263         for (;;) {
 264                 int r;
 265                 FD_ZERO(&rfds);
 266                 FD_SET(s1, &rfds);
 267                 FD_SET(s2, &rfds);
 268
 269                 r = select(s2 + 1, &rfds, NULL, NULL, &tv);
 270                 if (r < 0) {
 271                         perror("select");
 272                         goto fail;
 273                 }
 274
 275                 if (r == 0)
 276                         break; /* timeout */
 277
 278                 if (FD_ISSET(s1, &rfds))
 279                         rx(s1, 1, s_ifname, ifindex1, &res);
 280                 if (FD_ISSET(s2, &rfds))
 281                         rx(s2, 2, d_ifname, ifindex2, &res);
 282
 283                 if (res.rx_unicast1 && res.rx_broadcast1 &&
 284                     res.rx_unicast2 && res.rx_broadcast2) {
 285                         ret = 0;
 286                         break;
 287                 }
 288         }
 289
 290         if (ret) {
 291                 printf("Did not receive all expected frames:\n"
 292                        "rx_unicast1=%u rx_broadcast1=%u "
 293                        "rx_unicast2=%u rx_broadcast2=%u\n",
 294                        res.rx_unicast1, res.rx_broadcast1,
 295                        res.rx_unicast2, res.rx_broadcast2);
 296         } else {
 297                 printf("Both unicast and broadcast working in both "
 298                        "directions\n");
 299         }
 300
 301 fail:
 302         close(s1);
 303         close(s2);
 304
 305         return ret;
 306 }