From: mia soweli Date: Thu, 08 Jun 2023 20:13:04 +0000 Subject: [PATCH] wifi: factor out structs and crypto --- diff 04531178e5872af6fe4b82b4099496b153cdc8a8 0e5c0628e4b321e0a4f165961ab0abb1a058ada6 --- 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 @@ -2580,7 +2580,7 @@ tx = &ctlr->tx[qid]; nodeid = ctlr->bcastnodeid; - hdrlen = wifiunpack(&w, b->rp); + hdrlen = wframeunpack(&w, b->rp); p = pool->p + pool->i * TxwiDmaSz; if((w.a1[0] & 1) == 0){ *(p+4) = TxAck; /* xflags */ @@ -2740,13 +2740,13 @@ len = get16(b->rp + 2 /* wcid, keyidx */) & 0xfff; b->rp = d + Rxwisize; b->wp = b->rp + len; - hdrlen = wifiunpack(&w, b->rp); + 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; } - wifiunpack(&w, 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); wifiiq(ctlr->wifi, b); --- a/sys/src/9/pc/etherwpi.c +++ b/sys/src/9/pc/etherwpi.c @@ -1413,7 +1413,7 @@ nodeid = ctlr->bcastnodeid; p = wn->minrate; - wifiunpack(&w, b->rp); + wframeunpack(&w, b->rp); if((w.a1[0] & 1) == 0){ flags |= TFlagNeedACK; --- 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 @@ -3856,7 +3856,7 @@ sta = &ctlr->bcast; p = wn->minrate; - wifiunpack(&w, b->rp); + wframeunpack(&w, b->rp); if((w.a1[0] & 1) == 0){ flags |= TFlagNeedACK; --- a/sys/src/9/port/wifi.c +++ b/sys/src/9/port/wifi.c @@ -45,143 +45,9 @@ 0 }; -static Block* wifidecrypt(Wifi *, Wnode *, Wframe *, Block *); -static Block* wifiencrypt(Wifi *, Wnode *, Wframe *, Block *); static void freewifikeys(Wifi *, Wnode *); - static void dmatproxy(Block *bp, int upstream, uchar proxy[Eaddrlen], DMAT *t); -static uchar* -srcaddr(Wframe *f) -{ - if((f->fc[1] & 0x02) == 0) - return f->a2; - if((f->fc[1] & 0x01) == 0) - return f->a3; - - return f->a4; -} - -static uchar* -dstaddr(Wframe *f) -{ - if((f->fc[1] & 0x01) != 0) - return f->a3; - - return f->a1; -} - -int -wifilen(Wframe *f) -{ - int l = WIFIHDRSIZE; - - /* 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 -wifipack(Wframe *f, uchar *p) -{ - uchar *s = p; - - *p++ = f->fc[0]; - *p++ = f->fc[1]; - *p++ = f->dur; - *p++ = f->dur >> 8; - - memmove(p, f->a1, Eaddrlen); p += Eaddrlen; - memmove(p, f->a2, Eaddrlen); p += Eaddrlen; - memmove(p, f->a3, Eaddrlen); p += Eaddrlen; - - *p++ = f->seq; - *p++ = f->seq >> 8; - - /* 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) { - *p++ = f->qos; - *p++ = f->qos >> 8; - - /* got ht? */ - if(f->fc[1] & Wfc1order) - p += 4; - } - } - - return p - s; -} - -int -wifiunpack(Wframe *f, uchar *p) -{ - uchar *s = p; - - f->fc[0] = *p++; - f->fc[1] = *p++; - - put16(p, f->dur); p += 2; - - memmove(f->a1, p, Eaddrlen); p += Eaddrlen; - memmove(f->a2, p, Eaddrlen); p += Eaddrlen; - memmove(f->a3, p, Eaddrlen); p += Eaddrlen; - - put16(p, f->seq); 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) { - put16(p, f->seq); p += 2; - - /* got ht? */ - if(f->fc[1] & Wfc1order) - p += 4; - } - } - - return p - s; -} - - -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) { @@ -195,10 +61,10 @@ if(BLEN(b) < WIFIHDRSIZE) goto drop; - hdrlen = wifiunpack(&f, b->rp); + hdrlen = wframeunpack(&f, b->rp); if(BLEN(b) < hdrlen) goto drop; - if(memcmp(srcaddr(&f), wifi->ether->ea, Eaddrlen) == 0) + if(memcmp(wframesrc(&f), wifi->ether->ea, Eaddrlen) == 0) goto drop; if(f.fc[1] & 0x40){ @@ -237,8 +103,8 @@ b->rp += SNAPHDRSIZE-ETHERHDRSIZE; e = (Etherpkt*)b->rp; - memmove(e->d, dstaddr(&f), Eaddrlen); - memmove(e->s, srcaddr(&f), 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); @@ -260,13 +126,13 @@ return; } - b = padblock(b, wifilen(f)); + b = padblock(b, wframelen(f)); if(!b) return; f->dur = 0; f->seq = incref(&wifi->txseq) << 4; - wifipack(f, b->rp); + wframepack(f, b->rp); if((wn->txcount++ & 255) == 255 && wn->actrate != nil && wn->actrate != wn->maxrate){ uchar *a, *p; @@ -314,14 +180,14 @@ void wifitxfail(Wifi *wifi, Block *b) { - Wframe w; + Wframe f; Wnode *wn; if(b == nil) return; - wifiunpack(&w, b->rp); - wn = nodelookup(wifi, w.a1, 0); + wframeunpack(&f, b->rp); + wn = nodelookup(wifi, f.a1, 0); if(wn == nil) return; wn->txerror++; @@ -447,12 +313,9 @@ 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; @@ -481,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 */ @@ -531,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; @@ -559,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; @@ -720,7 +577,7 @@ if((b = qbread(wifi->iq, 100000)) == nil) break; - hdrlen = wifiunpack(&f, b->rp); + hdrlen = wframeunpack(&f, b->rp); if(f.fc[1] & Wfc1prot){ /* encrypted */ if((wn = nodelookup(wifi, f.a2, 0)) == nil) @@ -727,7 +584,7 @@ continue; wn->lastseen = MACHP(0)->ticks; if((b = wifidecrypt(wifi, wn, &f, b)) != nil){ - wifiunpack(&f, b->rp); + wframeunpack(&f, b->rp); if(f.fc[1] & Wfc1prot) continue; @@ -736,9 +593,6 @@ b = nil; } continue; - } else { - /* XXX: just no. */ - b->rp += hdrlen; } /* management */ @@ -745,6 +599,7 @@ if((f.fc[0] & Wfc0typemask) != Wfc0typemgt) continue; + b->rp += hdrlen; switch(f.fc[0] & Wfc0submask){ case Wfc0subprober: if(wifi->debug) @@ -755,7 +610,7 @@ continue; wn->lastseen = MACHP(0)->ticks; if(b->flag & Btimestamp) - wn->rs = getts(b->rp - 8); + wn->rs = get64(b->rp - 8); recvbeacon(wifi, wn, b->rp, BLEN(b)); @@ -775,7 +630,7 @@ continue; wn->lastseen = MACHP(0)->ticks; if(b->flag & Btimestamp) - wn->rs = getts(b->rp - 8); + wn->rs = get64(b->rp - 8); switch(f.fc[0] & Wfc0submask){ case Wfc0subassocr: case Wfc0subreassocr: @@ -839,8 +694,8 @@ f.fc[1] = Wfc1diraptoap; memmove(f.a2, wifi->ether->ea, Eaddrlen); } - memmove(dstaddr(&f), e.d, Eaddrlen); - memmove(srcaddr(&f), e.s, Eaddrlen); + memmove(wframedst(&f), e.d, Eaddrlen); + memmove(wframesrc(&f), e.s, Eaddrlen); b = padblock(b, SNAPHDRSIZE); s = (SNAP*)(b->rp); @@ -1235,608 +1090,6 @@ n = readstr(off, buf, n, s); free(s); return n; -} - -static void tkipencrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc); -static int tkipdecrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc); -static void ccmpencrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc); -static int ccmpdecrypt(Wkey *k, Wframe *f, Block *b, uvlong tsc); - -static 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; -} - -static Block* -wifidecrypt(Wifi *wifi, Wnode *wn, Wframe *f, Block *b) -{ - uvlong tsc; - int kid; - Wkey *k; - uchar *p; - - p = b->rp; b->rp += wifilen(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; - wifipack(f, b->rp); - 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, 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, dstaddr(f), Eaddrlen); - micupdate(&ms, srcaddr(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, dstaddr(f), Eaddrlen); - micupdate(&ms, srcaddr(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 -setupCCMP(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, - setupCCMP(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, - setupCCMP(f, tsc, nonce, auth), - b->rp, BLEN(b), (AESstate*)k->key); } /* --- a/sys/src/9/port/wifi.h +++ b/sys/src/9/port/wifi.h @@ -164,24 +164,13 @@ DMAT dmat; }; -#define get16(p) ((p)[0] | (p)[1] << 8) -#define get32(p) ((p)[0] | (p)[1] << 8 | (p)[2] << 16 | (p)[3] << 24) +u16int get16(uchar *p); +u32int get32(uchar *p); +u64int get64(uchar *p); -#define put16(p, v) (p)[0] = (v); (p)[1] = (v) >> 8 -#define put32(p, v) \ - (p)[0] = (v); \ - (p)[1] = (v) >> 8; \ - (p)[2] = (v) >> 16; \ - (p)[3] = (v) >> 24 -#define put64(p, 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 +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*)); @@ -188,10 +177,16 @@ void wifiiq(Wifi*, Block*); void wifitxfail(Wifi*, Block*); -int wifilen(Wframe *); -int wifipack(Wframe *, uchar *p); -int wifiunpack(Wframe *, uchar *p); - 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,193 @@ +#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) + 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) + p += 4; + } + } + + return p - s; +}