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