sys/src/9/pc/wifi.c

   1 #include "u.h"
   2 #include "../port/lib.h"
   3 #include "mem.h"
   4 #include "dat.h"
   5 #include "fns.h"
   6 #include "io.h"
   7 #include "ureg.h"
   8 #include "../port/error.h"
   9 #include "../port/netif.h"
  10
  11 #include "etherif.h"
  12 #include "wifi.h"
  13
  14 #include <libsec.h>
  15
  16 typedef struct SNAP SNAP;
  17 struct SNAP
  18 {
  19         uchar   dsap;
  20         uchar   ssap;
  21         uchar   control;
  22         uchar   orgcode[3];
  23         uchar   type[2];
  24 };
  25
  26 enum {
  27         WIFIHDRSIZE = 2+2+3*6+2,
  28         SNAPHDRSIZE = 8,
  29 };
  30
  31 static char Sconn[] = "connecting";
  32 static char Sauth[] = "authenticated";
  33 static char Sneedauth[] = "need authentication";
  34 static char Sunauth[] = "unauthenticated";
  35
  36 static char Sassoc[] = "associated";
  37 static char Sunassoc[] = "unassociated";
  38 static char Sblocked[] = "blocked";     /* no keys negotiated. only pass EAPOL frames */
  39
  40 static uchar basicrates[] = {
  41         0x80 | 2,       /* 1.0  Mb/s */
  42         0x80 | 4,       /* 2.0  Mb/s */
  43         0x80 | 11,      /* 5.5  Mb/s */
  44         0x80 | 22,      /* 11.0 Mb/s */
  45
  46         0
  47 };
  48
  49 static Block* wifidecrypt(Wifi *, Wnode *, Block *);
  50 static Block* wifiencrypt(Wifi *, Wnode *, Block *);
  51 static void freewifikeys(Wifi *, Wnode *);
  52
  53 static uchar*
  54 srcaddr(Wifipkt *w)
  55 {
  56         if((w->fc[1] & 0x02) == 0)
  57                 return w->a2;
  58         if((w->fc[1] & 0x01) == 0)
  59                 return w->a3;
  60         return w->a4;
  61 }
  62 static uchar*
  63 dstaddr(Wifipkt *w)
  64 {
  65         if((w->fc[1] & 0x01) != 0)
  66                 return w->a3;
  67         return w->a1;
  68 }
  69
  70 int
  71 wifihdrlen(Wifipkt *w)
  72 {
  73         int n;
  74
  75         n = WIFIHDRSIZE;
  76         if((w->fc[0] & 0x0c) == 0x08)
  77                 if((w->fc[0] & 0xf0) == 0x80){  /* QOS */
  78                         n += 2;
  79                         if(w->fc[1] & 0x80)
  80                                 n += 4;
  81                 }
  82         if((w->fc[1] & 3) == 0x03)
  83                 n += Eaddrlen;
  84         return n;
  85 }
  86
  87 void
  88 wifiiq(Wifi *wifi, Block *b)
  89 {
  90         SNAP s;
  91         Wifipkt h, *w;
  92         Etherpkt *e;
  93         int hdrlen;
  94
  95         if(BLEN(b) < WIFIHDRSIZE)
  96                 goto drop;
  97         w = (Wifipkt*)b->rp;
  98         hdrlen = wifihdrlen(w);
  99         if(BLEN(b) < hdrlen)
 100                 goto drop;
 101         if(w->fc[1] & 0x40){
 102                 /* encrypted */
 103                 qpass(wifi->iq, b);
 104                 return;
 105         }
 106         switch(w->fc[0] & 0x0c){
 107         case 0x00:      /* management */
 108                 if((w->fc[1] & 3) != 0x00)      /* STA->STA */
 109                         break;
 110                 qpass(wifi->iq, b);
 111                 return;
 112         case 0x04:      /* control */
 113                 break;
 114         case 0x08:      /* data */
 115                 b->rp += hdrlen;
 116                 switch(w->fc[0] & 0xf0){
 117                 default:
 118                         goto drop;
 119                 case 0x80:      /* QOS */
 120                 case 0x00:
 121                         break;
 122                 }
 123                 if(BLEN(b) < SNAPHDRSIZE)
 124                         break;
 125                 memmove(&s, b->rp, SNAPHDRSIZE);
 126                 if(s.dsap != 0xAA || s.ssap != 0xAA || s.control != 3)
 127                         break;
 128                 if(s.orgcode[0] != 0 || s.orgcode[1] != 0 || s.orgcode[2] != 0)
 129                         break;
 130                 b->rp += SNAPHDRSIZE-ETHERHDRSIZE;
 131                 h = *w;
 132                 e = (Etherpkt*)b->rp;
 133                 memmove(e->d, dstaddr(&h), Eaddrlen);
 134                 memmove(e->s, srcaddr(&h), Eaddrlen);
 135                 memmove(e->type, s.type, 2);
 136                 etheriq(wifi->ether, b, 1);
 137                 return;
 138         }
 139 drop:
 140         freeb(b);
 141 }
 142
 143 static void
 144 wifitx(Wifi *wifi, Wnode *wn, Block *b)
 145 {
 146         Wifipkt *w;
 147         uint seq;
 148
 149         wn->lastsend = MACHP(0)->ticks;
 150
 151         seq = incref(&wifi->txseq);
 152         seq <<= 4;
 153
 154         w = (Wifipkt*)b->rp;
 155         w->dur[0] = 0;
 156         w->dur[1] = 0;
 157         w->seq[0] = seq;
 158         w->seq[1] = seq>>8;
 159
 160         if((w->fc[0] & 0x0c) != 0x00){
 161                 b = wifiencrypt(wifi, wn, b);
 162                 if(b == nil)
 163                         return;
 164         }
 165
 166         if((wn->txcount++ & 255) == 255){
 167                 if(wn->actrate != nil && wn->actrate < wn->maxrate)
 168                         wn->actrate++;
 169         }
 170
 171         (*wifi->transmit)(wifi, wn, b);
 172 }
 173
 174 static Wnode*
 175 nodelookup(Wifi *wifi, uchar *bssid, int new)
 176 {
 177         Wnode *wn, *nn;
 178
 179         if(memcmp(bssid, wifi->ether->bcast, Eaddrlen) == 0)
 180                 return nil;
 181         if((wn = wifi->bss) != nil){
 182                 if(memcmp(wn->bssid, bssid, Eaddrlen) == 0){
 183                         wn->lastseen = MACHP(0)->ticks;
 184                         return wn;
 185                 }
 186         }
 187         if((nn = wifi->node) == wn)
 188                 nn++;
 189         for(wn = wifi->node; wn != &wifi->node[nelem(wifi->node)]; wn++){
 190                 if(wn == wifi->bss)
 191                         continue;
 192                 if(memcmp(wn->bssid, bssid, Eaddrlen) == 0){
 193                         wn->lastseen = MACHP(0)->ticks;
 194                         return wn;
 195                 }
 196                 if((long)(wn->lastseen - nn->lastseen) < 0)
 197                         nn = wn;
 198         }
 199         if(!new)
 200                 return nil;
 201         freewifikeys(wifi, nn);
 202         memset(nn, 0, sizeof(Wnode));
 203         memmove(nn->bssid, bssid, Eaddrlen);
 204         nn->lastseen = MACHP(0)->ticks;
 205         return nn;
 206 }
 207
 208 void
 209 wifitxfail(Wifi *wifi, Block *b)
 210 {
 211         Wifipkt *w;
 212         Wnode *wn;
 213
 214         if(b == nil)
 215                 return;
 216         w = (Wifipkt*)b->rp;
 217         wn = nodelookup(wifi, w->a1, 0);
 218         if(wn == nil)
 219                 return;
 220         wn->txerror++;
 221         if(wn->actrate != nil && wn->actrate > wn->minrate)
 222                 wn->actrate--;
 223 }
 224
 225 static uchar*
 226 putrates(uchar *p, uchar *rates)
 227 {
 228         int n, m;
 229
 230         n = m = strlen((char*)rates);
 231         if(n > 8)
 232                 n = 8;
 233         /* supported rates */
 234         *p++ = 1;
 235         *p++ = n;
 236         memmove(p, rates, n);
 237         p += n;
 238         if(m > 8){
 239                 /* extended supported rates */
 240                 *p++ = 50;
 241                 *p++ = m;
 242                 memmove(p, rates, m);
 243                 p += m;
 244         }
 245         return p;
 246 }
 247
 248 static void
 249 wifiprobe(Wifi *wifi, Wnode *wn)
 250 {
 251         Wifipkt *w;
 252         Block *b;
 253         uchar *p;
 254         int n;
 255
 256         n = strlen(wifi->essid);
 257         if(n == 0){
 258                 /* no specific essid, just tell driver to tune channel */
 259                 (*wifi->transmit)(wifi, wn, nil);
 260                 return;
 261         }
 262
 263         b = allocb(WIFIHDRSIZE + 512);
 264         w = (Wifipkt*)b->wp;
 265         w->fc[0] = 0x40;        /* probe request */
 266         w->fc[1] = 0x00;        /* STA->STA */
 267         memmove(w->a1, wifi->ether->bcast, Eaddrlen);   /* ??? */
 268         memmove(w->a2, wifi->ether->ea, Eaddrlen);
 269         memmove(w->a3, wifi->ether->bcast, Eaddrlen);
 270         b->wp += WIFIHDRSIZE;
 271         p = b->wp;
 272
 273         *p++ = 0;       /* set */
 274         *p++ = n;
 275         memmove(p, wifi->essid, n);
 276         p += n;
 277
 278         p = putrates(p, wifi->rates);
 279
 280         *p++ = 3;       /* ds parameter set */
 281         *p++ = 1;
 282         *p++ = wn->channel;
 283
 284         b->wp = p;
 285         wifitx(wifi, wn, b);
 286 }
 287
 288 static void
 289 sendauth(Wifi *wifi, Wnode *bss)
 290 {
 291         Wifipkt *w;
 292         Block *b;
 293         uchar *p;
 294
 295         b = allocb(WIFIHDRSIZE + 3*2);
 296         w = (Wifipkt*)b->wp;
 297         w->fc[0] = 0xB0;        /* auth request */
 298         w->fc[1] = 0x00;        /* STA->STA */
 299         memmove(w->a1, bss->bssid, Eaddrlen);   /* ??? */
 300         memmove(w->a2, wifi->ether->ea, Eaddrlen);
 301         memmove(w->a3, bss->bssid, Eaddrlen);
 302         b->wp += WIFIHDRSIZE;
 303         p = b->wp;
 304         *p++ = 0;       /* alg */
 305         *p++ = 0;
 306         *p++ = 1;       /* seq */
 307         *p++ = 0;
 308         *p++ = 0;       /* status */
 309         *p++ = 0;
 310         b->wp = p;
 311
 312         bss->aid = 0;
 313
 314         wifitx(wifi, bss, b);
 315 }
 316
 317 static void
 318 sendassoc(Wifi *wifi, Wnode *bss)
 319 {
 320         Wifipkt *w;
 321         Block *b;
 322         uchar *p;
 323         int n;
 324
 325         b = allocb(WIFIHDRSIZE + 512);
 326         w = (Wifipkt*)b->wp;
 327         w->fc[0] = 0x00;        /* assoc request */
 328         w->fc[1] = 0x00;        /* STA->STA */
 329         memmove(w->a1, bss->bssid, Eaddrlen);   /* ??? */
 330         memmove(w->a2, wifi->ether->ea, Eaddrlen);
 331         memmove(w->a3, bss->bssid, Eaddrlen);
 332         b->wp += WIFIHDRSIZE;
 333         p = b->wp;
 334         *p++ = 1;       /* capinfo */
 335         *p++ = 0;
 336         *p++ = 16;      /* interval */
 337         *p++ = 16>>8;
 338
 339         n = strlen(bss->ssid);
 340         *p++ = 0;       /* SSID */
 341         *p++ = n;
 342         memmove(p, bss->ssid, n);
 343         p += n;
 344
 345         p = putrates(p, wifi->rates);
 346
 347         n = bss->rsnelen;
 348         if(n > 0){
 349                 memmove(p, bss->rsne, n);
 350                 p += n;
 351         }
 352
 353         b->wp = p;
 354         wifitx(wifi, bss, b);
 355 }
 356
 357 static void
 358 setstatus(Wifi *wifi, Wnode *wn, char *new)
 359 {
 360         char *old;
 361
 362         old = wn->status;
 363         wn->status = new;
 364         if(wifi->debug && new != old)
 365                 print("#l%d: status %E: %s -> %s (from pc=%#p)\n",
 366                         wifi->ether->ctlrno,
 367                         wn->bssid,
 368                         old, new,
 369                         getcallerpc(&wifi));
 370 }
 371
 372 static void
 373 recvassoc(Wifi *wifi, Wnode *wn, uchar *d, int len)
 374 {
 375         uint s;
 376
 377         if(len < 2+2+2)
 378                 return;
 379
 380         d += 2; /* caps */
 381         s = d[0] | d[1]<<8;
 382         d += 2;
 383         switch(s){
 384         case 0x00:
 385                 wn->aid = d[0] | d[1]<<8;
 386                 if(wn->rsnelen > 0)
 387                         setstatus(wifi, wn, Sblocked);
 388                 else
 389                         setstatus(wifi, wn, Sassoc);
 390                 break;
 391         default:
 392                 wn->aid = 0;
 393                 setstatus(wifi, wn, Sunassoc);
 394         }
 395 }
 396
 397 static void
 398 recvbeacon(Wifi *wifi, Wnode *wn, uchar *d, int len)
 399 {
 400         static uchar wpa1oui[4] = { 0x00, 0x50, 0xf2, 0x01 };
 401         uchar *e, *x, *p, t;
 402         int rsnset;
 403
 404         len -= 8+2+2;
 405         if(len < 0)
 406                 return;
 407
 408         d += 8; /* timestamp */
 409         wn->ival = d[0] | d[1]<<8;
 410         d += 2;
 411         wn->cap = d[0] | d[1]<<8;
 412         d += 2;
 413
 414         rsnset = 0;
 415         for(e = d + len; d+2 <= e; d = x){
 416                 d += 2;
 417                 x = d + d[-1];
 418                 if(x > e)
 419                         break;  /* truncated */
 420                 t = d[-2];
 421                 switch(t){
 422                 case 0:         /* SSID */
 423                         len = 0;
 424                         while(len < Essidlen && d+len < x && d[len] != 0)
 425                                 len++;
 426                         if(len == 0)
 427                                 continue;
 428                         if(len != strlen(wn->ssid) || strncmp(wn->ssid, (char*)d, len) != 0){
 429                                 strncpy(wn->ssid, (char*)d, len);
 430                                 wn->ssid[len] = 0;
 431                         }
 432                         break;
 433                 case 1:         /* supported rates */
 434                 case 50:        /* extended rates */
 435                         if(wn->minrate != nil || wn->maxrate != nil || wifi->rates == nil)
 436                                 break;  /* already set */
 437                         while(d < x){
 438                                 t = *d++ & 0x7f;
 439                                 for(p = wifi->rates; *p != 0; p++){
 440                                         if((*p & 0x7f) == t){
 441                                                 if(wn->minrate == nil || t < (*wn->minrate & 0x7f))
 442                                                         wn->minrate = p;
 443                                                 if(wn->maxrate == nil || t > (*wn->maxrate & 0x7f))
 444                                                         wn->maxrate = p;
 445                                                 break;
 446                                         }
 447                                 }
 448                                 wn->actrate = wn->maxrate;
 449                         }
 450                         break;
 451                 case 3:         /* DSPARAMS */
 452                         if(d != x)
 453                                 wn->channel = d[0];
 454                         break;
 455                 case 221:       /* vendor specific */
 456                         len = x - d;
 457                         if(rsnset || len < sizeof(wpa1oui) || memcmp(d, wpa1oui, sizeof(wpa1oui)) != 0)
 458                                 break;
 459                         /* no break */
 460                 case 48:        /* RSN information */
 461                         len = x - &d[-2];
 462                         memmove(wn->brsne, &d[-2], len);
 463                         wn->brsnelen = len;
 464                         rsnset = 1;
 465                         break;
 466                 }
 467         }
 468 }
 469
 470 static void
 471 freewifikeys(Wifi *wifi, Wnode *wn)
 472 {
 473         int i;
 474
 475         wlock(&wifi->crypt);
 476         for(i=0; i<nelem(wn->rxkey); i++){
 477                 secfree(wn->rxkey[i]);
 478                 wn->rxkey[i] = nil;
 479         }
 480         for(i=0; i<nelem(wn->txkey); i++){
 481                 secfree(wn->txkey[i]);
 482                 wn->txkey[i] = nil;
 483         }
 484         wunlock(&wifi->crypt);
 485 }
 486
 487 static void
 488 wifideauth(Wifi *wifi, Wnode *wn)
 489 {
 490         Ether *ether;
 491         Netfile *f;
 492         int i;
 493
 494         /* deassociate node, clear keys */
 495         setstatus(wifi, wn, Sunauth);
 496         freewifikeys(wifi, wn);
 497         wn->aid = 0;
 498
 499         if(wn == wifi->bss){
 500                 /* notify driver about node aid association */
 501                 (*wifi->transmit)(wifi, wn, nil);
 502
 503                 /* notify aux/wpa with a zero length write that we got deassociated from the ap */
 504                 ether = wifi->ether;
 505                 for(i=0; i<ether->nfile; i++){
 506                         f = ether->f[i];
 507                         if(f == nil || f->in == nil || f->inuse == 0 || f->type != 0x888e)
 508                                 continue;
 509                         qwrite(f->in, 0, 0);
 510                 }
 511         }
 512 }
 513
 514 /* check if a node qualifies as our bss matching bssid and essid */
 515 static int
 516 goodbss(Wifi *wifi, Wnode *wn)
 517 {
 518         if(memcmp(wifi->bssid, wifi->ether->bcast, Eaddrlen) != 0){
 519                 if(memcmp(wifi->bssid, wn->bssid, Eaddrlen) != 0)
 520                         return 0;       /* bssid doesnt match */
 521         } else if(wifi->essid[0] == 0)
 522                 return 0;       /* both bssid and essid unspecified */
 523         if(wifi->essid[0] != 0 && strcmp(wifi->essid, wn->ssid) != 0)
 524                 return 0;       /* essid doesnt match */
 525         return 1;
 526 }
 527
 528 static void
 529 wifiproc(void *arg)
 530 {
 531         Wifi *wifi;
 532         Wifipkt *w;
 533         Wnode *wn;
 534         Block *b;
 535
 536         b = nil;
 537         wifi = arg;
 538         while(waserror())
 539                 ;
 540         for(;;){
 541                 if(b != nil){
 542                         freeb(b);
 543                         b = nil;
 544                         continue;
 545                 }
 546                 if((b = qbread(wifi->iq, 100000)) == nil)
 547                         break;
 548                 w = (Wifipkt*)b->rp;
 549                 if(w->fc[1] & 0x40){
 550                         /* encrypted */
 551                         if((wn = nodelookup(wifi, w->a2, 0)) == nil)
 552                                 continue;
 553                         if((b = wifidecrypt(wifi, wn, b)) != nil){
 554                                 w = (Wifipkt*)b->rp;
 555                                 if(w->fc[1] & 0x40)
 556                                         continue;
 557                                 wifiiq(wifi, b);
 558                                 b = nil;
 559                         }
 560                         continue;
 561                 }
 562                 /* management */
 563                 if((w->fc[0] & 0x0c) != 0x00)
 564                         continue;
 565
 566                 switch(w->fc[0] & 0xf0){
 567                 case 0x50:      /* probe response */
 568                         if(wifi->debug)
 569                                 print("#l%d: got probe from %E\n", wifi->ether->ctlrno, w->a3);
 570                         /* no break */
 571                 case 0x80:      /* beacon */
 572                         if((wn = nodelookup(wifi, w->a3, 1)) == nil)
 573                                 continue;
 574                         b->rp += wifihdrlen(w);
 575                         recvbeacon(wifi, wn, b->rp, BLEN(b));
 576
 577                         if(wifi->bss == nil
 578                         && TK2MS(MACHP(0)->ticks - wn->lastsend) > 1000
 579                         && goodbss(wifi, wn)){
 580                                 setstatus(wifi, wn, Sconn);
 581                                 sendauth(wifi, wn);
 582                                 wifi->lastauth = wn->lastsend;
 583                         }
 584                         continue;
 585                 }
 586
 587                 if(memcmp(w->a1, wifi->ether->ea, Eaddrlen))
 588                         continue;
 589                 if((wn = nodelookup(wifi, w->a3, 0)) == nil)
 590                         continue;
 591                 switch(w->fc[0] & 0xf0){
 592                 case 0x10:      /* assoc response */
 593                 case 0x30:      /* reassoc response */
 594                         b->rp += wifihdrlen(w);
 595                         recvassoc(wifi, wn, b->rp, BLEN(b));
 596                         /* notify driver about node aid association */
 597                         if(wn == wifi->bss)
 598                                 (*wifi->transmit)(wifi, wn, nil);
 599                         break;
 600                 case 0xb0:      /* auth */
 601                         if(wifi->debug)
 602                                 print("#l%d: got auth from %E\n", wifi->ether->ctlrno, wn->bssid);
 603                         if(wn->brsnelen > 0 && wn->rsnelen == 0)
 604                                 setstatus(wifi, wn, Sneedauth);
 605                         else
 606                                 setstatus(wifi, wn, Sauth);
 607                         if(wifi->bss == nil && goodbss(wifi, wn)){
 608                                 wifi->bss = wn;
 609                                 if(wn->status == Sauth)
 610                                         sendassoc(wifi, wn);
 611                         }
 612                         break;
 613                 case 0xc0:      /* deauth */
 614                         if(wifi->debug)
 615                                 print("#l%d: got deauth from %E\n", wifi->ether->ctlrno, wn->bssid);
 616                         wifideauth(wifi, wn);
 617                         break;
 618                 }
 619         }
 620         pexit("wifi in queue closed", 1);
 621 }
 622
 623 static void
 624 wifietheroq(Wifi *wifi, Block *b)
 625 {
 626         Etherpkt e;
 627         Wifipkt h;
 628         int hdrlen;
 629         Wnode *wn;
 630         SNAP *s;
 631
 632         if(BLEN(b) < ETHERHDRSIZE)
 633                 goto drop;
 634         if((wn = wifi->bss) == nil)
 635                 goto drop;
 636
 637         memmove(&e, b->rp, ETHERHDRSIZE);
 638         b->rp += ETHERHDRSIZE;
 639
 640         if(wn->status == Sblocked){
 641                 /* only pass EAPOL frames when port is blocked */
 642                 if((e.type[0]<<8 | e.type[1]) != 0x888e)
 643                         goto drop;
 644         } else if(wn->status != Sassoc)
 645                 goto drop;
 646
 647         h.fc[0] = 0x08; /* data */
 648         memmove(h.a1, wn->bssid, Eaddrlen);
 649         if(memcmp(e.s, wifi->ether->ea, Eaddrlen) == 0) {
 650                 h.fc[1] = 0x01; /* STA->AP */
 651         } else {
 652                 h.fc[1] = 0x03; /* AP->AP (WDS) */
 653                 memmove(h.a2, wifi->ether->ea, Eaddrlen);
 654         }
 655         memmove(dstaddr(&h), e.d, Eaddrlen);
 656         memmove(srcaddr(&h), e.s, Eaddrlen);
 657
 658         hdrlen = wifihdrlen(&h);
 659         b = padblock(b, hdrlen + SNAPHDRSIZE);
 660         memmove(b->rp, &h, hdrlen);
 661         s = (SNAP*)(b->rp + hdrlen);
 662         s->dsap = s->ssap = 0xAA;
 663         s->control = 0x03;
 664         s->orgcode[0] = 0;
 665         s->orgcode[1] = 0;
 666         s->orgcode[2] = 0;
 667         memmove(s->type, e.type, 2);
 668
 669         wifitx(wifi, wn, b);
 670         return;
 671 drop:
 672         freeb(b);
 673 }
 674
 675 static void
 676 wifoproc(void *arg)
 677 {
 678         Ether *ether;
 679         Wifi *wifi;
 680         Block *b;
 681
 682         wifi = arg;
 683         ether = wifi->ether;
 684         while(waserror())
 685                 ;
 686         while((b = qbread(ether->oq, 1000000)) != nil)
 687                 wifietheroq(wifi, b);
 688         pexit("ether out queue closed", 1);
 689 }
 690
 691 static void
 692 wifsproc(void *arg)
 693 {
 694         Ether *ether;
 695         Wifi *wifi;
 696         Wnode wnscan;
 697         Wnode *wn;
 698         ulong now, tmout;
 699         uchar *rate;
 700
 701         wifi = arg;
 702         ether = wifi->ether;
 703
 704         wn = &wnscan;
 705         memset(wn, 0, sizeof(*wn));
 706         memmove(wn->bssid, ether->bcast, Eaddrlen);
 707
 708         while(waserror())
 709                 ;
 710 Scan:
 711         /* scan for access point */
 712         while(wifi->bss == nil){
 713                 ether->link = 0;
 714                 wnscan.channel = 1 + ((wnscan.channel+4) % 13);
 715                 wifiprobe(wifi, &wnscan);
 716                 do {
 717                         tsleep(&up->sleep, return0, 0, 200);
 718                         now = MACHP(0)->ticks;
 719                 } while(TK2MS(now-wifi->lastauth) < 1000);
 720         }
 721
 722         /* maintain access point */
 723         tmout = 0;
 724         while((wn = wifi->bss) != nil){
 725                 ether->link = (wn->status == Sassoc) || (wn->status == Sblocked);
 726                 if(ether->link && (rate = wn->actrate) != nil)
 727                         ether->mbps = ((*rate & 0x7f)+1)/2;
 728                 now = MACHP(0)->ticks;
 729                 if(wn->status != Sneedauth && TK2SEC(now - wn->lastseen) > 60 || goodbss(wifi, wn) == 0){
 730                         wifideauth(wifi, wn);
 731                         wifi->bss = nil;
 732                         break;
 733                 }
 734                 if(TK2MS(now - wn->lastsend) > 1000){
 735                         if(wn->status == Sauth && (++tmout & 7) == 0)
 736                                 wifideauth(wifi, wn);   /* stuck in auth, start over */
 737                         if(wn->status == Sconn || wn->status == Sunauth)
 738                                 sendauth(wifi, wn);
 739                         if(wn->status == Sauth)
 740                                 sendassoc(wifi, wn);
 741                 }
 742                 tsleep(&up->sleep, return0, 0, 500);
 743         }
 744         goto Scan;
 745 }
 746
 747 Wifi*
 748 wifiattach(Ether *ether, void (*transmit)(Wifi*, Wnode*, Block*))
 749 {
 750         char name[32];
 751         Wifi *wifi;
 752
 753         wifi = malloc(sizeof(Wifi));
 754         if(wifi == nil)
 755                 error(Enomem);
 756         wifi->iq = qopen(ether->limit, 0, 0, 0);
 757         if(wifi->iq == nil){
 758                 free(wifi);
 759                 error(Enomem);
 760         }
 761         wifi->ether = ether;
 762         wifi->transmit = transmit;
 763
 764         wifi->rates = basicrates;
 765
 766         wifi->essid[0] = 0;
 767         memmove(wifi->bssid, ether->bcast, Eaddrlen);
 768
 769         wifi->lastauth = MACHP(0)->ticks;
 770
 771         snprint(name, sizeof(name), "#l%dwifi", ether->ctlrno);
 772         kproc(name, wifiproc, wifi);
 773         snprint(name, sizeof(name), "#l%dwifo", ether->ctlrno);
 774         kproc(name, wifoproc, wifi);
 775         snprint(name, sizeof(name), "#l%dwifs", ether->ctlrno);
 776         kproc(name, wifsproc, wifi);
 777
 778         return wifi;
 779 }
 780
 781 static int
 782 hextob(char *s, char **sp, uchar *b, int n)
 783 {
 784         int r;
 785
 786         n <<= 1;
 787         for(r = 0; r < n && *s; s++){
 788                 *b <<= 4;
 789                 if(*s >= '0' && *s <= '9')
 790                         *b |= (*s - '0');
 791                 else if(*s >= 'a' && *s <= 'f')
 792                         *b |= 10+(*s - 'a');
 793                 else if(*s >= 'A' && *s <= 'F')
 794                         *b |= 10+(*s - 'A');
 795                 else break;
 796                 if((++r & 1) == 0)
 797                         b++;
 798         }
 799         if(sp != nil)
 800                 *sp = s;
 801         return r >> 1;
 802 }
 803
 804 static char *ciphers[] = {
 805         [0]     "clear",
 806         [TKIP]  "tkip",
 807         [CCMP]  "ccmp",
 808 };
 809
 810 static Wkey*
 811 parsekey(char *s)
 812 {
 813         char buf[256], *p;
 814         uchar key[32];
 815         int i, n;
 816         Wkey *k;
 817
 818         strncpy(buf, s, sizeof(buf)-1);
 819         buf[sizeof(buf)-1] = 0;
 820         if((p = strchr(buf, ':')) != nil)
 821                 *p++ = 0;
 822         else
 823                 p = buf;
 824         n = hextob(p, &p, key, sizeof(key));
 825         for(i=0; i<nelem(ciphers); i++)
 826                 if(strcmp(ciphers[i], buf) == 0)
 827                         break;
 828         switch(i){
 829         case 0:
 830                 k = secalloc(sizeof(Wkey));
 831                 break;
 832         case TKIP:
 833                 if(n != 32)
 834                         return nil;
 835                 k = secalloc(sizeof(Wkey) + n);
 836                 memmove(k->key, key, n);
 837                 break;
 838         case CCMP:
 839                 if(n != 16)
 840                         return nil;
 841                 k = secalloc(sizeof(Wkey) + sizeof(AESstate));
 842                 setupAESstate((AESstate*)k->key, key, n, nil);
 843                 break;
 844         default:
 845                 return nil;
 846         }
 847         memset(key, 0, sizeof(key));
 848         if(*p == '@')
 849                 k->tsc = strtoull(++p, nil, 16);
 850         k->len = n;
 851         k->cipher = i;
 852         return k;
 853 }
 854
 855 void
 856 wificfg(Wifi *wifi, char *opt)
 857 {
 858         char *p, buf[64];
 859         int n;
 860
 861         if(strncmp(opt, "debug=", 6))
 862         if(strncmp(opt, "essid=", 6))
 863         if(strncmp(opt, "bssid=", 6))
 864                 return;
 865         if((p = strchr(opt, '=')) == nil)
 866                 return;
 867         if(waserror())
 868                 return;
 869         n = snprint(buf, sizeof(buf), "%.*s %q", (int)(p - opt), opt, p+1);
 870         wifictl(wifi, buf, n);
 871         poperror();
 872 }
 873
 874 enum {
 875         CMdebug,
 876         CMessid,
 877         CMauth,
 878         CMbssid,
 879         CMrxkey0,
 880         CMrxkey1,
 881         CMrxkey2,
 882         CMrxkey3,
 883         CMrxkey4,
 884         CMtxkey0,
 885 };
 886
 887 static Cmdtab wifictlmsg[] =
 888 {
 889         CMdebug,        "debug",        0,
 890         CMessid,        "essid",        0,
 891         CMauth,         "auth",         0,
 892         CMbssid,        "bssid",        0,
 893
 894         CMrxkey0,       "rxkey0",       0,      /* group keys */
 895         CMrxkey1,       "rxkey1",       0,
 896         CMrxkey2,       "rxkey2",       0,
 897         CMrxkey3,       "rxkey3",       0,
 898
 899         CMrxkey4,       "rxkey",        0,      /* peerwise keys */
 900         CMtxkey0,       "txkey",        0,
 901
 902         CMtxkey0,       "txkey0",       0,
 903 };
 904
 905 long
 906 wifictl(Wifi *wifi, void *buf, long n)
 907 {
 908         uchar addr[Eaddrlen];
 909         Cmdbuf *cb;
 910         Cmdtab *ct;
 911         Wnode *wn;
 912         Wkey *k, **kk;
 913
 914         cb = nil;
 915         if(waserror()){
 916                 free(cb);
 917                 nexterror();
 918         }
 919         if(wifi->debug)
 920                 print("#l%d: wifictl: %.*s\n", wifi->ether->ctlrno, (int)n, buf);
 921         memmove(addr, wifi->ether->bcast, Eaddrlen);
 922         wn = wifi->bss;
 923         cb = parsecmd(buf, n);
 924         ct = lookupcmd(cb, wifictlmsg, nelem(wifictlmsg));
 925         if(ct->index >= CMauth){
 926                 if(cb->nf > 1 && (ct->index == CMbssid || ct->index >= CMrxkey0)){
 927                         if(parseether(addr, cb->f[1]) == 0){
 928                                 cb->f++;
 929                                 cb->nf--;
 930                                 wn = nodelookup(wifi, addr, 0);
 931                         }
 932                 }
 933                 if(wn == nil && ct->index != CMbssid)
 934                         error("missing node");
 935         }
 936         switch(ct->index){
 937         case CMdebug:
 938                 if(cb->f[1] != nil)
 939                         wifi->debug = atoi(cb->f[1]);
 940                 else
 941                         wifi->debug ^= 1;
 942                 print("#l%d: debug: %d\n", wifi->ether->ctlrno, wifi->debug);
 943                 break;
 944         case CMessid:
 945                 if(cb->f[1] != nil)
 946                         strncpy(wifi->essid, cb->f[1], Essidlen);
 947                 else
 948                         wifi->essid[0] = 0;
 949         Findbss:
 950                 wn = wifi->bss;
 951                 if(wn != nil){
 952                         if(goodbss(wifi, wn))
 953                                 break;
 954                         wifideauth(wifi, wn);
 955                 }
 956                 wifi->bss = nil;
 957                 if(wifi->essid[0] == 0 && memcmp(wifi->bssid, wifi->ether->bcast, Eaddrlen) == 0)
 958                         break;
 959                 for(wn = wifi->node; wn != &wifi->node[nelem(wifi->node)]; wn++)
 960                         if(goodbss(wifi, wn)){
 961                                 setstatus(wifi, wn, Sconn);
 962                                 sendauth(wifi, wn);
 963                         }
 964                 break;
 965         case CMbssid:
 966                 memmove(wifi->bssid, addr, Eaddrlen);
 967                 goto Findbss;
 968         case CMauth:
 969                 freewifikeys(wifi, wn);
 970                 if(cb->f[1] == nil)
 971                         wn->rsnelen = 0;
 972                 else
 973                         wn->rsnelen = hextob(cb->f[1], nil, wn->rsne, sizeof(wn->rsne));
 974                 if(wn->aid == 0){
 975                         setstatus(wifi, wn, Sconn);
 976                         sendauth(wifi, wn);
 977                 } else {
 978                         setstatus(wifi, wn, Sauth);
 979                         sendassoc(wifi, wn);
 980                 }
 981                 break;
 982         case CMrxkey0: case CMrxkey1: case CMrxkey2: case CMrxkey3: case CMrxkey4:
 983         case CMtxkey0:
 984                 if(cb->f[1] == nil)
 985                         error(Ebadarg);
 986                 k = parsekey(cb->f[1]);
 987                 if(k == nil)
 988                         error("bad key");
 989                 memset(cb->f[1], 0, strlen(cb->f[1]));
 990                 if(k->cipher == 0){
 991                         secfree(k);
 992                         k = nil;
 993                 }
 994                 if(ct->index < CMtxkey0)
 995                         kk = &wn->rxkey[ct->index - CMrxkey0];
 996                 else
 997                         kk = &wn->txkey[ct->index - CMtxkey0];
 998                 wlock(&wifi->crypt);
 999                 secfree(*kk);
1000                 *kk = k;
1001                 wunlock(&wifi->crypt);
1002                 if(ct->index >= CMtxkey0 && wn->status == Sblocked)
1003                         setstatus(wifi, wn, Sassoc);
1004                 break;
1005         }
1006         poperror();
1007         free(cb);
1008         return n;
1009 }
1010
1011 long
1012 wifistat(Wifi *wifi, void *buf, long n, ulong off)
1013 {
1014         static uchar zeros[Eaddrlen];
1015         char *s, *p, *e;
1016         Wnode *wn;
1017         Wkey *k;
1018         long now;
1019         int i;
1020
1021         p = s = smalloc(4096);
1022         e = s + 4096;
1023
1024         wn = wifi->bss;
1025         if(wn != nil){
1026                 p = seprint(p, e, "essid: %s\n", wn->ssid);
1027                 p = seprint(p, e, "bssid: %E\n", wn->bssid);
1028                 p = seprint(p, e, "status: %s\n", wn->status);
1029                 p = seprint(p, e, "channel: %.2d\n", wn->channel);
1030
1031                 /* only print key ciphers and key length */
1032                 rlock(&wifi->crypt);
1033                 for(i = 0; i<nelem(wn->rxkey); i++){
1034                         if((k = wn->rxkey[i]) != nil)
1035                                 p = seprint(p, e, "rxkey%d: %s:[%d]\n", i,
1036                                         ciphers[k->cipher], k->len);
1037                 }
1038                 for(i = 0; i<nelem(wn->txkey); i++){
1039                         if((k = wn->txkey[i]) != nil)
1040                                 p = seprint(p, e, "txkey%d: %s:[%d]\n", i,
1041                                         ciphers[k->cipher], k->len);
1042                 }
1043                 runlock(&wifi->crypt);
1044
1045                 if(wn->brsnelen > 0){
1046                         p = seprint(p, e, "brsne: ");
1047                         for(i=0; i<wn->brsnelen; i++)
1048                                 p = seprint(p, e, "%.2X", wn->brsne[i]);
1049                         p = seprint(p, e, "\n");
1050                 }
1051         } else {
1052                 p = seprint(p, e, "essid: %s\n", wifi->essid);
1053                 p = seprint(p, e, "bssid: %E\n", wifi->bssid);
1054         }
1055
1056         now = MACHP(0)->ticks;
1057         for(wn = wifi->node; wn != &wifi->node[nelem(wifi->node)]; wn++){
1058                 if(wn->lastseen == 0)
1059                         continue;
1060                 p = seprint(p, e, "node: %E %.4x %-11ld %.2d %s\n",
1061                         wn->bssid, wn->cap, TK2MS(now - wn->lastseen), wn->channel, wn->ssid);
1062         }
1063         n = readstr(off, buf, n, s);
1064         free(s);
1065         return n;
1066 }
1067
1068 static void tkipencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc);
1069 static int tkipdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc);
1070 static void ccmpencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc);
1071 static int ccmpdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc);
1072
1073 static Block*
1074 wifiencrypt(Wifi *wifi, Wnode *wn, Block *b)
1075 {
1076         uvlong tsc;
1077         int n, kid;
1078         Wifipkt *w;
1079         Wkey *k;
1080
1081         rlock(&wifi->crypt);
1082
1083         kid = 0;
1084         k = wn->txkey[kid];
1085         if(k == nil){
1086                 runlock(&wifi->crypt);
1087                 return b;
1088         }
1089
1090         n = wifihdrlen((Wifipkt*)b->rp);
1091
1092         b = padblock(b, 8);
1093         b = padblock(b, -(8+4));
1094
1095         w = (Wifipkt*)b->rp;
1096         memmove(w, b->rp+8, n);
1097         b->rp += n;
1098
1099         tsc = ++k->tsc;
1100
1101         switch(k->cipher){
1102         case TKIP:
1103                 b->rp[0] = tsc>>8;
1104                 b->rp[1] = (b->rp[0] | 0x20) & 0x7f;
1105                 b->rp[2] = tsc;
1106                 b->rp[3] = kid<<6 | 0x20;
1107                 b->rp[4] = tsc>>16;
1108                 b->rp[5] = tsc>>24;
1109                 b->rp[6] = tsc>>32;
1110                 b->rp[7] = tsc>>40;
1111                 b->rp += 8;
1112                 tkipencrypt(k, w, b, tsc);
1113                 break;
1114         case CCMP:
1115                 b->rp[0] = tsc;
1116                 b->rp[1] = tsc>>8;
1117                 b->rp[2] = 0;
1118                 b->rp[3] = kid<<6 | 0x20;
1119                 b->rp[4] = tsc>>16;
1120                 b->rp[5] = tsc>>24;
1121                 b->rp[6] = tsc>>32;
1122                 b->rp[7] = tsc>>40;
1123                 b->rp += 8;
1124                 ccmpencrypt(k, w, b, tsc);
1125                 break;
1126         }
1127         runlock(&wifi->crypt);
1128
1129         b->rp = (uchar*)w;
1130         w->fc[1] |= 0x40;
1131         return b;
1132 }
1133
1134 static Block*
1135 wifidecrypt(Wifi *wifi, Wnode *wn, Block *b)
1136 {
1137         uvlong tsc;
1138         int n, kid;
1139         Wifipkt *w;
1140         Wkey *k;
1141
1142         rlock(&wifi->crypt);
1143
1144         w = (Wifipkt*)b->rp;
1145         n = wifihdrlen(w);
1146         b->rp += n;
1147         if(BLEN(b) < 8+8)
1148                 goto drop;
1149
1150         kid = b->rp[3]>>6;
1151         if((b->rp[3] & 0x20) == 0)
1152                 goto drop;
1153         if((w->a1[0] & 1) == 0)
1154                 kid = 4;        /* use peerwise key for non-unicast */
1155
1156         k = wn->rxkey[kid];
1157         if(k == nil)
1158                 goto drop;
1159         switch(k->cipher){
1160         case TKIP:
1161                 tsc =   (uvlong)b->rp[7]<<40 |
1162                         (uvlong)b->rp[6]<<32 |
1163                         (uvlong)b->rp[5]<<24 |
1164                         (uvlong)b->rp[4]<<16 |
1165                         (uvlong)b->rp[0]<<8 |
1166                         (uvlong)b->rp[2];
1167                 b->rp += 8;
1168                 if(tsc <= k->tsc)
1169                         goto drop;
1170                 if(tkipdecrypt(k, w, b, tsc) != 0)
1171                         goto drop;
1172                 break;
1173         case CCMP:
1174                 tsc =   (uvlong)b->rp[7]<<40 |
1175                         (uvlong)b->rp[6]<<32 |
1176                         (uvlong)b->rp[5]<<24 |
1177                         (uvlong)b->rp[4]<<16 |
1178                         (uvlong)b->rp[1]<<8 |
1179                         (uvlong)b->rp[0];
1180                 b->rp += 8;
1181                 if(tsc <= k->tsc)
1182                         goto drop;
1183                 if(ccmpdecrypt(k, w, b, tsc) != 0)
1184                         goto drop;
1185                 break;
1186         default:
1187         drop:
1188                 runlock(&wifi->crypt);
1189                 freeb(b);
1190                 return nil;
1191         }
1192         runlock(&wifi->crypt);
1193
1194         k->tsc = tsc;
1195         b->rp -= n;
1196         memmove(b->rp, w, n);
1197         w = (Wifipkt*)b->rp;
1198         w->fc[1] &= ~0x40;
1199         return b;
1200 }
1201
1202 static u16int Sbox[256] = {
1203         0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
1204         0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
1205         0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
1206         0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
1207         0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
1208         0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
1209         0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
1210         0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
1211         0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
1212         0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
1213         0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
1214         0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
1215         0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
1216         0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
1217         0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
1218         0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
1219         0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
1220         0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
1221         0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
1222         0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
1223         0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
1224         0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
1225         0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
1226         0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
1227         0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
1228         0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
1229         0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
1230         0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
1231         0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
1232         0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
1233         0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
1234         0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A
1235 };
1236
1237 static void
1238 tkipk2tk(uchar key[16], u16int tk[8])
1239 {
1240         tk[0] = (u16int)key[1]<<8 | key[0];
1241         tk[1] = (u16int)key[3]<<8 | key[2];
1242         tk[2] = (u16int)key[5]<<8 | key[4];
1243         tk[3] = (u16int)key[7]<<8 | key[6];
1244         tk[4] = (u16int)key[9]<<8 | key[8];
1245         tk[5] = (u16int)key[11]<<8 | key[10];
1246         tk[6] = (u16int)key[13]<<8 | key[12];
1247         tk[7] = (u16int)key[15]<<8 | key[14];
1248 }
1249
1250 static void
1251 tkipphase1(u32int tscu, uchar ta[Eaddrlen], u16int tk[8], u16int p1k[5])
1252 {
1253         u16int *k, i, x0, x1, x2;
1254
1255         p1k[0] = tscu;
1256         p1k[1] = tscu>>16;
1257         p1k[2] = (u16int)ta[1]<<8 | ta[0];
1258         p1k[3] = (u16int)ta[3]<<8 | ta[2];
1259         p1k[4] = (u16int)ta[5]<<8 | ta[4];
1260
1261         for(i=0; i<8; i++){
1262                 k = &tk[i & 1];
1263
1264                 x0 = p1k[4] ^ k[0];
1265                 x1 = Sbox[x0 >> 8];
1266                 x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1267                 p1k[0] += x2;
1268                 x0 = p1k[0] ^ k[2];
1269                 x1 = Sbox[x0 >> 8];
1270                 x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1271                 p1k[1] += x2;
1272                 x0 = p1k[1] ^ k[4];
1273                 x1 = Sbox[x0 >> 8];
1274                 x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1275                 p1k[2] += x2;
1276                 x0 = p1k[2] ^ k[6];
1277                 x1 = Sbox[x0 >> 8];
1278                 x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1279                 p1k[3] += x2;
1280                 x0 = p1k[3] ^ k[0];
1281                 x1 = Sbox[x0 >> 8];
1282                 x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1283                 p1k[4] += x2;
1284
1285                 p1k[4] += i;
1286         }
1287 }
1288
1289 static void
1290 tkipphase2(u16int tscl, u16int p1k[5], u16int tk[8], uchar rc4key[16])
1291 {
1292         u16int ppk[6], x0, x1, x2;
1293
1294         ppk[0] = p1k[0];
1295         ppk[1] = p1k[1];
1296         ppk[2] = p1k[2];
1297         ppk[3] = p1k[3];
1298         ppk[4] = p1k[4];
1299         ppk[5] = p1k[4] + tscl;
1300
1301         x0 = ppk[5] ^ tk[0];
1302         x1 = Sbox[x0 >> 8];
1303         x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1304         ppk[0] += x2;
1305         x0 = ppk[0] ^ tk[1];
1306         x1 = Sbox[x0 >> 8];
1307         x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1308         ppk[1] += x2;
1309         x0 = ppk[1] ^ tk[2];
1310         x1 = Sbox[x0 >> 8];
1311         x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1312         ppk[2] += x2;
1313         x0 = ppk[2] ^ tk[3];
1314         x1 = Sbox[x0 >> 8];
1315         x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1316         ppk[3] += x2;
1317         x0 = ppk[3] ^ tk[4];
1318         x1 = Sbox[x0 >> 8];
1319         x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1320         ppk[4] += x2;
1321         x0 = ppk[4] ^ tk[5];
1322         x1 = Sbox[x0 >> 8];
1323         x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8));
1324         ppk[5] += x2;
1325
1326         x2 = ppk[5] ^ tk[6];
1327         ppk[0] += (x2 >> 1) | (x2 << 15);
1328         x2 = ppk[0] ^ tk[7];
1329         ppk[1] += (x2 >> 1) | (x2 << 15);
1330
1331         x2 = ppk[1];
1332         ppk[2] += (x2 >> 1) | (x2 << 15);
1333         x2 = ppk[2];
1334         ppk[3] += (x2 >> 1) | (x2 << 15);
1335         x2 = ppk[3];
1336         ppk[4] += (x2 >> 1) | (x2 << 15);
1337         x2 = ppk[4];
1338         ppk[5] += (x2 >> 1) | (x2 << 15);
1339
1340         rc4key[0] = tscl >> 8;
1341         rc4key[1] = (rc4key[0] | 0x20) & 0x7F;
1342         rc4key[2] = tscl;
1343         rc4key[3] = (ppk[5] ^ tk[0]) >> 1;
1344         rc4key[4]  = ppk[0];
1345         rc4key[5]  = ppk[0] >> 8;
1346         rc4key[6]  = ppk[1];
1347         rc4key[7]  = ppk[1] >> 8;
1348         rc4key[8]  = ppk[2];
1349         rc4key[9]  = ppk[2] >> 8;
1350         rc4key[10] = ppk[3];
1351         rc4key[11] = ppk[3] >> 8;
1352         rc4key[12] = ppk[4];
1353         rc4key[13] = ppk[4] >> 8;
1354         rc4key[14] = ppk[5];
1355         rc4key[15] = ppk[5] >> 8;
1356 }
1357
1358 typedef struct MICstate MICstate;
1359 struct MICstate
1360 {
1361         u32int  l;
1362         u32int  r;
1363         u32int  m;
1364         u32int  n;
1365 };
1366
1367 static void
1368 micsetup(MICstate *s, uchar key[8])
1369 {
1370         s->l =  (u32int)key[0] |
1371                 (u32int)key[1]<<8 |
1372                 (u32int)key[2]<<16 |
1373                 (u32int)key[3]<<24;
1374         s->r =  (u32int)key[4] |
1375                 (u32int)key[5]<<8 |
1376                 (u32int)key[6]<<16 |
1377                 (u32int)key[7]<<24;
1378         s->m = 0;
1379         s->n = 0;
1380 }
1381
1382 static void
1383 micupdate(MICstate *s, uchar *data, ulong len)
1384 {
1385         u32int l, r, m, n, e;
1386
1387         l = s->l;
1388         r = s->r;
1389         m = s->m;
1390         n = s->n;
1391         e = n + len;
1392         while(n != e){
1393                 m >>= 8;
1394                 m |= (u32int)*data++ << 24;
1395                 if(++n & 3)
1396                         continue;
1397                 l ^= m;
1398                 r ^= (l << 17) | (l >> 15);
1399                 l += r;
1400                 r ^= ((l & 0x00FF00FFUL)<<8) | ((l & 0xFF00FF00UL)>>8);
1401                 l += r;
1402                 r ^= (l << 3) | (l >> 29);
1403                 l += r;
1404                 r ^= (l >> 2) | (l << 30);
1405                 l += r;
1406         }
1407         s->l = l;
1408         s->r = r;
1409         s->m = m;
1410         s->n = n;
1411 }
1412
1413 static void
1414 micfinish(MICstate *s, uchar mic[8])
1415 {
1416         static uchar pad[8] = { 0x5a, 0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00, };
1417
1418         micupdate(s, pad, sizeof(pad));
1419
1420         mic[0] = s->l;
1421         mic[1] = s->l>>8;
1422         mic[2] = s->l>>16;
1423         mic[3] = s->l>>24;
1424         mic[4] = s->r;
1425         mic[5] = s->r>>8;
1426         mic[6] = s->r>>16;
1427         mic[7] = s->r>>24;
1428 }
1429
1430 static uchar pad4[4] = { 0x00, 0x00, 0x00, 0x00, };
1431
1432 static void
1433 tkipencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc)
1434 {
1435         u16int tk[8], p1k[5];
1436         uchar seed[16];
1437         RC4state rs;
1438         MICstate ms;
1439         ulong crc;
1440
1441         micsetup(&ms, k->key+24);
1442         micupdate(&ms, dstaddr(w), Eaddrlen);
1443         micupdate(&ms, srcaddr(w), Eaddrlen);
1444         micupdate(&ms, pad4, 4);
1445         micupdate(&ms, b->rp, BLEN(b));
1446         micfinish(&ms, b->wp);
1447         b->wp += 8;
1448
1449         crc = ethercrc(b->rp, BLEN(b));
1450         crc = ~crc;
1451         b->wp[0] = crc;
1452         b->wp[1] = crc>>8;
1453         b->wp[2] = crc>>16;
1454         b->wp[3] = crc>>24;
1455         b->wp += 4;
1456
1457         tkipk2tk(k->key, tk);
1458         tkipphase1(tsc >> 16, w->a2, tk, p1k);
1459         tkipphase2(tsc & 0xFFFF, p1k, tk, seed);
1460         setupRC4state(&rs, seed, sizeof(seed));
1461         rc4(&rs, b->rp, BLEN(b));
1462 }
1463
1464 static int
1465 tkipdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc)
1466 {
1467         uchar seed[16], mic[8];
1468         u16int tk[8], p1k[5];
1469         RC4state rs;
1470         MICstate ms;
1471         ulong crc;
1472
1473         if(BLEN(b) < 8+4)
1474                 return -1;
1475
1476         tkipk2tk(k->key, tk);
1477         tkipphase1(tsc >> 16, w->a2, tk, p1k);
1478         tkipphase2(tsc & 0xFFFF, p1k, tk, seed);
1479         setupRC4state(&rs, seed, sizeof(seed));
1480         rc4(&rs, b->rp, BLEN(b));
1481
1482         b->wp -= 4;
1483         crc =   (ulong)b->wp[0] |
1484                 (ulong)b->wp[1]<<8 |
1485                 (ulong)b->wp[2]<<16 |
1486                 (ulong)b->wp[3]<<24;
1487         crc = ~crc;
1488         crc ^= ethercrc(b->rp, BLEN(b));
1489
1490         b->wp -= 8;
1491         micsetup(&ms, k->key+16);
1492         micupdate(&ms, dstaddr(w), Eaddrlen);
1493         micupdate(&ms, srcaddr(w), Eaddrlen);
1494         micupdate(&ms, pad4, 4);
1495         micupdate(&ms, b->rp, BLEN(b));
1496         micfinish(&ms, mic);
1497
1498         return tsmemcmp(b->wp, mic, 8) | crc;
1499 }
1500
1501 static uchar*
1502 putbe(uchar *p, int L, uint v)
1503 {
1504         while(--L >= 0)
1505                 *p++ = (v >> L*8) & 0xFF;
1506         return p;
1507 }
1508
1509 static void
1510 xblock(int L, int M, uchar *N, uchar *a, int la, int lm, uchar t[16], AESstate *s)
1511 {
1512         uchar l[8], *p, *x, *e;
1513
1514         assert(M >= 4 && M <= 16);
1515         assert(L >= 2 && L <= 4);
1516
1517         t[0] = ((la > 0)<<6) | ((M-2)/2)<<3 | (L-1);    /* flags */
1518         memmove(&t[1], N, 15-L);
1519         putbe(&t[16-L], L, lm);
1520         aes_encrypt(s->ekey, s->rounds, t, t);
1521
1522         if(la > 0){
1523                 assert(la < 0xFF00);
1524                 for(p = l, e = putbe(l, 2, la), x = t; p < e; x++, p++)
1525                         *x ^= *p;
1526                 for(e = a + la; a < e; x = t){
1527                         for(; a < e && x < &t[16]; x++, a++)
1528                                 *x ^= *a;
1529                         aes_encrypt(s->ekey, s->rounds, t, t);
1530                 }
1531         }
1532 }
1533
1534 static uchar*
1535 sblock(int L, uchar *N, uint i, uchar b[16], AESstate *s)
1536 {
1537         b[0] = L-1;     /* flags */
1538         memmove(&b[1], N, 15-L);
1539         putbe(&b[16-L], L, i);
1540         aes_encrypt(s->ekey, s->rounds, b, b);
1541         return b;
1542 };
1543
1544 static void
1545 aesCCMencrypt(int L, int M, uchar *N /* N[15-L] */,
1546         uchar *a /* a[la] */, int la,
1547         uchar *m /* m[lm+M] */, int lm,
1548         AESstate *s)
1549 {
1550         uchar t[16], b[16], *p, *x;
1551         uint i;
1552
1553         xblock(L, M, N, a, la, lm, t, s);
1554
1555         for(i = 1; lm >= 16; i++, m += 16, lm -= 16){
1556                 sblock(L, N, i, b, s);
1557
1558                 *((u32int*)&t[0]) ^= *((u32int*)&m[0]);
1559                 *((u32int*)&m[0]) ^= *((u32int*)&b[0]);
1560                 *((u32int*)&t[4]) ^= *((u32int*)&m[4]);
1561                 *((u32int*)&m[4]) ^= *((u32int*)&b[4]);
1562                 *((u32int*)&t[8]) ^= *((u32int*)&m[8]);
1563                 *((u32int*)&m[8]) ^= *((u32int*)&b[8]);
1564                 *((u32int*)&t[12]) ^= *((u32int*)&m[12]);
1565                 *((u32int*)&m[12]) ^= *((u32int*)&b[12]);
1566
1567                 aes_encrypt(s->ekey, s->rounds, t, t);
1568         }
1569         if(lm > 0){
1570                 for(p = sblock(L, N, i, b, s), x = t; p < &b[lm]; x++, m++, p++){
1571                         *x ^= *m;
1572                         *m ^= *p;
1573                 }
1574                 aes_encrypt(s->ekey, s->rounds, t, t);
1575         }
1576
1577         for(p = sblock(L, N, 0, b, s), x = t; p < &b[M]; x++, p++)
1578                 *x ^= *p;
1579
1580         memmove(m, t, M);
1581 }
1582
1583 static int
1584 aesCCMdecrypt(int L, int M, uchar *N /* N[15-L] */,
1585         uchar *a /* a[la] */, int la,
1586         uchar *m /* m[lm+M] */, int lm,
1587         AESstate *s)
1588 {
1589         uchar t[16], b[16], *p, *x;
1590         uint i;
1591
1592         xblock(L, M, N, a, la, lm, t, s);
1593
1594         for(i = 1; lm >= 16; i++, m += 16, lm -= 16){
1595                 sblock(L, N, i, b, s);
1596
1597                 *((u32int*)&m[0]) ^= *((u32int*)&b[0]);
1598                 *((u32int*)&t[0]) ^= *((u32int*)&m[0]);
1599                 *((u32int*)&m[4]) ^= *((u32int*)&b[4]);
1600                 *((u32int*)&t[4]) ^= *((u32int*)&m[4]);
1601                 *((u32int*)&m[8]) ^= *((u32int*)&b[8]);
1602                 *((u32int*)&t[8]) ^= *((u32int*)&m[8]);
1603                 *((u32int*)&m[12]) ^= *((u32int*)&b[12]);
1604                 *((u32int*)&t[12]) ^= *((u32int*)&m[12]);
1605
1606                 aes_encrypt(s->ekey, s->rounds, t, t);
1607         }
1608         if(lm > 0){
1609                 for(p = sblock(L, N, i, b, s), x = t; p < &b[lm]; x++, m++, p++){
1610                         *m ^= *p;
1611                         *x ^= *m;
1612                 }
1613                 aes_encrypt(s->ekey, s->rounds, t, t);
1614         }
1615
1616         for(p = sblock(L, N, 0, b, s), x = t; p < &b[M]; x++, p++)
1617                 *x ^= *p;
1618
1619         return tsmemcmp(m, t, M);
1620 }
1621
1622 static int
1623 setupCCMP(Wifipkt *w, uvlong tsc, uchar nonce[13], uchar auth[32])
1624 {
1625         uchar *p;
1626
1627         nonce[0] = ((w->fc[0] & 0x0c) == 0x00) << 4;
1628         memmove(&nonce[1], w->a2, Eaddrlen);
1629         nonce[7]  = tsc >> 40;
1630         nonce[8]  = tsc >> 32;
1631         nonce[9]  = tsc >> 24;
1632         nonce[10] = tsc >> 16;
1633         nonce[11] = tsc >> 8;
1634         nonce[12] = tsc;
1635
1636         p = auth;
1637         *p++ = (w->fc[0] & (((w->fc[0] & 0x0c) == 0x08) ? 0x0f : 0xff));
1638         *p++ = (w->fc[1] & ~0x38) | 0x40;
1639         memmove(p, w->a1, Eaddrlen); p += Eaddrlen;
1640         memmove(p, w->a2, Eaddrlen); p += Eaddrlen;
1641         memmove(p, w->a3, Eaddrlen); p += Eaddrlen;
1642         *p++ = w->seq[0] & 0x0f;
1643         *p++ = 0;
1644         if((w->fc[1] & 3) == 0x03) {
1645                 memmove(p, w->a4, Eaddrlen);
1646                 p += Eaddrlen;
1647         }
1648
1649         return p - auth;
1650 }
1651
1652 static void
1653 ccmpencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc)
1654 {
1655         uchar auth[32], nonce[13];
1656
1657         aesCCMencrypt(2, 8, nonce, auth,
1658                 setupCCMP(w, tsc, nonce, auth),
1659                 b->rp, BLEN(b), (AESstate*)k->key);
1660         b->wp += 8;
1661 }
1662
1663 static int
1664 ccmpdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc)
1665 {
1666         uchar auth[32], nonce[13];
1667
1668         if(BLEN(b) < 8)
1669                 return -1;
1670
1671         b->wp -= 8;
1672         return aesCCMdecrypt(2, 8, nonce, auth,
1673                 setupCCMP(w, tsc, nonce, auth),
1674                 b->rp, BLEN(b), (AESstate*)k->key);
1675 }