tools/tools/net80211/w00t/libw00t/w00t.c

   1 /*-
   2  * Copyright (c) 2006, Andrea Bittau <a.bittau@cs.ucl.ac.uk>
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  *
  26  * $FreeBSD: src/tools/tools/net80211/w00t/libw00t/w00t.c,v 1.2 2006/08/06 23:50:56 sam Exp $
  27  */
  28 #include <stdio.h>
  29 #include <assert.h>
  30 #include <fcntl.h>
  31 #include <errno.h>
  32 #include <sys/socket.h>
  33 #include <sys/types.h>
  34 #include <sys/endian.h>
  35 #include <sys/uio.h>
  36 #include <unistd.h>
  37 #include <net/if.h>
  38 #include <string.h>
  39 #include <sys/ioctl.h>
  40 #include <net/bpf.h>
  41 #include <netproto/802_11/ieee80211_radiotap.h>
  42 #include <netproto/802_11/ieee80211.h>
  43 #include <openssl/rc4.h>
  44 #include <zlib.h>
  45 #include "w00t.h"
  46
  47 int str2mac(char *mac, char *str)
  48 {
  49         unsigned int macf[6];
  50         int i;
  51
  52         if (sscanf(str, "%x:%x:%x:%x:%x:%x",
  53                    &macf[0], &macf[1], &macf[2],
  54                    &macf[3], &macf[4], &macf[5]) != 6)
  55                 return -1;
  56
  57         for (i = 0; i < 6; i++)
  58                 *mac++ = (char) macf[i];
  59
  60         return 0;
  61 }
  62
  63 void mac2str(char *str, char* m)
  64 {
  65         unsigned char *mac = m;
  66         sprintf(str, "%.2X:%.2X:%.2X:%.2X:%.2X:%.2X",
  67                 mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
  68 }
  69
  70 short seqfn(unsigned short seq, unsigned short fn)
  71 {
  72         unsigned short r = 0;
  73
  74         assert(fn < 16);
  75
  76         r = fn;
  77         r |=  ( (seq % 4096) << IEEE80211_SEQ_SEQ_SHIFT);
  78         return r;
  79 }
  80
  81 unsigned short seqno(struct ieee80211_frame *wh)
  82 {
  83         unsigned short *s = (unsigned short*) wh->i_seq;
  84
  85         return (*s & IEEE80211_SEQ_SEQ_MASK) >> IEEE80211_SEQ_SEQ_SHIFT;
  86 }
  87
  88 int open_bpf(char *dev, int dlt)
  89 {
  90         int i;
  91         char buf[64];
  92         int fd = -1;
  93         struct ifreq ifr;
  94
  95         for(i = 0;i < 16; i++) {
  96                 sprintf(buf, "/dev/bpf%d", i);
  97
  98                 fd = open(buf, O_RDWR);
  99                 if(fd == -1) {
 100                         if(errno != EBUSY)
 101                                 return -1;
 102                         continue;
 103                 }
 104                 else
 105                         break;
 106         }
 107
 108         if(fd == -1)
 109                 return -1;
 110
 111         strncpy(ifr.ifr_name, dev, sizeof(ifr.ifr_name)-1);
 112         ifr.ifr_name[sizeof(ifr.ifr_name)-1] = 0;
 113
 114         if(ioctl(fd, BIOCSETIF, &ifr) < 0)
 115                 return -1;
 116
 117         if (ioctl(fd, BIOCSDLT, &dlt) < 0)
 118                 return -1;
 119
 120         i = 1;
 121         if (ioctl(fd, BIOCIMMEDIATE, &i) < 0)
 122                 return -1;
 123
 124         return fd;
 125 }
 126
 127 int open_tx(char *iface)
 128 {
 129         return open_bpf(iface, DLT_IEEE802_11_RADIO);
 130 }
 131
 132 int open_rx(char *iface)
 133 {
 134         return open_bpf(iface, DLT_IEEE802_11_RADIO);
 135 }
 136
 137 int open_rxtx(char *iface, int *rx, int *tx)
 138 {
 139         *rx = open_bpf(iface, DLT_IEEE802_11_RADIO);
 140         *tx = *rx;
 141
 142         return *rx;
 143 }
 144
 145 int inject(int fd, void *buf, int len)
 146 {
 147         return inject_params(fd, buf, len, NULL);
 148 }
 149
 150 int inject_params(int fd, void *buf, int len,
 151                   struct ieee80211_bpf_params *params)
 152 {
 153         static struct ieee80211_bpf_params defaults = {
 154                 .ibp_vers = IEEE80211_BPF_VERSION,
 155                 /* NB: no need to pass series 2-4 rate+try */
 156                 .ibp_len = sizeof(struct ieee80211_bpf_params) - 6,
 157                 .ibp_rate0 = 2,         /* 1 MB/s XXX */
 158                 .ibp_try0 = 1,          /* no retransmits */
 159                 .ibp_flags = IEEE80211_BPF_NOACK,
 160                 .ibp_power = 100,       /* nominal max */
 161                 .ibp_pri = WME_AC_VO,   /* high priority */
 162         };
 163         struct iovec iov[2];
 164         int rc;
 165
 166         if (params == NULL)
 167                 params = &defaults;
 168         iov[0].iov_base = params;
 169         iov[0].iov_len = params->ibp_len;
 170         iov[1].iov_base = buf;
 171         iov[1].iov_len = len;
 172
 173         rc = writev(fd, iov, 2);
 174         if (rc == -1)
 175                 return rc;
 176
 177         rc -= iov[0].iov_len; /* XXX could be negative */
 178         return rc;
 179 }
 180
 181 int sniff(int fd, void *buf, int len)
 182 {
 183         return read(fd, buf, len);
 184 }
 185
 186 void *get_wifi(void *buf, int *len)
 187 {
 188 #define BIT(n)  (1<<(n))
 189         struct bpf_hdr* bpfh = (struct bpf_hdr*) buf;
 190         struct ieee80211_radiotap_header* rth;
 191         uint32_t present;
 192         uint8_t rflags;
 193         void *ptr;
 194
 195         /* bpf */
 196         *len -= bpfh->bh_hdrlen;
 197
 198         if (bpfh->bh_caplen != *len) {
 199                 assert(bpfh->bh_caplen < *len);
 200                 *len = bpfh->bh_caplen;
 201         }
 202         assert(bpfh->bh_caplen == *len);
 203
 204         /* radiotap */
 205         rth = (struct ieee80211_radiotap_header*)
 206               ((char*)bpfh + bpfh->bh_hdrlen);
 207         /* XXX cache; drivers won't change this per-packet */
 208         /* check if FCS/CRC is included in packet */
 209         present = le32toh(rth->it_present);
 210         if (present & BIT(IEEE80211_RADIOTAP_FLAGS)) {
 211                 if (present & BIT(IEEE80211_RADIOTAP_TSFT))
 212                         rflags = ((const uint8_t *)rth)[8];
 213                 else
 214                         rflags = ((const uint8_t *)rth)[0];
 215         } else
 216                 rflags = 0;
 217         *len -= rth->it_len;
 218
 219         /* 802.11 CRC */
 220         if (rflags & IEEE80211_RADIOTAP_F_FCS)
 221                 *len -= IEEE80211_CRC_LEN;
 222
 223         ptr = (char*)rth + rth->it_len;
 224         return ptr;
 225 #undef BIT
 226 }
 227
 228 int send_ack(int fd, char *mac)
 229 {
 230         static char buf[2+2+6];
 231         static char *p = NULL;
 232         int rc;
 233
 234         if (!p) {
 235                 memset(buf, 0, sizeof(buf));
 236                 buf[0] |= IEEE80211_FC0_TYPE_CTL | IEEE80211_FC0_SUBTYPE_ACK;
 237                 p = &buf[4];
 238         }
 239
 240         memcpy(p, mac, 6);
 241
 242         rc = inject(fd, buf, sizeof(buf));
 243         return rc;
 244 }
 245
 246 int open_tap(char *iface)
 247 {
 248         char buf[64];
 249
 250         snprintf(buf, sizeof(buf), "/dev/%s", iface);
 251         return open(buf, O_RDWR);
 252 }
 253
 254 int set_iface_mac(char *iface, char *mac)
 255 {
 256         int s, rc;
 257         struct ifreq ifr;
 258
 259         s = socket(PF_INET, SOCK_DGRAM, 0);
 260         if (s == -1)
 261                 return -1;
 262
 263         memset(&ifr, 0, sizeof(ifr));
 264         strcpy(ifr.ifr_name, iface);
 265
 266         ifr.ifr_addr.sa_family = AF_LINK;
 267         ifr.ifr_addr.sa_len = 6;
 268         memcpy(ifr.ifr_addr.sa_data, mac, 6);
 269
 270         rc = ioctl(s, SIOCSIFLLADDR, &ifr);
 271
 272         close(s);
 273
 274         return rc;
 275 }
 276
 277 int str2wep(char *wep, int *len, char *str)
 278 {
 279         int klen;
 280
 281         klen = strlen(str);
 282         if (klen % 2)
 283                 return -1;
 284         klen /= 2;
 285
 286         if (klen != 5 && klen != 13)
 287                 return -1;
 288
 289         *len = klen;
 290
 291         while (klen--) {
 292                 unsigned int x;
 293
 294                 if (sscanf(str, "%2x", &x) != 1)
 295                         return -1;
 296
 297                 *wep = (unsigned char) x;
 298                 wep++;
 299                 str += 2;
 300         }
 301
 302         return 0;
 303 }
 304
 305 int wep_decrypt(struct ieee80211_frame *wh, int len, char *key, int klen)
 306 {
 307         RC4_KEY k;
 308         char seed[64];
 309         char *p = (char*) (wh+1);
 310         uLong crc = crc32(0L, Z_NULL, 0);
 311         uLong *pcrc;
 312
 313         assert(sizeof(seed) >= klen + 3);
 314         memcpy(seed, p, 3);
 315         memcpy(&seed[3], key, klen);
 316
 317         RC4_set_key(&k, klen+3, seed);
 318
 319         len -= sizeof(*wh);
 320         len -= 4;
 321         p += 4;
 322         RC4(&k, len, p, p);
 323
 324         crc = crc32(crc, p, len - 4);
 325         pcrc = (uLong*) (p+len-4);
 326
 327         if (*pcrc == crc)
 328                 return 0;
 329
 330         return -1;
 331 }
 332
 333 void wep_encrypt(struct ieee80211_frame *wh, int len, char *key, int klen)
 334 {
 335         RC4_KEY k;
 336         char seed[64];
 337         char *p = (char*) (wh+1);
 338         uLong crc = crc32(0L, Z_NULL, 0);
 339         uLong *pcrc;
 340
 341         assert(sizeof(seed) >= klen + 3);
 342         memcpy(seed, p, 3);
 343         memcpy(&seed[3], key, klen);
 344
 345         RC4_set_key(&k, klen+3, seed);
 346
 347         len -= sizeof(*wh);
 348         p += 4;
 349         crc = crc32(crc, p, len - 4);
 350         pcrc = (uLong*) (p+len-4);
 351         *pcrc = crc;
 352
 353         RC4(&k, len, p, p);
 354 }
 355
 356 int frame_type(struct ieee80211_frame *wh, int type, int stype)
 357 {
 358         if ((wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) != type)
 359                 return 0;
 360
 361         if ((wh->i_fc[0] & IEEE80211_FC0_SUBTYPE_MASK) != stype)
 362                 return 0;
 363
 364         return 1;
 365 }
 366
 367 void hexdump(void *b, int len)
 368 {
 369         unsigned char *p = (unsigned char*) b;
 370
 371         while (len--)
 372                 printf("%.2X ", *p++);
 373         printf("\n");
 374 }
 375
 376 int elapsed(struct timeval *past, struct timeval *now)
 377 {
 378         int el;
 379
 380         el = now->tv_sec - past->tv_sec;
 381         assert(el >= 0);
 382         if (el == 0) {
 383                 el = now->tv_usec - past->tv_usec;
 384         } else {
 385                 el = (el - 1)*1000*1000;
 386                 el += 1000*1000-past->tv_usec;
 387                 el += now->tv_usec;
 388         }
 389
 390         return el;
 391 }
 392
 393 static int is_arp(struct ieee80211_frame *wh, int len)
 394 {
 395         /* XXX */
 396         if (len > (sizeof(*wh) + 4 + 4 + 39))
 397                 return 0;
 398
 399         return 1;
 400 }
 401
 402 char *known_pt(struct ieee80211_frame *wh, int *len)
 403 {
 404         static char *known_pt_arp = "\xAA\xAA\x03\x00\x00\x00\x08\x06";
 405         static char *known_pt_ip = "\xAA\xAA\x03\x00\x00\x00\x08\x00";
 406         int arp;
 407
 408         arp = is_arp(wh, *len);
 409         *len = 8;
 410         if (arp)
 411                 return known_pt_arp;
 412         else
 413                 return known_pt_ip;
 414 }