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