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