diff 528fa0acfa3532dee4a50fca1ae49d147403edfc uncommitted --- a/sys/src/9/imx8/reform +++ b/sys/src/9/imx8/reform @@ -56,6 +56,7 @@ iomux sdnvme pci sdmmc usdhc + wifi wifipack wificrypt port int cpuserver = 0; bootdir --- a/sys/src/9/pc/etherrt2860.c +++ b/sys/src/9/pc/etherrt2860.c @@ -1251,23 +1251,7 @@ static void selchangroup(Ctlr*, int); static void setleds(Ctlr*, u16int); -static uint -get16(uchar *p){ - return *((u16int*)p); -} -static uint -get32(uchar *p){ - return *((u32int*)p); -} static void -put32(uchar *p, uint v){ - *((u32int*)p) = v; -} -static void -put16(uchar *p, uint v){ - *((u16int*)p) = v; -}; -static void memwrite(Ctlr *ctlr, u32int off, uchar *data, uint size){ memmove((uchar*)ctlr->nic + off, data, size); } @@ -2562,7 +2546,7 @@ { Ether *edev; Ctlr *ctlr; - Wifipkt *w; + Wframe w; u8int mcs, qid; int ridx, /*ctl_ridx,*/ hdrlen; uchar *p; @@ -2596,17 +2580,15 @@ tx = &ctlr->tx[qid]; nodeid = ctlr->bcastnodeid; - w = (Wifipkt*)b->rp; - hdrlen = wifihdrlen(w); - + hdrlen = wframeunpack(&w, b->rp); p = pool->p + pool->i * TxwiDmaSz; - if((w->a1[0] & 1) == 0){ + if((w.a1[0] & 1) == 0){ *(p+4) = TxAck; /* xflags */ if(BLEN(b) > 512-4) *(p+1) = TxTxopBackoff; /* txop */ - if((w->fc[0] & 0x0c) == 0x08 && ctlr->bssnodeid != -1){ + if((w.fc[0] & Wfc0typemask) == Wfc0typedat && ctlr->bssnodeid != -1){ nodeid = ctlr->bssnodeid; ridx = 2; /* BUG: hardcode 11Mbit */ } @@ -2652,11 +2634,8 @@ /* setup payload segments */ put32(p + 8, PCIWADDR(outb->rp)); /* sdp1 */ put16(p + 4, BLEN(outb) | TxLs1); /* sdl1 */ - - p = pool->p + pool->i * TxwiDmaSz; - w = (Wifipkt*)(p + Txwisize); if(ctlr->wifi->debug){ - print("transmit: %E->%E,%E nodeid=%x txq[%d]=%d size=%zd\n", w->a2, w->a1, w->a3, nodeid, qid, ctlr->tx[qid].i, BLEN(outb)); + print("transmit: %E->%E,%E nodeid=%x txq[%d]=%d size=%zd\n", w.a2, w.a1, w.a3, nodeid, qid, ctlr->tx[qid].i, BLEN(outb)); } tx->i = (tx->i + 1) % Ntx; @@ -2730,7 +2709,7 @@ u16int sdl0, len; u32int flags; uchar *p; - Wifipkt *w; + Wframe w; int hdrlen; p = (uchar*)rx->p + Rdscsize * rx->i; @@ -2761,16 +2740,15 @@ len = get16(b->rp + 2 /* wcid, keyidx */) & 0xfff; b->rp = d + Rxwisize; b->wp = b->rp + len; - w = (Wifipkt*)b->rp; - hdrlen = wifihdrlen(w); + hdrlen = wframeunpack(&w, b->rp); /* HW may insert 2 padding bytes after 802.11 header */ if(flags & RxL2pad){ memmove(b->rp + 2, b->rp, hdrlen); b->rp += 2; } - w = (Wifipkt*)b->rp; + wframeunpack(&w, b->rp); if(ctlr->wifi->debug) - print("receive: %E->%E,%E wcid 0x%x \n", w->a2, w->a1, w->a3, ctlr->wcid); + print("receive: %E->%E,%E wcid 0x%x \n", w.a2, w.a1, w.a3, ctlr->wcid); wifiiq(ctlr->wifi, b); skip: put16(p + 4 /* sdp0 */ + 2 /* sdl1 */, sdl0 & ~RxDdone); --- a/sys/src/9/pc/etherwpi.c +++ b/sys/src/9/pc/etherwpi.c @@ -290,24 +290,6 @@ #define csr32r(c, r) (*((c)->nic+((r)/4))) #define csr32w(c, r, v) (*((c)->nic+((r)/4)) = (v)) -static uint -get32(uchar *p){ - return *((u32int*)p); -} -static uint -get16(uchar *p) -{ - return *((u16int*)p); -} -static void -put32(uchar *p, uint v){ - *((u32int*)p) = v; -} -static void -put16(uchar *p, uint v){ - *((u16int*)p) = v; -}; - static char* niclock(Ctlr *ctlr) { @@ -1402,7 +1384,7 @@ uchar c[Tcmdsize], *p; Ether *edev; Ctlr *ctlr; - Wifipkt *w; + Wframe w; int flags, nodeid, rate, timeout; char *err; @@ -1430,14 +1412,15 @@ timeout = 3; nodeid = ctlr->bcastnodeid; p = wn->minrate; - w = (Wifipkt*)b->rp; - if((w->a1[0] & 1) == 0){ + + wframeunpack(&w, b->rp); + if((w.a1[0] & 1) == 0){ flags |= TFlagNeedACK; if(BLEN(b) > 512-4) flags |= TFlagNeedRTS|TFlagFullTxOp; - if((w->fc[0] & 0x0c) == 0x08 && ctlr->bssnodeid != -1){ + if((w.fc[0] & Wfc0typemask) == Wfc0typedat && ctlr->bssnodeid != -1){ timeout = 0; nodeid = ctlr->bssnodeid; p = wn->actrate; --- a/sys/src/9/pc/pc +++ b/sys/src/9/pc/pc @@ -151,6 +151,8 @@ dtracytimer dtracydev + wifi wifipack wificrypt + ip tcp udp --- a/sys/src/9/pc64/pc64 +++ b/sys/src/9/pc64/pc64 @@ -148,6 +148,8 @@ dtracytimer dtracydev + wifi wifipack wificrypt + ip tcp udp --- a/sys/src/9/port/etheriwl.c +++ b/sys/src/9/port/etheriwl.c @@ -722,28 +722,6 @@ #define csr32r(c, r) (*((c)->nic+((r)/4))) #define csr32w(c, r, v) (*((c)->nic+((r)/4)) = (v)) -static uint -get16(uchar *p){ - return *((u16int*)p); -} -static uint -get32(uchar *p){ - return *((u32int*)p); -} -static void -put32(uchar *p, uint v){ - *((u32int*)p) = v; -} -static void -put64(uchar *p, uvlong v) -{ - *((u64int*)p) = v; -} -static void -put16(uchar *p, uint v){ - *((u16int*)p) = v; -}; - static char* niclock(Ctlr *ctlr) { @@ -1692,12 +1670,13 @@ coherence(); ctlr->rx.b[i] = b; - if(ctlr->mqrx) + if(ctlr->mqrx) { put64(ctlr->rx.p + (i<<3), PCIWADDR(b->rp)); - else + } else { put32(ctlr->rx.p + (i<<2), PCIWADDR(b->rp) >> 8); - dmaflush(1, ctlr->rx.p + i*ctlr->rx.psz, ctlr->rx.psz); + } + dmaflush(1, ctlr->rx.p + i*ctlr->rx.psz, ctlr->rx.psz); return 0; } @@ -3839,8 +3818,8 @@ Ether *edev; Station *sta; Ctlr *ctlr; - Wifipkt *w; char *err; + Wframe w; edev = wifi->ether; ctlr = edev->ctlr; @@ -3876,14 +3855,15 @@ flags = 0; sta = &ctlr->bcast; p = wn->minrate; - w = (Wifipkt*)b->rp; - if((w->a1[0] & 1) == 0){ + + wframeunpack(&w, b->rp); + if((w.a1[0] & 1) == 0){ flags |= TFlagNeedACK; if(BLEN(b) > 512-4) flags |= TFlagNeedRTS; - if((w->fc[0] & 0x0c) == 0x08 && ctlr->bss.id != -1){ + if((w.fc[0] & 0x0c) == 0x08 && ctlr->bss.id != -1){ sta = &ctlr->bss; p = wn->actrate; } --- a/sys/src/9/port/portdat.h +++ b/sys/src/9/port/portdat.h @@ -135,12 +135,13 @@ /* flag values */ enum { - BINTR = (1<<0), - BFREE = (1<<1), - Bipck = (1<<2), /* ip checksum */ - Budpck = (1<<3), /* udp checksum */ - Btcpck = (1<<4), /* tcp checksum */ - Bpktck = (1<<5), /* packet checksum */ + BINTR = (1<<0), + BFREE = (1<<1), + Bipck = (1<<2), /* ip checksum */ + Budpck = (1<<3), /* udp checksum */ + Btcpck = (1<<4), /* tcp checksum */ + Bpktck = (1<<5), /* packet checksum */ + Btimestamp = (1<<6), /* timestamp prefix */ }; struct Block @@ -152,6 +153,7 @@ uchar* lim; /* 1 past the end of the buffer */ uchar* base; /* start of the buffer */ void (*free)(Block*); + ushort flag; ushort checksum; /* IP checksum of complete packet (minus media header) */ }; --- a/sys/src/9/port/wifi.c +++ b/sys/src/9/port/wifi.c @@ -45,64 +45,14 @@ 0 }; -static Block* wifidecrypt(Wifi *, Wnode *, Block *); -static Block* wifiencrypt(Wifi *, Wnode *, Block *); static void freewifikeys(Wifi *, Wnode *); - static void dmatproxy(Block *bp, int upstream, uchar proxy[Eaddrlen], DMAT *t); -static uchar* -srcaddr(Wifipkt *w) -{ - if((w->fc[1] & 0x02) == 0) - return w->a2; - if((w->fc[1] & 0x01) == 0) - return w->a3; - return w->a4; -} -static uchar* -dstaddr(Wifipkt *w) -{ - if((w->fc[1] & 0x01) != 0) - return w->a3; - return w->a1; -} - -int -wifihdrlen(Wifipkt *w) -{ - int n; - - n = WIFIHDRSIZE; - if((w->fc[0] & 0x0c) == 0x08) - if((w->fc[0] & 0xf0) == 0x80){ /* QOS */ - n += 2; - if(w->fc[1] & 0x80) - n += 4; - } - if((w->fc[1] & 3) == 0x03) - n += Eaddrlen; - return n; -} - -static uvlong -getts(uchar *d) -{ - return (uvlong)d[0] | - (uvlong)d[1]<<8 | - (uvlong)d[2]<<16 | - (uvlong)d[3]<<24 | - (uvlong)d[4]<<32 | - (uvlong)d[5]<<40 | - (uvlong)d[6]<<48 | - (uvlong)d[7]<<56; -} - void wifiiq(Wifi *wifi, Block *b) { SNAP s; - Wifipkt h, *w; + Wframe f; Etherpkt *e; int hdrlen; @@ -110,49 +60,54 @@ assert(b->rp - b->base >= 8); if(BLEN(b) < WIFIHDRSIZE) goto drop; - w = (Wifipkt*)b->rp; - hdrlen = wifihdrlen(w); + + hdrlen = wframeunpack(&f, b->rp); if(BLEN(b) < hdrlen) goto drop; - if(memcmp(srcaddr(w), wifi->ether->ea, Eaddrlen) == 0) + if(memcmp(wframesrc(&f), wifi->ether->ea, Eaddrlen) == 0) goto drop; - if(w->fc[1] & 0x40){ + + if(f.fc[1] & 0x40){ /* encrypted */ qpass(wifi->iq, b); return; } - switch(w->fc[0] & 0x0c){ - case 0x00: /* management */ - if((w->fc[1] & 3) != 0x00) /* STA->STA */ + switch(f.fc[0] & Wfc0typemask){ + case Wfc0typemgt: + if((f.fc[1] & Wfc1dirmask) != Wfc1dirstatosta) break; qpass(wifi->iq, b); return; - case 0x04: /* control */ + case Wfc0typectl: break; - case 0x08: /* data */ + case Wfc0typedat: b->flag &= ~Btimestamp; b->rp += hdrlen; - switch(w->fc[0] & 0xf0){ + switch(f.fc[0] & Wfc0submask){ default: goto drop; - case 0x80: /* QOS */ - case 0x00: + + case Wfc0subqos: + case Wfc0subdata: break; } + if(BLEN(b) < SNAPHDRSIZE) break; + memmove(&s, b->rp, SNAPHDRSIZE); if(s.dsap != 0xAA || s.ssap != 0xAA || s.control != 3) break; if(s.orgcode[0] != 0 || s.orgcode[1] != 0 || s.orgcode[2] != 0) break; + b->rp += SNAPHDRSIZE-ETHERHDRSIZE; - h = *w; e = (Etherpkt*)b->rp; - memmove(e->d, dstaddr(&h), Eaddrlen); - memmove(e->s, srcaddr(&h), Eaddrlen); + memmove(e->d, wframedst(&f), Eaddrlen); + memmove(e->s, wframesrc(&f), Eaddrlen); memmove(e->type, s.type, 2); dmatproxy(b, 0, wifi->ether->ea, &wifi->dmat); + etheriq(wifi->ether, b); return; } @@ -161,28 +116,24 @@ } static void -wifitx(Wifi *wifi, Wnode *wn, Block *b) +wifitx(Wifi *wifi, Wnode *wn, Wframe *f, Block *b) { - Wifipkt *w; - uint seq; - wn->lastsend = MACHP(0)->ticks; - seq = incref(&wifi->txseq); - seq <<= 4; - - w = (Wifipkt*)b->rp; - w->dur[0] = 0; - w->dur[1] = 0; - w->seq[0] = seq; - w->seq[1] = seq>>8; - - if((w->fc[0] & 0x0c) != 0x00){ - b = wifiencrypt(wifi, wn, b); + if((f->fc[0] & Wfc0typemask) != Wfc0typemgt){ + b = wifiencrypt(wifi, wn, f, b); if(b == nil) return; } + b = padblock(b, wframelen(f)); + if(!b) + return; + + f->dur = 0; + f->seq = incref(&wifi->txseq) << 4; + wframepack(f, b->rp); + if((wn->txcount++ & 255) == 255 && wn->actrate != nil && wn->actrate != wn->maxrate){ uchar *a, *p; @@ -229,13 +180,14 @@ void wifitxfail(Wifi *wifi, Block *b) { - Wifipkt *w; + Wframe f; Wnode *wn; if(b == nil) return; - w = (Wifipkt*)b->rp; - wn = nodelookup(wifi, w->a1, 0); + + wframeunpack(&f, b->rp); + wn = nodelookup(wifi, f.a1, 0); if(wn == nil) return; wn->txerror++; @@ -310,7 +262,7 @@ static void wifiprobe(Wifi *wifi, Wnode *wn) { - Wifipkt *w; + Wframe f; Block *b; uchar *p; int n; @@ -322,15 +274,13 @@ return; } - b = allocb(WIFIHDRSIZE + 512); - w = (Wifipkt*)b->wp; - - w->fc[0] = 0x40; /* probe request */ - w->fc[1] = 0x00; /* STA->STA */ - memmove(w->a1, wifi->ether->bcast, Eaddrlen); /* ??? */ - memmove(w->a2, wifi->ether->ea, Eaddrlen); - memmove(w->a3, wifi->ether->bcast, Eaddrlen); - b->wp += WIFIHDRSIZE; + b = allocb(512); + f.fc[0] = Wfc0typemgt | Wfc0subprobe; + f.fc[1] = Wfc1dirstatosta; + memmove(f.a1, wifi->ether->bcast, Eaddrlen); + memmove(f.a2, wifi->ether->ea, Eaddrlen); + memmove(f.a3, wifi->ether->bcast, Eaddrlen); + p = b->wp; *p++ = 0; /* set */ @@ -345,55 +295,49 @@ *p++ = wn->channel; b->wp = p; - wifitx(wifi, wn, b); + wifitx(wifi, wn, &f, b); } static void sendauth(Wifi *wifi, Wnode *bss) { - Wifipkt *w; + Wframe f; Block *b; uchar *p; - b = allocb(WIFIHDRSIZE + 3*2); - w = (Wifipkt*)b->wp; - w->fc[0] = 0xB0; /* auth request */ - w->fc[1] = 0x00; /* STA->STA */ - memmove(w->a1, bss->bssid, Eaddrlen); /* ??? */ - memmove(w->a2, wifi->ether->ea, Eaddrlen); - memmove(w->a3, bss->bssid, Eaddrlen); - b->wp += WIFIHDRSIZE; + b = allocb(3*2); + f.fc[0] = Wfc0typemgt | Wfc0subauth; + f.fc[1] = Wfc1dirstatosta; + memmove(f.a1, bss->bssid, Eaddrlen); /* ??? */ + memmove(f.a2, wifi->ether->ea, Eaddrlen); + memmove(f.a3, bss->bssid, Eaddrlen); + p = b->wp; - *p++ = 0; /* alg */ - *p++ = 0; - *p++ = 1; /* seq */ - *p++ = 0; - *p++ = 0; /* status */ - *p++ = 0; + put16(p, 0); p += 2; /* alg */ + put16(p, 1); p += 2; /* seq */ + put16(p, 0); p += 2; /* status */ b->wp = p; bss->aid = 0; - wifitx(wifi, bss, b); + wifitx(wifi, bss, &f, b); } static void sendassoc(Wifi *wifi, Wnode *bss) { - Wifipkt *w; + Wframe f; Block *b; uchar *p; int cap, n; - b = allocb(WIFIHDRSIZE + 512); - w = (Wifipkt*)b->wp; - w->fc[0] = 0x00; /* assoc request */ - w->fc[1] = 0x00; /* STA->STA */ - memmove(w->a1, bss->bssid, Eaddrlen); /* ??? */ - memmove(w->a2, wifi->ether->ea, Eaddrlen); - memmove(w->a3, bss->bssid, Eaddrlen); + b = allocb(512); + f.fc[0] = Wfc0typemgt | Wfc0subassoc; + f.fc[1] = Wfc1dirstatosta; + memmove(f.a1, bss->bssid, Eaddrlen); /* ??? */ + memmove(f.a2, wifi->ether->ea, Eaddrlen); + memmove(f.a3, bss->bssid, Eaddrlen); - b->wp += WIFIHDRSIZE; p = b->wp; /* capinfo */ @@ -400,12 +344,10 @@ cap = 1; // ESS cap |= (1<<5); // Short Preamble cap |= (1<<10) & bss->cap; // Short Slot Time - *p++ = cap; - *p++ = cap>>8; + put16(p, cap); p += 2; /* interval */ - *p++ = 16; - *p++ = 16>>8; + put16(p, 0x1010); p += 2; n = strlen(bss->ssid); *p++ = 0; /* SSID */ @@ -422,7 +364,7 @@ } b->wp = p; - wifitx(wifi, bss, b); + wifitx(wifi, bss, &f, b); } static void @@ -450,8 +392,7 @@ return; d += 2; /* caps */ - s = d[0] | d[1]<<8; - d += 2; + s = get16(d); d += 2; switch(s){ case 0x00: wn->aid = d[0] | d[1]<<8; @@ -478,12 +419,9 @@ return; /* timestamp */ - wn->ts = getts(d); - d += 8; - wn->ival = d[0] | d[1]<<8; - d += 2; - wn->cap = d[0] | d[1]<<8; - d += 2; + wn->ts = get64(d); d += 8; + wn->ival = get16(d); d += 2; + wn->cap = get16(d); d += 2; wn->dtimcount = 0; wn->dtimperiod = 1; @@ -621,9 +559,10 @@ wifiproc(void *arg) { Wifi *wifi; - Wifipkt *w; + Wframe f; Wnode *wn; Block *b; + int hdrlen; b = nil; wifi = arg; @@ -637,16 +576,18 @@ } if((b = qbread(wifi->iq, 100000)) == nil) break; - w = (Wifipkt*)b->rp; - if(w->fc[1] & 0x40){ + + hdrlen = wframeunpack(&f, b->rp); + if(f.fc[1] & Wfc1prot){ /* encrypted */ - if((wn = nodelookup(wifi, w->a2, 0)) == nil) + if((wn = nodelookup(wifi, f.a2, 0)) == nil) continue; wn->lastseen = MACHP(0)->ticks; - if((b = wifidecrypt(wifi, wn, b)) != nil){ - w = (Wifipkt*)b->rp; - if(w->fc[1] & 0x40) + if((b = wifidecrypt(wifi, wn, &f, b)) != nil){ + wframeunpack(&f, b->rp); + if(f.fc[1] & Wfc1prot) continue; + b->flag &= ~Btimestamp; wifiiq(wifi, b); b = nil; @@ -653,22 +594,24 @@ } continue; } + /* management */ - if((w->fc[0] & 0x0c) != 0x00) + if((f.fc[0] & Wfc0typemask) != Wfc0typemgt) continue; - switch(w->fc[0] & 0xf0){ - case 0x50: /* probe response */ + b->rp += hdrlen; + switch(f.fc[0] & Wfc0submask){ + case Wfc0subprober: if(wifi->debug) - print("#l%d: got probe from %E\n", wifi->ether->ctlrno, w->a3); + print("#l%d: got probe from %E\n", wifi->ether->ctlrno, f.a3); /* no break */ - case 0x80: /* beacon */ - if((wn = nodelookup(wifi, w->a3, 1)) == nil) + case Wfc0subbeacon: + if((wn = nodelookup(wifi, f.a3, 1)) == nil) continue; wn->lastseen = MACHP(0)->ticks; if(b->flag & Btimestamp) - wn->rs = getts(b->rp - 8); - b->rp += wifihdrlen(w); + wn->rs = get64(b->rp - 8); + recvbeacon(wifi, wn, b->rp, BLEN(b)); if(wifi->bss == nil @@ -681,23 +624,22 @@ continue; } - if(memcmp(w->a1, wifi->ether->ea, Eaddrlen)) + if(memcmp(f.a1, wifi->ether->ea, Eaddrlen)) continue; - if((wn = nodelookup(wifi, w->a3, 0)) == nil) + if((wn = nodelookup(wifi, f.a3, 0)) == nil) continue; wn->lastseen = MACHP(0)->ticks; if(b->flag & Btimestamp) - wn->rs = getts(b->rp - 8); - switch(w->fc[0] & 0xf0){ - case 0x10: /* assoc response */ - case 0x30: /* reassoc response */ - b->rp += wifihdrlen(w); + wn->rs = get64(b->rp - 8); + switch(f.fc[0] & Wfc0submask){ + case Wfc0subassocr: + case Wfc0subreassocr: recvassoc(wifi, wn, b->rp, BLEN(b)); /* notify driver about node aid association */ if(wn == wifi->bss) (*wifi->transmit)(wifi, wn, nil); break; - case 0xb0: /* auth */ + case Wfc0subauth: if(wifi->debug) print("#l%d: got auth from %E\n", wifi->ether->ctlrno, wn->bssid); if(wn->brsnelen > 0 && wn->rsnelen == 0) @@ -710,7 +652,7 @@ sendassoc(wifi, wn); } break; - case 0xc0: /* deauth */ + case Wfc0subdeauth: if(wifi->debug) print("#l%d: got deauth from %E\n", wifi->ether->ctlrno, wn->bssid); wifideauth(wifi, wn); @@ -724,8 +666,7 @@ wifietheroq(Wifi *wifi, Block *b) { Etherpkt e; - Wifipkt h; - int hdrlen; + Wframe f; Wnode *wn; SNAP *s; @@ -745,21 +686,19 @@ } else if(wn->status != Sassoc) goto drop; - h.fc[0] = 0x08; /* data */ - memmove(h.a1, wn->bssid, Eaddrlen); + f.fc[0] = Wfc0typedat; + memmove(f.a1, wn->bssid, Eaddrlen); if(memcmp(e.s, wifi->ether->ea, Eaddrlen) == 0) { - h.fc[1] = 0x01; /* STA->AP */ + f.fc[1] = Wfc1dirstatoap; } else { - h.fc[1] = 0x03; /* AP->AP (WDS) */ - memmove(h.a2, wifi->ether->ea, Eaddrlen); + f.fc[1] = Wfc1diraptoap; + memmove(f.a2, wifi->ether->ea, Eaddrlen); } - memmove(dstaddr(&h), e.d, Eaddrlen); - memmove(srcaddr(&h), e.s, Eaddrlen); + memmove(wframedst(&f), e.d, Eaddrlen); + memmove(wframesrc(&f), e.s, Eaddrlen); - hdrlen = wifihdrlen(&h); - b = padblock(b, hdrlen + SNAPHDRSIZE); - memmove(b->rp, &h, hdrlen); - s = (SNAP*)(b->rp + hdrlen); + b = padblock(b, SNAPHDRSIZE); + s = (SNAP*)(b->rp); s->dsap = s->ssap = 0xAA; s->control = 0x03; s->orgcode[0] = 0; @@ -767,7 +706,7 @@ s->orgcode[2] = 0; memmove(s->type, e.type, 2); - wifitx(wifi, wn, b); + wifitx(wifi, wn, &f, b); return; drop: freeb(b); @@ -1151,615 +1090,6 @@ n = readstr(off, buf, n, s); free(s); return n; -} - -static void tkipencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc); -static int tkipdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc); -static void ccmpencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc); -static int ccmpdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc); - -static Block* -wifiencrypt(Wifi *wifi, Wnode *wn, Block *b) -{ - uvlong tsc; - int n, kid; - Wifipkt *w; - Wkey *k; - - rlock(&wifi->crypt); - - kid = 0; - k = wn->txkey[kid]; - if(k == nil){ - runlock(&wifi->crypt); - return b; - } - - n = wifihdrlen((Wifipkt*)b->rp); - - b = padblock(b, 8); - b = padblock(b, -(8+4)); - - w = (Wifipkt*)b->rp; - memmove(w, b->rp+8, n); - b->rp += n; - - tsc = ++k->tsc; - - switch(k->cipher){ - case TKIP: - b->rp[0] = tsc>>8; - b->rp[1] = (b->rp[0] | 0x20) & 0x7f; - b->rp[2] = tsc; - b->rp[3] = kid<<6 | 0x20; - b->rp[4] = tsc>>16; - b->rp[5] = tsc>>24; - b->rp[6] = tsc>>32; - b->rp[7] = tsc>>40; - b->rp += 8; - tkipencrypt(k, w, b, tsc); - break; - case CCMP: - b->rp[0] = tsc; - b->rp[1] = tsc>>8; - b->rp[2] = 0; - b->rp[3] = kid<<6 | 0x20; - b->rp[4] = tsc>>16; - b->rp[5] = tsc>>24; - b->rp[6] = tsc>>32; - b->rp[7] = tsc>>40; - b->rp += 8; - ccmpencrypt(k, w, b, tsc); - break; - } - runlock(&wifi->crypt); - - b->rp = (uchar*)w; - w->fc[1] |= 0x40; - return b; -} - -static Block* -wifidecrypt(Wifi *wifi, Wnode *wn, Block *b) -{ - uvlong tsc; - int n, kid; - Wifipkt *w; - Wkey *k; - - rlock(&wifi->crypt); - - w = (Wifipkt*)b->rp; - n = wifihdrlen(w); - b->rp += n; - if(BLEN(b) < 8+8) - goto drop; - - kid = b->rp[3]>>6; - if((b->rp[3] & 0x20) == 0) - goto drop; - if((w->a1[0] & 1) == 0) - kid = 4; /* use peerwise key for non-unicast */ - - k = wn->rxkey[kid]; - if(k == nil) - goto drop; - switch(k->cipher){ - case TKIP: - tsc = (uvlong)b->rp[7]<<40 | - (uvlong)b->rp[6]<<32 | - (uvlong)b->rp[5]<<24 | - (uvlong)b->rp[4]<<16 | - (uvlong)b->rp[0]<<8 | - (uvlong)b->rp[2]; - b->rp += 8; - if(tsc <= k->tsc) - goto drop; - if(tkipdecrypt(k, w, b, tsc) != 0) - goto drop; - break; - case CCMP: - tsc = (uvlong)b->rp[7]<<40 | - (uvlong)b->rp[6]<<32 | - (uvlong)b->rp[5]<<24 | - (uvlong)b->rp[4]<<16 | - (uvlong)b->rp[1]<<8 | - (uvlong)b->rp[0]; - b->rp += 8; - if(tsc <= k->tsc) - goto drop; - if(ccmpdecrypt(k, w, b, tsc) != 0) - goto drop; - break; - default: - drop: - runlock(&wifi->crypt); - freeb(b); - return nil; - } - runlock(&wifi->crypt); - - k->tsc = tsc; - b->rp -= n; - memmove(b->rp, w, n); - w = (Wifipkt*)b->rp; - w->fc[1] &= ~0x40; - return b; -} - -static u16int Sbox[256] = { - 0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154, - 0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A, - 0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B, - 0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B, - 0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F, - 0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F, - 0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5, - 0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F, - 0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB, - 0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397, - 0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED, - 0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A, - 0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194, - 0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3, - 0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104, - 0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D, - 0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39, - 0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695, - 0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83, - 0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76, - 0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4, - 0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B, - 0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0, - 0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018, - 0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751, - 0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85, - 0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12, - 0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9, - 0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7, - 0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A, - 0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8, - 0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A -}; - -static void -tkipk2tk(uchar key[16], u16int tk[8]) -{ - tk[0] = (u16int)key[1]<<8 | key[0]; - tk[1] = (u16int)key[3]<<8 | key[2]; - tk[2] = (u16int)key[5]<<8 | key[4]; - tk[3] = (u16int)key[7]<<8 | key[6]; - tk[4] = (u16int)key[9]<<8 | key[8]; - tk[5] = (u16int)key[11]<<8 | key[10]; - tk[6] = (u16int)key[13]<<8 | key[12]; - tk[7] = (u16int)key[15]<<8 | key[14]; -} - -static void -tkipphase1(u32int tscu, uchar ta[Eaddrlen], u16int tk[8], u16int p1k[5]) -{ - u16int *k, i, x0, x1, x2; - - p1k[0] = tscu; - p1k[1] = tscu>>16; - p1k[2] = (u16int)ta[1]<<8 | ta[0]; - p1k[3] = (u16int)ta[3]<<8 | ta[2]; - p1k[4] = (u16int)ta[5]<<8 | ta[4]; - - for(i=0; i<8; i++){ - k = &tk[i & 1]; - - x0 = p1k[4] ^ k[0]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - p1k[0] += x2; - x0 = p1k[0] ^ k[2]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - p1k[1] += x2; - x0 = p1k[1] ^ k[4]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - p1k[2] += x2; - x0 = p1k[2] ^ k[6]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - p1k[3] += x2; - x0 = p1k[3] ^ k[0]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - p1k[4] += x2; - - p1k[4] += i; - } -} - -static void -tkipphase2(u16int tscl, u16int p1k[5], u16int tk[8], uchar rc4key[16]) -{ - u16int ppk[6], x0, x1, x2; - - ppk[0] = p1k[0]; - ppk[1] = p1k[1]; - ppk[2] = p1k[2]; - ppk[3] = p1k[3]; - ppk[4] = p1k[4]; - ppk[5] = p1k[4] + tscl; - - x0 = ppk[5] ^ tk[0]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - ppk[0] += x2; - x0 = ppk[0] ^ tk[1]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - ppk[1] += x2; - x0 = ppk[1] ^ tk[2]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - ppk[2] += x2; - x0 = ppk[2] ^ tk[3]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - ppk[3] += x2; - x0 = ppk[3] ^ tk[4]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - ppk[4] += x2; - x0 = ppk[4] ^ tk[5]; - x1 = Sbox[x0 >> 8]; - x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); - ppk[5] += x2; - - x2 = ppk[5] ^ tk[6]; - ppk[0] += (x2 >> 1) | (x2 << 15); - x2 = ppk[0] ^ tk[7]; - ppk[1] += (x2 >> 1) | (x2 << 15); - - x2 = ppk[1]; - ppk[2] += (x2 >> 1) | (x2 << 15); - x2 = ppk[2]; - ppk[3] += (x2 >> 1) | (x2 << 15); - x2 = ppk[3]; - ppk[4] += (x2 >> 1) | (x2 << 15); - x2 = ppk[4]; - ppk[5] += (x2 >> 1) | (x2 << 15); - - rc4key[0] = tscl >> 8; - rc4key[1] = (rc4key[0] | 0x20) & 0x7F; - rc4key[2] = tscl; - rc4key[3] = (ppk[5] ^ tk[0]) >> 1; - rc4key[4] = ppk[0]; - rc4key[5] = ppk[0] >> 8; - rc4key[6] = ppk[1]; - rc4key[7] = ppk[1] >> 8; - rc4key[8] = ppk[2]; - rc4key[9] = ppk[2] >> 8; - rc4key[10] = ppk[3]; - rc4key[11] = ppk[3] >> 8; - rc4key[12] = ppk[4]; - rc4key[13] = ppk[4] >> 8; - rc4key[14] = ppk[5]; - rc4key[15] = ppk[5] >> 8; -} - -typedef struct MICstate MICstate; -struct MICstate -{ - u32int l; - u32int r; - u32int m; - u32int n; -}; - -static void -micsetup(MICstate *s, uchar key[8]) -{ - s->l = (u32int)key[0] | - (u32int)key[1]<<8 | - (u32int)key[2]<<16 | - (u32int)key[3]<<24; - s->r = (u32int)key[4] | - (u32int)key[5]<<8 | - (u32int)key[6]<<16 | - (u32int)key[7]<<24; - s->m = 0; - s->n = 0; -} - -static void -micupdate(MICstate *s, uchar *data, ulong len) -{ - u32int l, r, m, n, e; - - l = s->l; - r = s->r; - m = s->m; - n = s->n; - e = n + len; - while(n != e){ - m >>= 8; - m |= (u32int)*data++ << 24; - if(++n & 3) - continue; - l ^= m; - r ^= (l << 17) | (l >> 15); - l += r; - r ^= ((l & 0x00FF00FFUL)<<8) | ((l & 0xFF00FF00UL)>>8); - l += r; - r ^= (l << 3) | (l >> 29); - l += r; - r ^= (l >> 2) | (l << 30); - l += r; - } - s->l = l; - s->r = r; - s->m = m; - s->n = n; -} - -static void -micfinish(MICstate *s, uchar mic[8]) -{ - static uchar pad[8] = { 0x5a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; - - micupdate(s, pad, sizeof(pad)); - - mic[0] = s->l; - mic[1] = s->l>>8; - mic[2] = s->l>>16; - mic[3] = s->l>>24; - mic[4] = s->r; - mic[5] = s->r>>8; - mic[6] = s->r>>16; - mic[7] = s->r>>24; -} - -static uchar pad4[4] = { 0x00, 0x00, 0x00, 0x00, }; - -static void -tkipencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc) -{ - u16int tk[8], p1k[5]; - uchar seed[16]; - RC4state rs; - MICstate ms; - ulong crc; - - micsetup(&ms, k->key+24); - micupdate(&ms, dstaddr(w), Eaddrlen); - micupdate(&ms, srcaddr(w), Eaddrlen); - micupdate(&ms, pad4, 4); - micupdate(&ms, b->rp, BLEN(b)); - micfinish(&ms, b->wp); - b->wp += 8; - - crc = ethercrc(b->rp, BLEN(b)); - crc = ~crc; - b->wp[0] = crc; - b->wp[1] = crc>>8; - b->wp[2] = crc>>16; - b->wp[3] = crc>>24; - b->wp += 4; - - tkipk2tk(k->key, tk); - tkipphase1(tsc >> 16, w->a2, tk, p1k); - tkipphase2(tsc & 0xFFFF, p1k, tk, seed); - setupRC4state(&rs, seed, sizeof(seed)); - rc4(&rs, b->rp, BLEN(b)); -} - -static int -tkipdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc) -{ - uchar seed[16], mic[8]; - u16int tk[8], p1k[5]; - RC4state rs; - MICstate ms; - ulong crc; - - if(BLEN(b) < 8+4) - return -1; - - tkipk2tk(k->key, tk); - tkipphase1(tsc >> 16, w->a2, tk, p1k); - tkipphase2(tsc & 0xFFFF, p1k, tk, seed); - setupRC4state(&rs, seed, sizeof(seed)); - rc4(&rs, b->rp, BLEN(b)); - - b->wp -= 4; - crc = (ulong)b->wp[0] | - (ulong)b->wp[1]<<8 | - (ulong)b->wp[2]<<16 | - (ulong)b->wp[3]<<24; - crc = ~crc; - crc ^= ethercrc(b->rp, BLEN(b)); - - b->wp -= 8; - micsetup(&ms, k->key+16); - micupdate(&ms, dstaddr(w), Eaddrlen); - micupdate(&ms, srcaddr(w), Eaddrlen); - micupdate(&ms, pad4, 4); - micupdate(&ms, b->rp, BLEN(b)); - micfinish(&ms, mic); - - return tsmemcmp(b->wp, mic, 8) | crc; -} - -static uchar* -putbe(uchar *p, int L, uint v) -{ - while(--L >= 0) - *p++ = (v >> L*8) & 0xFF; - return p; -} - -static void -xblock(int L, int M, uchar *N, uchar *a, int la, int lm, uchar t[16], AESstate *s) -{ - uchar l[8], *p, *x, *e; - - assert(M >= 4 && M <= 16); - assert(L >= 2 && L <= 4); - - t[0] = ((la > 0)<<6) | ((M-2)/2)<<3 | (L-1); /* flags */ - memmove(&t[1], N, 15-L); - putbe(&t[16-L], L, lm); - aes_encrypt(s->ekey, s->rounds, t, t); - - if(la > 0){ - assert(la < 0xFF00); - for(p = l, e = putbe(l, 2, la), x = t; p < e; x++, p++) - *x ^= *p; - for(e = a + la; a < e; x = t){ - for(; a < e && x < &t[16]; x++, a++) - *x ^= *a; - aes_encrypt(s->ekey, s->rounds, t, t); - } - } -} - -static uchar* -sblock(int L, uchar *N, uint i, uchar b[16], AESstate *s) -{ - b[0] = L-1; /* flags */ - memmove(&b[1], N, 15-L); - putbe(&b[16-L], L, i); - aes_encrypt(s->ekey, s->rounds, b, b); - return b; -}; - -static void -aesCCMencrypt(int L, int M, uchar *N /* N[15-L] */, - uchar *a /* a[la] */, int la, - uchar *m /* m[lm+M] */, int lm, - AESstate *s) -{ - uchar t[16], b[16], *p, *x; - uint i; - - xblock(L, M, N, a, la, lm, t, s); - - for(i = 1; lm >= 16; i++, m += 16, lm -= 16){ - sblock(L, N, i, b, s); - - *((u32int*)&t[0]) ^= *((u32int*)&m[0]); - *((u32int*)&m[0]) ^= *((u32int*)&b[0]); - *((u32int*)&t[4]) ^= *((u32int*)&m[4]); - *((u32int*)&m[4]) ^= *((u32int*)&b[4]); - *((u32int*)&t[8]) ^= *((u32int*)&m[8]); - *((u32int*)&m[8]) ^= *((u32int*)&b[8]); - *((u32int*)&t[12]) ^= *((u32int*)&m[12]); - *((u32int*)&m[12]) ^= *((u32int*)&b[12]); - - aes_encrypt(s->ekey, s->rounds, t, t); - } - if(lm > 0){ - for(p = sblock(L, N, i, b, s), x = t; p < &b[lm]; x++, m++, p++){ - *x ^= *m; - *m ^= *p; - } - aes_encrypt(s->ekey, s->rounds, t, t); - } - - for(p = sblock(L, N, 0, b, s), x = t; p < &b[M]; x++, p++) - *x ^= *p; - - memmove(m, t, M); -} - -static int -aesCCMdecrypt(int L, int M, uchar *N /* N[15-L] */, - uchar *a /* a[la] */, int la, - uchar *m /* m[lm+M] */, int lm, - AESstate *s) -{ - uchar t[16], b[16], *p, *x; - uint i; - - xblock(L, M, N, a, la, lm, t, s); - - for(i = 1; lm >= 16; i++, m += 16, lm -= 16){ - sblock(L, N, i, b, s); - - *((u32int*)&m[0]) ^= *((u32int*)&b[0]); - *((u32int*)&t[0]) ^= *((u32int*)&m[0]); - *((u32int*)&m[4]) ^= *((u32int*)&b[4]); - *((u32int*)&t[4]) ^= *((u32int*)&m[4]); - *((u32int*)&m[8]) ^= *((u32int*)&b[8]); - *((u32int*)&t[8]) ^= *((u32int*)&m[8]); - *((u32int*)&m[12]) ^= *((u32int*)&b[12]); - *((u32int*)&t[12]) ^= *((u32int*)&m[12]); - - aes_encrypt(s->ekey, s->rounds, t, t); - } - if(lm > 0){ - for(p = sblock(L, N, i, b, s), x = t; p < &b[lm]; x++, m++, p++){ - *m ^= *p; - *x ^= *m; - } - aes_encrypt(s->ekey, s->rounds, t, t); - } - - for(p = sblock(L, N, 0, b, s), x = t; p < &b[M]; x++, p++) - *x ^= *p; - - return tsmemcmp(m, t, M); -} - -static int -setupCCMP(Wifipkt *w, uvlong tsc, uchar nonce[13], uchar auth[32]) -{ - uchar *p; - - nonce[0] = ((w->fc[0] & 0x0c) == 0x00) << 4; - memmove(&nonce[1], w->a2, Eaddrlen); - nonce[7] = tsc >> 40; - nonce[8] = tsc >> 32; - nonce[9] = tsc >> 24; - nonce[10] = tsc >> 16; - nonce[11] = tsc >> 8; - nonce[12] = tsc; - - p = auth; - *p++ = (w->fc[0] & (((w->fc[0] & 0x0c) == 0x08) ? 0x0f : 0xff)); - *p++ = (w->fc[1] & ~0x38) | 0x40; - memmove(p, w->a1, Eaddrlen); p += Eaddrlen; - memmove(p, w->a2, Eaddrlen); p += Eaddrlen; - memmove(p, w->a3, Eaddrlen); p += Eaddrlen; - *p++ = w->seq[0] & 0x0f; - *p++ = 0; - if((w->fc[1] & 3) == 0x03) { - memmove(p, w->a4, Eaddrlen); - p += Eaddrlen; - } - - return p - auth; -} - -static void -ccmpencrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc) -{ - uchar auth[32], nonce[13]; - - aesCCMencrypt(2, 8, nonce, auth, - setupCCMP(w, tsc, nonce, auth), - b->rp, BLEN(b), (AESstate*)k->key); - b->wp += 8; -} - -static int -ccmpdecrypt(Wkey *k, Wifipkt *w, Block *b, uvlong tsc) -{ - uchar auth[32], nonce[13]; - - if(BLEN(b) < 8) - return -1; - - b->wp -= 8; - return aesCCMdecrypt(2, 8, nonce, auth, - setupCCMP(w, tsc, nonce, auth), - b->rp, BLEN(b), (AESstate*)k->key); } /* --- a/sys/src/9/port/wifi.h +++ b/sys/src/9/port/wifi.h @@ -1,7 +1,7 @@ typedef struct Wkey Wkey; typedef struct Wnode Wnode; +typedef struct Wframe Wframe; typedef struct Wifi Wifi; -typedef struct Wifipkt Wifipkt; typedef struct DMAT DMAT; typedef struct DMTE DMTE; @@ -15,6 +15,53 @@ CCMP = 2, }; +/* 802.11 frame fc[0] */ +enum { + Wfc0vermask = 0x03, + Wfc0typemask = 0x0c, + Wfc0submask = 0xf0, + + /* frame types */ + Wfc0typemgt = 0x00, + Wfc0typectl = 0x04, + Wfc0typedat = 0x08, + + /* frame subtypes (management) */ + Wfc0subassoc = 0x00, + Wfc0subassocr = 0x10, + Wfc0subreassoc = 0x20, + Wfc0subreassocr = 0x30, + Wfc0subprobe = 0x40, + Wfc0subprober = 0x50, + Wfc0subbeacon = 0x80, + Wfc0subdisassoc = 0xa0, + Wfc0subauth = 0xb0, + Wfc0subdeauth = 0xc0, + + /* frame subtypes (data) */ + Wfc0subdata = 0x00, + Wfc0subnodata = 0x40, + Wfc0subqos = 0x80, +}; + +/* 802.11 frame fc[1] */ +enum { + Wfc1dirmask = 0x03, + + /* frame direction */ + Wfc1dirstatosta = 0x00, + Wfc1dirstatoap = 0x01, + Wfc1diraptosta = 0x02, + Wfc1diraptoap = 0x03, + + Wfc1frag = 0x04, + Wfc1retry = 0x08, + Wfc1pwrmgt = 0x10, + Wfc1data = 0x20, + Wfc1prot = 0x40, + Wfc1order = 0x80, +}; + struct Wkey { int cipher; @@ -61,6 +108,22 @@ uchar brsne[258]; }; +struct Wframe +{ + u8int fc[2]; + + u16int dur; + u16int seq; + u16int qos; + + u32int ht; + + uchar a1[Eaddrlen]; + uchar a2[Eaddrlen]; + uchar a3[Eaddrlen]; + uchar a4[Eaddrlen]; +}; + struct DMTE { uchar ip[16]; @@ -85,6 +148,7 @@ ulong watchdog; ulong lastauth; Ref txseq; + void (*transmit)(Wifi*, Wnode*, Block*); /* for searching */ @@ -102,26 +166,29 @@ DMAT dmat; }; -struct Wifipkt -{ - uchar fc[2]; - uchar dur[2]; - uchar a1[Eaddrlen]; - uchar a2[Eaddrlen]; - uchar a3[Eaddrlen]; - uchar seq[2]; - uchar a4[Eaddrlen]; -}; +u16int get16(uchar *p); +u32int get32(uchar *p); +u64int get64(uchar *p); -enum { - Btimestamp = 1<<15, -}; +void put16(uchar *p, u16int v); +void put32(uchar *p, u32int v); +void put64(uchar *p, u64int v); Wifi *wifiattach(Ether *ether, void (*transmit)(Wifi*, Wnode*, Block*)); + void wifiiq(Wifi*, Block*); -int wifihdrlen(Wifipkt*); void wifitxfail(Wifi*, Block*); long wifistat(Wifi*, void*, long, ulong); long wifictl(Wifi*, void*, long); void wificfg(Wifi*, char*); + +uchar *wframesrc(Wframe *); +uchar *wframedst(Wframe *); + +int wframelen(Wframe *); +int wframepack(Wframe *, uchar *p); +int wframeunpack(Wframe *, uchar *p); + +Block *wifiencrypt(Wifi *, Wnode *, Wframe *, Block *); +Block *wifidecrypt(Wifi *, Wnode *, Wframe *, Block *); --- /dev/null +++ b/sys/src/9/port/wificrypt.c @@ -1,0 +1,610 @@ +#include "u.h" +#include "../port/lib.h" +#include "mem.h" +#include "dat.h" +#include "fns.h" +#include "io.h" +#include "ureg.h" +#include "../port/error.h" +#include "../port/netif.h" +#include "../port/etherif.h" +#include "../port/wifi.h" + +#include + +static u16int Sbox[256] = { + 0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154, + 0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A, + 0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B, + 0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B, + 0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F, + 0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F, + 0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5, + 0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F, + 0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB, + 0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397, + 0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED, + 0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A, + 0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194, + 0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3, + 0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104, + 0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D, + 0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39, + 0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695, + 0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83, + 0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76, + 0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4, + 0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B, + 0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0, + 0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018, + 0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751, + 0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85, + 0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12, + 0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9, + 0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7, + 0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A, + 0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8, + 0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A +}; + +static void +tkipk2tk(uchar key[16], u16int tk[8]) +{ + tk[0] = (u16int)key[1]<<8 | key[0]; + tk[1] = (u16int)key[3]<<8 | key[2]; + tk[2] = (u16int)key[5]<<8 | key[4]; + tk[3] = (u16int)key[7]<<8 | key[6]; + tk[4] = (u16int)key[9]<<8 | key[8]; + tk[5] = (u16int)key[11]<<8 | key[10]; + tk[6] = (u16int)key[13]<<8 | key[12]; + tk[7] = (u16int)key[15]<<8 | key[14]; +} + +static void +tkipphase1(u32int tscu, uchar ta[Eaddrlen], u16int tk[8], u16int p1k[5]) +{ + u16int *k, i, x0, x1, x2; + + p1k[0] = tscu; + p1k[1] = tscu>>16; + p1k[2] = (u16int)ta[1]<<8 | ta[0]; + p1k[3] = (u16int)ta[3]<<8 | ta[2]; + p1k[4] = (u16int)ta[5]<<8 | ta[4]; + + for(i=0; i<8; i++){ + k = &tk[i & 1]; + + x0 = p1k[4] ^ k[0]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + p1k[0] += x2; + x0 = p1k[0] ^ k[2]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + p1k[1] += x2; + x0 = p1k[1] ^ k[4]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + p1k[2] += x2; + x0 = p1k[2] ^ k[6]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + p1k[3] += x2; + x0 = p1k[3] ^ k[0]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + p1k[4] += x2; + + p1k[4] += i; + } +} + +static void +tkipphase2(u16int tscl, u16int p1k[5], u16int tk[8], uchar rc4key[16]) +{ + u16int ppk[6], x0, x1, x2; + + ppk[0] = p1k[0]; + ppk[1] = p1k[1]; + ppk[2] = p1k[2]; + ppk[3] = p1k[3]; + ppk[4] = p1k[4]; + ppk[5] = p1k[4] + tscl; + + x0 = ppk[5] ^ tk[0]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + ppk[0] += x2; + x0 = ppk[0] ^ tk[1]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + ppk[1] += x2; + x0 = ppk[1] ^ tk[2]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + ppk[2] += x2; + x0 = ppk[2] ^ tk[3]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + ppk[3] += x2; + x0 = ppk[3] ^ tk[4]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + ppk[4] += x2; + x0 = ppk[4] ^ tk[5]; + x1 = Sbox[x0 >> 8]; + x2 = Sbox[x0 & 0xFF] ^ ((x1>>8) | (x1<<8)); + ppk[5] += x2; + + x2 = ppk[5] ^ tk[6]; + ppk[0] += (x2 >> 1) | (x2 << 15); + x2 = ppk[0] ^ tk[7]; + ppk[1] += (x2 >> 1) | (x2 << 15); + + x2 = ppk[1]; + ppk[2] += (x2 >> 1) | (x2 << 15); + x2 = ppk[2]; + ppk[3] += (x2 >> 1) | (x2 << 15); + x2 = ppk[3]; + ppk[4] += (x2 >> 1) | (x2 << 15); + x2 = ppk[4]; + ppk[5] += (x2 >> 1) | (x2 << 15); + + rc4key[0] = tscl >> 8; + rc4key[1] = (rc4key[0] | 0x20) & 0x7F; + rc4key[2] = tscl; + rc4key[3] = (ppk[5] ^ tk[0]) >> 1; + rc4key[4] = ppk[0]; + rc4key[5] = ppk[0] >> 8; + rc4key[6] = ppk[1]; + rc4key[7] = ppk[1] >> 8; + rc4key[8] = ppk[2]; + rc4key[9] = ppk[2] >> 8; + rc4key[10] = ppk[3]; + rc4key[11] = ppk[3] >> 8; + rc4key[12] = ppk[4]; + rc4key[13] = ppk[4] >> 8; + rc4key[14] = ppk[5]; + rc4key[15] = ppk[5] >> 8; +} + +typedef struct MICstate MICstate; +struct MICstate +{ + u32int l; + u32int r; + u32int m; + u32int n; +}; + +static void +micsetup(MICstate *s, uchar key[8]) +{ + s->l = (u32int)key[0] | + (u32int)key[1]<<8 | + (u32int)key[2]<<16 | + (u32int)key[3]<<24; + s->r = (u32int)key[4] | + (u32int)key[5]<<8 | + (u32int)key[6]<<16 | + (u32int)key[7]<<24; + s->m = 0; + s->n = 0; +} + +static void +micupdate(MICstate *s, uchar *data, ulong len) +{ + u32int l, r, m, n, e; + + l = s->l; + r = s->r; + m = s->m; + n = s->n; + e = n + len; + while(n != e){ + m >>= 8; + m |= (u32int)*data++ << 24; + if(++n & 3) + continue; + l ^= m; + r ^= (l << 17) | (l >> 15); + l += r; + r ^= ((l & 0x00FF00FFUL)<<8) | ((l & 0xFF00FF00UL)>>8); + l += r; + r ^= (l << 3) | (l >> 29); + l += r; + r ^= (l >> 2) | (l << 30); + l += r; + } + s->l = l; + s->r = r; + s->m = m; + s->n = n; +} + +static void +micfinish(MICstate *s, uchar mic[8]) +{ + static uchar pad[8] = { 0x5a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; + + micupdate(s, pad, sizeof(pad)); + + mic[0] = s->l; + mic[1] = s->l>>8; + mic[2] = s->l>>16; + mic[3] = s->l>>24; + mic[4] = s->r; + mic[5] = s->r>>8; + mic[6] = s->r>>16; + mic[7] = s->r>>24; +} + +static uchar pad4[4] = { 0x00, 0x00, 0x00, 0x00, }; + +static void +tkipencrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc) +{ + u16int tk[8], p1k[5]; + uchar seed[16]; + RC4state rs; + MICstate ms; + ulong crc; + + micsetup(&ms, k->key+24); + micupdate(&ms, wframedst(f), Eaddrlen); + micupdate(&ms, wframesrc(f), Eaddrlen); + micupdate(&ms, pad4, 4); + micupdate(&ms, b->rp, BLEN(b)); + micfinish(&ms, b->wp); + b->wp += 8; + + crc = ethercrc(b->rp, BLEN(b)); + crc = ~crc; + b->wp[0] = crc; + b->wp[1] = crc>>8; + b->wp[2] = crc>>16; + b->wp[3] = crc>>24; + b->wp += 4; + + tkipk2tk(k->key, tk); + tkipphase1(tsc >> 16, f->a2, tk, p1k); + tkipphase2(tsc & 0xFFFF, p1k, tk, seed); + setupRC4state(&rs, seed, sizeof(seed)); + rc4(&rs, b->rp, BLEN(b)); +} + +static int +tkipdecrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc) +{ + uchar seed[16], mic[8]; + u16int tk[8], p1k[5]; + RC4state rs; + MICstate ms; + ulong crc; + + if(BLEN(b) < 8+4) + return -1; + + tkipk2tk(k->key, tk); + tkipphase1(tsc >> 16, f->a2, tk, p1k); + tkipphase2(tsc & 0xFFFF, p1k, tk, seed); + setupRC4state(&rs, seed, sizeof(seed)); + rc4(&rs, b->rp, BLEN(b)); + + b->wp -= 4; + crc = (ulong)b->wp[0] | + (ulong)b->wp[1]<<8 | + (ulong)b->wp[2]<<16 | + (ulong)b->wp[3]<<24; + crc = ~crc; + crc ^= ethercrc(b->rp, BLEN(b)); + + b->wp -= 8; + micsetup(&ms, k->key+16); + micupdate(&ms, wframedst(f), Eaddrlen); + micupdate(&ms, wframesrc(f), Eaddrlen); + micupdate(&ms, pad4, 4); + micupdate(&ms, b->rp, BLEN(b)); + micfinish(&ms, mic); + + return tsmemcmp(b->wp, mic, 8) | crc; +} + +static uchar* +putbe(uchar *p, int L, uint v) +{ + while(--L >= 0) + *p++ = (v >> L*8) & 0xFF; + return p; +} + +static void +xblock(int L, int M, uchar *N, uchar *a, int la, int lm, uchar t[16], AESstate *s) +{ + uchar l[8], *p, *x, *e; + + assert(M >= 4 && M <= 16); + assert(L >= 2 && L <= 4); + + t[0] = ((la > 0)<<6) | ((M-2)/2)<<3 | (L-1); /* flags */ + memmove(&t[1], N, 15-L); + putbe(&t[16-L], L, lm); + aes_encrypt(s->ekey, s->rounds, t, t); + + if(la > 0){ + assert(la < 0xFF00); + for(p = l, e = putbe(l, 2, la), x = t; p < e; x++, p++) + *x ^= *p; + for(e = a + la; a < e; x = t){ + for(; a < e && x < &t[16]; x++, a++) + *x ^= *a; + aes_encrypt(s->ekey, s->rounds, t, t); + } + } +} + +static uchar* +sblock(int L, uchar *N, uint i, uchar b[16], AESstate *s) +{ + b[0] = L-1; /* flags */ + memmove(&b[1], N, 15-L); + putbe(&b[16-L], L, i); + aes_encrypt(s->ekey, s->rounds, b, b); + return b; +}; + +static void +aesCCMencrypt(int L, int M, uchar *N /* N[15-L] */, + uchar *a /* a[la] */, int la, + uchar *m /* m[lm+M] */, int lm, + AESstate *s) +{ + uchar t[16], b[16], *p, *x; + uint i; + + xblock(L, M, N, a, la, lm, t, s); + + for(i = 1; lm >= 16; i++, m += 16, lm -= 16){ + sblock(L, N, i, b, s); + + *((u32int*)&t[0]) ^= *((u32int*)&m[0]); + *((u32int*)&m[0]) ^= *((u32int*)&b[0]); + *((u32int*)&t[4]) ^= *((u32int*)&m[4]); + *((u32int*)&m[4]) ^= *((u32int*)&b[4]); + *((u32int*)&t[8]) ^= *((u32int*)&m[8]); + *((u32int*)&m[8]) ^= *((u32int*)&b[8]); + *((u32int*)&t[12]) ^= *((u32int*)&m[12]); + *((u32int*)&m[12]) ^= *((u32int*)&b[12]); + + aes_encrypt(s->ekey, s->rounds, t, t); + } + if(lm > 0){ + for(p = sblock(L, N, i, b, s), x = t; p < &b[lm]; x++, m++, p++){ + *x ^= *m; + *m ^= *p; + } + aes_encrypt(s->ekey, s->rounds, t, t); + } + + for(p = sblock(L, N, 0, b, s), x = t; p < &b[M]; x++, p++) + *x ^= *p; + + memmove(m, t, M); +} + +static int +aesCCMdecrypt(int L, int M, uchar *N /* N[15-L] */, + uchar *a /* a[la] */, int la, + uchar *m /* m[lm+M] */, int lm, + AESstate *s) +{ + uchar t[16], b[16], *p, *x; + uint i; + + xblock(L, M, N, a, la, lm, t, s); + + for(i = 1; lm >= 16; i++, m += 16, lm -= 16){ + sblock(L, N, i, b, s); + + *((u32int*)&m[0]) ^= *((u32int*)&b[0]); + *((u32int*)&t[0]) ^= *((u32int*)&m[0]); + *((u32int*)&m[4]) ^= *((u32int*)&b[4]); + *((u32int*)&t[4]) ^= *((u32int*)&m[4]); + *((u32int*)&m[8]) ^= *((u32int*)&b[8]); + *((u32int*)&t[8]) ^= *((u32int*)&m[8]); + *((u32int*)&m[12]) ^= *((u32int*)&b[12]); + *((u32int*)&t[12]) ^= *((u32int*)&m[12]); + + aes_encrypt(s->ekey, s->rounds, t, t); + } + if(lm > 0){ + for(p = sblock(L, N, i, b, s), x = t; p < &b[lm]; x++, m++, p++){ + *m ^= *p; + *x ^= *m; + } + aes_encrypt(s->ekey, s->rounds, t, t); + } + + for(p = sblock(L, N, 0, b, s), x = t; p < &b[M]; x++, p++) + *x ^= *p; + + return tsmemcmp(m, t, M); +} + +static int +ccmpsetup(Wframe *f, uvlong tsc, uchar nonce[13], uchar auth[32]) +{ + uchar *p; + + nonce[0] = ((f->fc[0] & 0x0c) == 0x00) << 4; + memmove(&nonce[1], f->a2, Eaddrlen); + nonce[7] = tsc >> 40; + nonce[8] = tsc >> 32; + nonce[9] = tsc >> 24; + nonce[10] = tsc >> 16; + nonce[11] = tsc >> 8; + nonce[12] = tsc; + + p = auth; + *p++ = (f->fc[0] & (((f->fc[0] & 0x0c) == 0x08) ? 0x0f : 0xff)); + *p++ = (f->fc[1] & ~0x38) | 0x40; + memmove(p, f->a1, Eaddrlen); p += Eaddrlen; + memmove(p, f->a2, Eaddrlen); p += Eaddrlen; + memmove(p, f->a3, Eaddrlen); p += Eaddrlen; + *p++ = f->seq & 0x0f; + *p++ = 0; + if((f->fc[1] & 3) == 0x03) { + memmove(p, f->a4, Eaddrlen); + p += Eaddrlen; + } + + return p - auth; +} + + +static void +ccmpencrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc) +{ + uchar auth[32], nonce[13]; + + aesCCMencrypt(2, 8, nonce, auth, + ccmpsetup(f, tsc, nonce, auth), + b->rp, BLEN(b), (AESstate*)k->key); + b->wp += 8; +} + +static int +ccmpdecrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc) +{ + uchar auth[32], nonce[13]; + + if(BLEN(b) < 8) + return -1; + + b->wp -= 8; + return aesCCMdecrypt(2, 8, nonce, auth, + ccmpsetup(f, tsc, nonce, auth), + b->rp, BLEN(b), (AESstate*)k->key); +} + +Block* +wifiencrypt(Wifi *wifi, Wnode *wn, Wframe *f, Block *b) +{ + uvlong tsc; + int kid; + Wkey *k; + uchar *p; + + rlock(&wifi->crypt); + + kid = 0; + k = wn->txkey[kid]; + if(k == nil){ + runlock(&wifi->crypt); + return b; + } + + b = padblock(b, 8); + b = padblock(b, -(8+4)); + p = b->rp; + + tsc = ++k->tsc; + + switch(k->cipher){ + case TKIP: + b->rp[0] = tsc>>8; + b->rp[1] = (b->rp[0] | 0x20) & 0x7f; + b->rp[2] = tsc; + b->rp[3] = kid<<6 | 0x20; + b->rp[4] = tsc>>16; + b->rp[5] = tsc>>24; + b->rp[6] = tsc>>32; + b->rp[7] = tsc>>40; + b->rp += 8; + tkipencrypt(k, f, b, tsc); + break; + case CCMP: + b->rp[0] = tsc; + b->rp[1] = tsc>>8; + b->rp[2] = 0; + b->rp[3] = kid<<6 | 0x20; + b->rp[4] = tsc>>16; + b->rp[5] = tsc>>24; + b->rp[6] = tsc>>32; + b->rp[7] = tsc>>40; + b->rp += 8; + ccmpencrypt(k, f, b, tsc); + break; + } + + runlock(&wifi->crypt); + f->fc[1] |= Wfc1prot; + b->rp = p; + return b; +} + +Block* +wifidecrypt(Wifi *wifi, Wnode *wn, Wframe *f, Block *b) +{ + uvlong tsc; + int kid; + Wkey *k; + uchar *p; + + p = b->rp; b->rp += wframelen(f); + + rlock(&wifi->crypt); + if(BLEN(b) < 8+8) + goto drop; + + kid = b->rp[3]>>6; + if((b->rp[3] & 0x20) == 0) + goto drop; + if((f->a1[0] & 1) == 0) + kid = 4; /* use peerwise key for non-unicast */ + + k = wn->rxkey[kid]; + if(k == nil) + goto drop; + switch(k->cipher){ + case TKIP: + tsc = (uvlong)b->rp[7]<<40 | + (uvlong)b->rp[6]<<32 | + (uvlong)b->rp[5]<<24 | + (uvlong)b->rp[4]<<16 | + (uvlong)b->rp[0]<<8 | + (uvlong)b->rp[2]; + b->rp += 8; + if(tsc <= k->tsc) + goto drop; + if(tkipdecrypt(k, f, b, tsc) != 0) + goto drop; + break; + case CCMP: + tsc = (uvlong)b->rp[7]<<40 | + (uvlong)b->rp[6]<<32 | + (uvlong)b->rp[5]<<24 | + (uvlong)b->rp[4]<<16 | + (uvlong)b->rp[1]<<8 | + (uvlong)b->rp[0]; + b->rp += 8; + if(tsc <= k->tsc) + goto drop; + if(ccmpdecrypt(k, f, b, tsc) != 0) + goto drop; + break; + default: + drop: + runlock(&wifi->crypt); + freeb(b); + return nil; + } + runlock(&wifi->crypt); + + k->tsc = tsc; + f->fc[1] &= ~Wfc1prot; + b->rp = p + 8; + wframepack(f, b->rp); + return b; +} --- /dev/null +++ b/sys/src/9/port/wifipack.c @@ -1,0 +1,197 @@ +#include "u.h" +#include "../port/lib.h" +#include "mem.h" +#include "dat.h" +#include "fns.h" +#include "../port/netif.h" +#include "../port/etherif.h" +#include "../port/wifi.h" + +u16int +get16(uchar *p) +{ + u16int v; + + v = p[0]; + v |= p[1] << 8; + + return v; +} + +u32int +get32(uchar *p) +{ + u32int v; + + v = p[0]; + v |= p[1] << 8; + v |= p[2] << 16; + v |= p[3] << 24; + + return v; +} + +u64int +get64(uchar *p) +{ + u64int v; + + v = (u64int)p[0]; + v |= (u64int)p[1] << 8; + v |= (u64int)p[2] << 16; + v |= (u64int)p[3] << 24; + v |= (u64int)p[4] << 32; + v |= (u64int)p[5] << 40; + v |= (u64int)p[6] << 48; + v |= (u64int)p[7] << 56; + + return v; +} + +void +put16(uchar *p, u16int v) +{ + p[0] = v; + p[1] = v >> 8; +} + +void +put32(uchar *p, u32int v) +{ + p[0] = v; + p[1] = v >> 8; + p[2] = v >> 16; + p[3] = v >> 24; +} + +void +put64(uchar *p, u64int v) +{ + p[0] = v; + p[1] = v >> 8; + p[2] = v >> 16; + p[3] = v >> 24; + p[4] = v >> 32; + p[5] = v >> 40; + p[6] = v >> 48; + p[7] = v >> 56; +} + +uchar* +wframesrc(Wframe *f) +{ + if((f->fc[1] & 0x02) == 0) + return f->a2; + if((f->fc[1] & 0x01) == 0) + return f->a3; + + return f->a4; +} + +uchar* +wframedst(Wframe *f) +{ + if((f->fc[1] & 0x01) != 0) + return f->a3; + + return f->a1; +} + +int +wframelen(Wframe *f) +{ + int l = 2+2+3*6+2; + + /* got a4? */ + if((f->fc[1] & Wfc1dirmask) == Wfc1diraptoap) + l += Eaddrlen; + + /* got qos? */ + if((f->fc[0] & Wfc0typemask) == Wfc0typedat) { + if((f->fc[0] & Wfc0submask) == Wfc0subqos) { + l += 2; + + /* got ht? */ + if(f->fc[1] & Wfc1order) + l += 4; + } + } + + return l; +} + +int +wframepack(Wframe *f, uchar *p) +{ + uchar *s = p; + + *p++ = f->fc[0]; + *p++ = f->fc[1]; + + put16(p, f->dur); p += 2; + + memmove(p, f->a1, Eaddrlen); p += Eaddrlen; + memmove(p, f->a2, Eaddrlen); p += Eaddrlen; + memmove(p, f->a3, Eaddrlen); p += Eaddrlen; + + put16(p, f->dur); p += 2; + + /* got a4? */ + if((f->fc[1] & Wfc1dirmask) == Wfc1diraptoap) { + memmove(p, f->a4, Eaddrlen); + p += Eaddrlen; + } + + /* got qos? */ + if((f->fc[0] & Wfc0typemask) == Wfc0typedat) { + if((f->fc[0] & Wfc0submask) == Wfc0subqos) { + put16(p, f->qos); p += 2; + + /* got ht? */ + if(f->fc[1] & Wfc1order) { + put32(p, f->ht); + p += 4; + } + } + } + + return p - s; +} + +int +wframeunpack(Wframe *f, uchar *p) +{ + uchar *s = p; + + f->fc[0] = *p++; + f->fc[1] = *p++; + + f->dur = get16(p); p += 2; + + memmove(f->a1, p, Eaddrlen); p += Eaddrlen; + memmove(f->a2, p, Eaddrlen); p += Eaddrlen; + memmove(f->a3, p, Eaddrlen); p += Eaddrlen; + + f->seq = get16(p); p += 2; + + /* got a4? */ + if((f->fc[1] & Wfc1dirmask) == Wfc1diraptoap) { + memmove(f->a4, p, Eaddrlen); + p += Eaddrlen; + } + + /* got qos? */ + if((f->fc[0] & Wfc0typemask) == Wfc0typedat) { + if((f->fc[0] & Wfc0submask) == Wfc0subqos) { + f->qos = get16(p); p += 2; + + /* got ht? */ + if(f->fc[1] & Wfc1order) { + f->ht = get32(p); + p += 4; + } + } + } + + return p - s; +}