ref: 96ac4cdaa40a473766aa985120764dfe9863cd67
dir: /sys/src/9/port/wifi.c/
#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 <libsec.h>
typedef struct SNAP SNAP;
struct SNAP
{
uchar dsap;
uchar ssap;
uchar control;
uchar orgcode[3];
uchar type[2];
};
enum {
WIFIHDRSIZE = 2+2+3*6+2,
SNAPHDRSIZE = 8,
};
static char Sconn[] = "connecting";
static char Sauth[] = "authenticated";
static char Sneedauth[] = "need authentication";
static char Sunauth[] = "unauthenticated";
static char Sassoc[] = "associated";
static char Sunassoc[] = "unassociated";
static char Sblocked[] = "blocked"; /* no keys negotiated. only pass EAPOL frames */
static uchar basicrates[] = {
0x80 | 2, /* 1.0 Mb/s */
0x80 | 4, /* 2.0 Mb/s */
0x80 | 11, /* 5.5 Mb/s */
0x80 | 22, /* 11.0 Mb/s */
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;
Etherpkt *e;
int hdrlen;
if(b->flag & Btimestamp)
assert(b->rp - b->base >= 8);
if(BLEN(b) < WIFIHDRSIZE)
goto drop;
w = (Wifipkt*)b->rp;
hdrlen = wifihdrlen(w);
if(BLEN(b) < hdrlen)
goto drop;
if(memcmp(srcaddr(w), wifi->ether->ea, Eaddrlen) == 0)
goto drop;
if(w->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 */
break;
qpass(wifi->iq, b);
return;
case 0x04: /* control */
break;
case 0x08: /* data */
b->flag &= ~Btimestamp;
b->rp += hdrlen;
switch(w->fc[0] & 0xf0){
default:
goto drop;
case 0x80: /* QOS */
case 0x00:
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->type, s.type, 2);
dmatproxy(b, 0, wifi->ether->ea, &wifi->dmat);
etheriq(wifi->ether, b);
return;
}
drop:
freeb(b);
}
static void
wifitx(Wifi *wifi, Wnode *wn, 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(b == nil)
return;
}
if((wn->txcount++ & 255) == 255 && wn->actrate != nil && wn->actrate != wn->maxrate){
uchar *a, *p;
for(a = wn->maxrate, p = wifi->rates; *p; p++){
if(*p < *a && *p > *wn->actrate && (wn->validrates & (1UL << p-wifi->rates)) != 0)
a = p;
}
wn->actrate = a;
}
(*wifi->transmit)(wifi, wn, b);
}
static Wnode*
nodelookup(Wifi *wifi, uchar *bssid, int new)
{
Wnode *wn, *nn;
if(memcmp(bssid, wifi->ether->bcast, Eaddrlen) == 0)
return nil;
if((wn = wifi->bss) != nil){
if(memcmp(wn->bssid, bssid, Eaddrlen) == 0)
return wn;
}
if((nn = wifi->node) == wn)
nn++;
for(wn = wifi->node; wn != &wifi->node[nelem(wifi->node)]; wn++){
if(wn == wifi->bss)
continue;
if(memcmp(wn->bssid, bssid, Eaddrlen) == 0)
return wn;
if((long)(wn->lastsend - nn->lastsend) < 0
|| (long)(wn->lastseen - nn->lastseen) < 0)
nn = wn;
}
if(!new)
return nil;
freewifikeys(wifi, nn);
memset(nn, 0, sizeof(Wnode));
memmove(nn->bssid, bssid, Eaddrlen);
return nn;
}
void
wifitxfail(Wifi *wifi, Block *b)
{
Wifipkt *w;
Wnode *wn;
if(b == nil)
return;
w = (Wifipkt*)b->rp;
wn = nodelookup(wifi, w->a1, 0);
if(wn == nil)
return;
wn->txerror++;
if(wn->actrate != nil && wn->minrate != wn->actrate){
uchar *a, *p;
for(a = wn->minrate, p = wifi->rates; *p; p++){
if(*p > *a && *p < *wn->actrate && (wn->validrates & (1UL << p-wifi->rates)) != 0)
a = p;
}
wn->actrate = a;
}
}
static uchar*
putrates(uchar *p, uchar *rates, ulong valid, ulong basic)
{
int n, i, j;
valid |= basic;
for(i = n = 0; i < 32 && rates[i] != 0; i++)
if(valid & (1UL<<i))
n++;
valid &= ~basic;
if(n > 0){
/* supported rates */
*p++ = 1;
*p++ = n;
for(i = j = 0; j < n; i++){
if(basic & (1UL<<i)){
*p++ = rates[i] | 0x80;
j++;
}
}
for(i = 0; j < n; i++){
if(valid & (1UL<<i)){
*p++ = rates[i] & 0x7f;
j++;
}
}
}
if(n > 8){
/* truncate supported rates element */
p -= n;
p[-1] = 8;
p += 8;
/* extended supported rates */
*p++ = 50;
*p++ = n;
for(i = j = 0; j < n; i++){
if(basic & (1UL<<i)){
*p++ = rates[i] | 0x80;
j++;
}
}
for(i = 0; j < n; i++){
if(valid & (1UL<<i)){
*p++ = rates[i] & 0x7f;
j++;
}
}
}
return p;
}
static void
wifiprobe(Wifi *wifi, Wnode *wn)
{
Wifipkt *w;
Block *b;
uchar *p;
int n;
n = strlen(wifi->essid);
if(n == 0){
/* no specific essid, just tell driver to tune channel */
(*wifi->transmit)(wifi, wn, nil);
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;
p = b->wp;
*p++ = 0; /* set */
*p++ = n;
memmove(p, wifi->essid, n);
p += n;
p = putrates(p, wifi->rates, wn->validrates, wn->basicrates);
*p++ = 3; /* ds parameter set */
*p++ = 1;
*p++ = wn->channel;
b->wp = p;
wifitx(wifi, wn, b);
}
static void
sendauth(Wifi *wifi, Wnode *bss)
{
Wifipkt *w;
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;
p = b->wp;
*p++ = 0; /* alg */
*p++ = 0;
*p++ = 1; /* seq */
*p++ = 0;
*p++ = 0; /* status */
*p++ = 0;
b->wp = p;
bss->aid = 0;
wifitx(wifi, bss, b);
}
static void
sendassoc(Wifi *wifi, Wnode *bss)
{
Wifipkt *w;
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->wp += WIFIHDRSIZE;
p = b->wp;
/* capinfo */
cap = 1; // ESS
cap |= (1<<5); // Short Preamble
cap |= (1<<10) & bss->cap; // Short Slot Time
*p++ = cap;
*p++ = cap>>8;
/* interval */
*p++ = 16;
*p++ = 16>>8;
n = strlen(bss->ssid);
*p++ = 0; /* SSID */
*p++ = n;
memmove(p, bss->ssid, n);
p += n;
p = putrates(p, wifi->rates, bss->validrates, bss->basicrates);
n = bss->rsnelen;
if(n > 0){
memmove(p, bss->rsne, n);
p += n;
}
b->wp = p;
wifitx(wifi, bss, b);
}
static void
setstatus(Wifi *wifi, Wnode *wn, char *new)
{
char *old;
old = wn->status;
wn->status = new;
if(wifi->debug && new != old)
print("#l%d: status %E: %.12ld %.12ld: %s -> %s (from pc=%#p)\n",
wifi->ether->ctlrno,
wn->bssid,
TK2MS(MACHP(0)->ticks), TK2MS(MACHP(0)->ticks - wn->lastsend),
old, new,
getcallerpc(&wifi));
}
static void
recvassoc(Wifi *wifi, Wnode *wn, uchar *d, int len)
{
uint s;
if(len < 2+2+2)
return;
d += 2; /* caps */
s = d[0] | d[1]<<8;
d += 2;
switch(s){
case 0x00:
wn->aid = d[0] | d[1]<<8;
if(wn->rsnelen > 0)
setstatus(wifi, wn, Sblocked);
else
setstatus(wifi, wn, Sassoc);
break;
default:
wn->aid = 0;
setstatus(wifi, wn, Sunassoc);
}
}
static void
recvbeacon(Wifi *wifi, Wnode *wn, uchar *d, int len)
{
static uchar wpa1oui[4] = { 0x00, 0x50, 0xf2, 0x01 };
uchar *e, *x, *p, t;
int rsnset;
len -= 8+2+2;
if(len < 0)
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->dtimcount = 0;
wn->dtimperiod = 1;
rsnset = 0;
for(e = d + len; d+2 <= e; d = x){
d += 2;
x = d + d[-1];
if(x > e)
break; /* truncated */
t = d[-2];
switch(t){
case 0: /* SSID */
len = 0;
while(len < Essidlen && d+len < x && d[len] != 0)
len++;
if(len == 0)
continue;
if(len != strlen(wn->ssid) || strncmp(wn->ssid, (char*)d, len) != 0){
strncpy(wn->ssid, (char*)d, len);
wn->ssid[len] = 0;
}
break;
case 1: /* supported rates */
case 50: /* extended rates */
if(wifi->rates == nil)
break;
while(d < x){
t = *d | 0x80;
for(p = wifi->rates; *p != 0; p++){
if(*p == t){
wn->validrates |= 1UL << p-wifi->rates;
if(*d & 0x80)
wn->basicrates |= 1UL << p-wifi->rates;
if(wn->minrate == nil || t < *wn->minrate)
wn->minrate = p;
if(wn->maxrate == nil || t > *wn->maxrate)
wn->maxrate = p;
break;
}
}
d++;
}
if(wn->actrate == nil)
wn->actrate = wn->maxrate;
break;
case 3: /* DSPARAMS */
if(d != x)
wn->channel = d[0];
break;
case 5:
if(x - d < 2)
break;
wn->dtimcount = d[0];
if(d[1] > 0)
wn->dtimperiod = d[1];
break;
case 221: /* vendor specific */
len = x - d;
if(rsnset || len < sizeof(wpa1oui) || memcmp(d, wpa1oui, sizeof(wpa1oui)) != 0)
break;
/* no break */
case 48: /* RSN information */
len = x - &d[-2];
memmove(wn->brsne, &d[-2], len);
wn->brsnelen = len;
rsnset = 1;
break;
}
}
}
static void
freewifikeys(Wifi *wifi, Wnode *wn)
{
int i;
wlock(&wifi->crypt);
for(i=0; i<nelem(wn->rxkey); i++){
secfree(wn->rxkey[i]);
wn->rxkey[i] = nil;
}
for(i=0; i<nelem(wn->txkey); i++){
secfree(wn->txkey[i]);
wn->txkey[i] = nil;
}
wunlock(&wifi->crypt);
}
static void
wifideauth(Wifi *wifi, Wnode *wn)
{
Ether *ether;
Netfile *f;
int i;
/* deassociate node, clear keys */
setstatus(wifi, wn, Sunauth);
freewifikeys(wifi, wn);
memset(&wifi->dmat, 0, sizeof(wifi->dmat));
wn->aid = 0;
if(wn == wifi->bss){
/* notify driver about node aid association */
(*wifi->transmit)(wifi, wn, nil);
/* notify aux/wpa with a zero length packet that we got deassociated from the ap */
ether = wifi->ether;
for(i=0; i<ether->nfile; i++){
f = ether->f[i];
if(f == nil || f->in == nil || f->inuse == 0 || f->type != 0x888e)
continue;
qflush(f->in);
qwrite(f->in, 0, 0);
}
qflush(ether->oq);
}
}
/* check if a node qualifies as our bss matching bssid and essid */
static int
goodbss(Wifi *wifi, Wnode *wn)
{
if(memcmp(wifi->bssid, wifi->ether->bcast, Eaddrlen) != 0){
if(memcmp(wifi->bssid, wn->bssid, Eaddrlen) != 0)
return 0; /* bssid doesnt match */
} else if(wifi->essid[0] == 0)
return 0; /* both bssid and essid unspecified */
if(wifi->essid[0] != 0 && strcmp(wifi->essid, wn->ssid) != 0)
return 0; /* essid doesnt match */
return 1;
}
static void
wifiproc(void *arg)
{
Wifi *wifi;
Wifipkt *w;
Wnode *wn;
Block *b;
b = nil;
wifi = arg;
while(waserror())
;
for(;;){
if(b != nil){
freeb(b);
b = nil;
continue;
}
if((b = qbread(wifi->iq, 100000)) == nil)
break;
w = (Wifipkt*)b->rp;
if(w->fc[1] & 0x40){
/* encrypted */
if((wn = nodelookup(wifi, w->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)
continue;
b->flag &= ~Btimestamp;
wifiiq(wifi, b);
b = nil;
}
continue;
}
/* management */
if((w->fc[0] & 0x0c) != 0x00)
continue;
switch(w->fc[0] & 0xf0){
case 0x50: /* probe response */
if(wifi->debug)
print("#l%d: got probe from %E\n", wifi->ether->ctlrno, w->a3);
/* no break */
case 0x80: /* beacon */
if((wn = nodelookup(wifi, w->a3, 1)) == nil)
continue;
wn->lastseen = MACHP(0)->ticks;
if(b->flag & Btimestamp)
wn->rs = getts(b->rp - 8);
b->rp += wifihdrlen(w);
recvbeacon(wifi, wn, b->rp, BLEN(b));
if(wifi->bss == nil
&& TK2MS(MACHP(0)->ticks - wn->lastsend) > 1000
&& goodbss(wifi, wn)){
setstatus(wifi, wn, Sconn);
sendauth(wifi, wn);
wifi->lastauth = wn->lastsend;
}
continue;
}
if(memcmp(w->a1, wifi->ether->ea, Eaddrlen))
continue;
if((wn = nodelookup(wifi, w->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);
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 */
if(wifi->debug)
print("#l%d: got auth from %E\n", wifi->ether->ctlrno, wn->bssid);
if(wn->brsnelen > 0 && wn->rsnelen == 0)
setstatus(wifi, wn, Sneedauth);
else
setstatus(wifi, wn, Sauth);
if(wifi->bss == nil && goodbss(wifi, wn)){
wifi->bss = wn;
if(wn->status == Sauth)
sendassoc(wifi, wn);
}
break;
case 0xc0: /* deauth */
if(wifi->debug)
print("#l%d: got deauth from %E\n", wifi->ether->ctlrno, wn->bssid);
wifideauth(wifi, wn);
break;
}
}
pexit("wifi in queue closed", 1);
}
static void
wifietheroq(Wifi *wifi, Block *b)
{
Etherpkt e;
Wifipkt h;
int hdrlen;
Wnode *wn;
SNAP *s;
if(BLEN(b) < ETHERHDRSIZE)
goto drop;
if((wn = wifi->bss) == nil)
goto drop;
dmatproxy(b, 1, wifi->ether->ea, &wifi->dmat);
memmove(&e, b->rp, ETHERHDRSIZE);
b->rp += ETHERHDRSIZE;
if(wn->status == Sblocked){
/* only pass EAPOL frames when port is blocked */
if((e.type[0]<<8 | e.type[1]) != 0x888e)
goto drop;
} else if(wn->status != Sassoc)
goto drop;
h.fc[0] = 0x08; /* data */
memmove(h.a1, wn->bssid, Eaddrlen);
if(memcmp(e.s, wifi->ether->ea, Eaddrlen) == 0) {
h.fc[1] = 0x01; /* STA->AP */
} else {
h.fc[1] = 0x03; /* AP->AP (WDS) */
memmove(h.a2, wifi->ether->ea, Eaddrlen);
}
memmove(dstaddr(&h), e.d, Eaddrlen);
memmove(srcaddr(&h), e.s, Eaddrlen);
hdrlen = wifihdrlen(&h);
b = padblock(b, hdrlen + SNAPHDRSIZE);
memmove(b->rp, &h, hdrlen);
s = (SNAP*)(b->rp + hdrlen);
s->dsap = s->ssap = 0xAA;
s->control = 0x03;
s->orgcode[0] = 0;
s->orgcode[1] = 0;
s->orgcode[2] = 0;
memmove(s->type, e.type, 2);
wifitx(wifi, wn, b);
return;
drop:
freeb(b);
}
static void
wifoproc(void *arg)
{
Ether *ether;
Wifi *wifi;
Block *b;
wifi = arg;
ether = wifi->ether;
while(waserror())
;
while((b = qbread(ether->oq, 1000000)) != nil)
wifietheroq(wifi, b);
pexit("ether out queue closed", 1);
}
static void
wifsproc(void *arg)
{
Ether *ether;
Wifi *wifi;
Wnode wnscan;
Wnode *wn;
ulong now, tmout;
uchar *rate;
wifi = arg;
ether = wifi->ether;
wn = &wnscan;
memset(wn, 0, sizeof(*wn));
memmove(wn->bssid, ether->bcast, Eaddrlen);
while(waserror())
;
Scan:
/* scan for access point */
while(wifi->bss == nil){
ether->link = 0;
wnscan.channel = 1 + ((wnscan.channel+4) % 13);
wifiprobe(wifi, &wnscan);
do {
tsleep(&up->sleep, return0, 0, 200);
now = MACHP(0)->ticks;
} while(TK2MS(now-wifi->lastauth) < 1000);
}
/* maintain access point */
tmout = 0;
while((wn = wifi->bss) != nil){
ether->link = (wn->status == Sassoc) || (wn->status == Sblocked);
if(ether->link && (rate = wn->actrate) != nil)
ether->mbps = ((*rate & 0x7f)+3)/4;
now = MACHP(0)->ticks;
if(wn->status != Sneedauth && TK2SEC(now - wn->lastseen) > 20 || goodbss(wifi, wn) == 0){
wifideauth(wifi, wn);
wifi->bss = nil;
break;
}
if(TK2MS(now - wn->lastsend) > 1000){
if((wn->status == Sauth || wn->status == Sblocked) && (++tmout & 7) == 0)
wifideauth(wifi, wn); /* stuck in auth, start over */
if(wn->status == Sconn || wn->status == Sunauth)
sendauth(wifi, wn);
if(wn->status == Sauth){
sendassoc(wifi, wn);
}
}
tsleep(&up->sleep, return0, 0, 500);
}
goto Scan;
}
Wifi*
wifiattach(Ether *ether, void (*transmit)(Wifi*, Wnode*, Block*))
{
char name[32];
Wifi *wifi;
wifi = malloc(sizeof(Wifi));
if(wifi == nil)
error(Enomem);
wifi->iq = qopen(ether->limit, 0, 0, 0);
if(wifi->iq == nil){
free(wifi);
error(Enomem);
}
wifi->ether = ether;
wifi->transmit = transmit;
wifi->rates = basicrates;
wifi->essid[0] = 0;
memmove(wifi->bssid, ether->bcast, Eaddrlen);
wifi->lastauth = MACHP(0)->ticks;
snprint(name, sizeof(name), "#l%dwifi", ether->ctlrno);
kproc(name, wifiproc, wifi);
snprint(name, sizeof(name), "#l%dwifo", ether->ctlrno);
kproc(name, wifoproc, wifi);
snprint(name, sizeof(name), "#l%dwifs", ether->ctlrno);
kproc(name, wifsproc, wifi);
return wifi;
}
static char *ciphers[] = {
[0] "clear",
[TKIP] "tkip",
[CCMP] "ccmp",
};
static Wkey*
parsekey(char *s)
{
static char Ebadkey[] = "bad key";
uchar key[32];
int len, cipher;
char *e;
Wkey *k;
for(cipher=0; cipher<nelem(ciphers); cipher++){
if(strncmp(s, ciphers[cipher], len = strlen(ciphers[cipher])) == 0){
if(cipher == 0) /* clear */
return nil;
if(s[len] == ':'){
s += len+1;
break;
}
}
}
if(cipher >= nelem(ciphers))
error(Ebadkey);
if((e = strchr(s, '@')) == nil)
e = strchr(s, 0);
len = dec16(key, sizeof(key), s, e - s);
switch(cipher){
case TKIP:
if(len != 32)
error(Ebadkey);
k = secalloc(sizeof(Wkey) + len);
memmove(k->key, key, len);
break;
case CCMP:
if(len != 16)
error(Ebadkey);
k = secalloc(sizeof(Wkey) + sizeof(AESstate));
setupAESstate((AESstate*)k->key, key, len, nil);
break;
default:
error(Ebadkey);
return nil;
}
memset(key, 0, sizeof(key));
if(*e++ == '@')
k->tsc = strtoull(e, nil, 16);
k->len = len;
k->cipher = cipher;
return k;
}
void
wificfg(Wifi *wifi, char *opt)
{
char *p, buf[64];
int n;
if(strncmp(opt, "debug=", 6))
if(strncmp(opt, "essid=", 6))
if(strncmp(opt, "bssid=", 6))
return;
if((p = strchr(opt, '=')) == nil)
return;
if(waserror())
return;
n = snprint(buf, sizeof(buf), "%.*s %q", utfnlen(opt, p - opt), opt, p+1);
wifictl(wifi, buf, n);
poperror();
}
enum {
CMdebug,
CMessid,
CMauth,
CMbssid,
CMrxkey0,
CMrxkey1,
CMrxkey2,
CMrxkey3,
CMrxkey4,
CMtxkey0,
};
static Cmdtab wifictlmsg[] =
{
CMdebug, "debug", 0,
CMessid, "essid", 0,
CMauth, "auth", 0,
CMbssid, "bssid", 0,
CMrxkey0, "rxkey0", 0, /* group keys */
CMrxkey1, "rxkey1", 0,
CMrxkey2, "rxkey2", 0,
CMrxkey3, "rxkey3", 0,
CMrxkey4, "rxkey", 0, /* peerwise keys */
CMtxkey0, "txkey", 0,
CMtxkey0, "txkey0", 0,
};
long
wifictl(Wifi *wifi, void *buf, long n)
{
uchar addr[Eaddrlen];
Cmdbuf *cb;
Cmdtab *ct;
Wnode *wn;
Wkey *k, **kk;
cb = nil;
if(waserror()){
free(cb);
nexterror();
}
if(wifi->debug)
print("#l%d: wifictl: %.*s\n", wifi->ether->ctlrno, utfnlen(buf, n), buf);
memmove(addr, wifi->ether->bcast, Eaddrlen);
wn = wifi->bss;
cb = parsecmd(buf, n);
ct = lookupcmd(cb, wifictlmsg, nelem(wifictlmsg));
if(ct->index >= CMauth){
if(cb->nf > 1 && (ct->index == CMbssid || ct->index >= CMrxkey0)){
if(parseether(addr, cb->f[1]) == 0){
cb->f++;
cb->nf--;
wn = nodelookup(wifi, addr, 0);
}
}
if(wn == nil && ct->index != CMbssid)
error("missing node");
}
switch(ct->index){
case CMdebug:
if(cb->f[1] != nil)
wifi->debug = atoi(cb->f[1]);
else
wifi->debug ^= 1;
print("#l%d: debug: %d\n", wifi->ether->ctlrno, wifi->debug);
break;
case CMessid:
if(cb->f[1] != nil)
strncpy(wifi->essid, cb->f[1], Essidlen);
else
wifi->essid[0] = 0;
Findbss:
wn = wifi->bss;
if(wn != nil){
if(goodbss(wifi, wn))
break;
wifideauth(wifi, wn);
}
wifi->bss = nil;
if(wifi->essid[0] == 0 && memcmp(wifi->bssid, wifi->ether->bcast, Eaddrlen) == 0)
break;
for(wn = wifi->node; wn != &wifi->node[nelem(wifi->node)]; wn++)
if(goodbss(wifi, wn)){
setstatus(wifi, wn, Sconn);
sendauth(wifi, wn);
}
break;
case CMbssid:
memmove(wifi->bssid, addr, Eaddrlen);
goto Findbss;
case CMauth:
freewifikeys(wifi, wn);
if(cb->f[1] == nil)
wn->rsnelen = 0;
else
wn->rsnelen = dec16(wn->rsne, sizeof(wn->rsne), cb->f[1], strlen(cb->f[1]));
if(wn->aid == 0){
setstatus(wifi, wn, Sconn);
sendauth(wifi, wn);
} else {
setstatus(wifi, wn, Sauth);
sendassoc(wifi, wn);
}
break;
case CMrxkey0: case CMrxkey1: case CMrxkey2: case CMrxkey3: case CMrxkey4:
case CMtxkey0:
if(cb->f[1] == nil)
error(Ebadarg);
k = parsekey(cb->f[1]);
memset(cb->f[1], 0, strlen(cb->f[1]));
if(ct->index < CMtxkey0)
kk = &wn->rxkey[ct->index - CMrxkey0];
else
kk = &wn->txkey[ct->index - CMtxkey0];
wlock(&wifi->crypt);
secfree(*kk);
*kk = k;
wunlock(&wifi->crypt);
if(ct->index >= CMtxkey0 && wn->status == Sblocked)
setstatus(wifi, wn, Sassoc);
break;
}
poperror();
free(cb);
return n;
}
long
wifistat(Wifi *wifi, void *buf, long n, ulong off)
{
static uchar zeros[Eaddrlen];
char essid[Essidlen+1];
char *s, *p, *e;
Wnode *wn;
Wkey *k;
long now;
int i;
p = s = smalloc(4096);
e = s + 4096;
wn = wifi->bss;
if(wn != nil){
strncpy(essid, wn->ssid, Essidlen);
essid[Essidlen] = 0;
p = seprint(p, e, "essid: %s\n", essid);
p = seprint(p, e, "bssid: %E\n", wn->bssid);
p = seprint(p, e, "status: %s\n", wn->status);
p = seprint(p, e, "channel: %.2d\n", wn->channel);
/* only print key ciphers and key length */
rlock(&wifi->crypt);
for(i = 0; i<nelem(wn->rxkey); i++){
if((k = wn->rxkey[i]) != nil)
p = seprint(p, e, "rxkey%d: %s:[%d]\n", i,
ciphers[k->cipher], k->len);
}
for(i = 0; i<nelem(wn->txkey); i++){
if((k = wn->txkey[i]) != nil)
p = seprint(p, e, "txkey%d: %s:[%d]\n", i,
ciphers[k->cipher], k->len);
}
runlock(&wifi->crypt);
if(wn->brsnelen > 0){
p = seprint(p, e, "brsne: ");
for(i=0; i<wn->brsnelen; i++)
p = seprint(p, e, "%.2X", wn->brsne[i]);
p = seprint(p, e, "\n");
}
} else {
p = seprint(p, e, "essid: %s\n", wifi->essid);
p = seprint(p, e, "bssid: %E\n", wifi->bssid);
}
now = MACHP(0)->ticks;
for(wn = wifi->node; wn != &wifi->node[nelem(wifi->node)]; wn++){
if(wn->lastseen == 0)
continue;
strncpy(essid, wn->ssid, Essidlen);
essid[Essidlen] = 0;
p = seprint(p, e, "node: %E %.4x %-11ld %.2d %s\n",
wn->bssid, wn->cap, TK2MS(now - wn->lastseen), wn->channel, essid);
}
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);
}
/*
* Dynamic Mac Address Translation (DMAT)
*
* Wifi does not allow spoofing of the source mac which breaks
* bridging. To solve this we proxy mac addresses, maintaining
* a translation table from ip address to destination mac address.
* Upstream ARP and NDP packets get ther source mac address changed
* to proxy and a translation entry is added with the original mac
* for downstream translation. The proxy does not appear in the
* table.
*/
#include "../ip/ip.h"
#include "../ip/ipv6.h"
static void
dmatproxy(Block *bp, int upstream, uchar proxy[Eaddrlen], DMAT *t)
{
static uchar arp4[] = {
0x00, 0x01,
0x08, 0x00,
0x06, 0x04,
0x00,
};
uchar ip[IPaddrlen], mac[Eaddrlen], *targ, *end, *a, *o;
ulong csum, c, h;
Etherpkt *pkt;
int proto, i;
DMTE *te;
end = bp->wp;
pkt = (Etherpkt*)bp->rp;
a = pkt->data;
if(a >= end)
return;
if(upstream)
memmove(pkt->s, proxy, Eaddrlen);
else if(t->map == 0 || (pkt->d[0]&1) != 0 || memcmp(pkt->d, proxy, Eaddrlen) != 0)
return;
targ = nil;
switch(pkt->type[0]<<8 | pkt->type[1]){
default:
return;
case ETIP4:
case ETIP6:
switch(a[0]&0xF0){
default:
return;
case IP_VER4:
if(a+IP4HDR > end || (a[0]&15) < IP_HLEN4)
return;
v4tov6(ip, a+12+4*(upstream==0));
proto = a[9];
a += (a[0]&15)*4;
break;
case IP_VER6:
if(a+IP6HDR > end)
return;
memmove(ip, a+8+16*(upstream==0), 16);
proto = a[6];
a += IP6HDR;
break;
}
if(!upstream)
break;
switch(proto){
case ICMPv6:
if(a+8 > end)
return;
switch(a[0]){
default:
return;
case 133: /* Router Solicitation */
o = a+8;
break;
case 134: /* Router Advertisement */
o = a+8+8;
break;
case 136: /* Neighbor Advertisement */
targ = a+8;
/* wet floor */
case 135: /* Neighbor Solicitation */
o = a+8+16;
break;
case 137: /* Redirect */
o = a+8+16+16;
break;
}
memset(mac, 0xFF, Eaddrlen);
csum = (a[2]<<8 | a[3])^0xFFFF;
while(o+8 <= end && o[1] != 0){
switch(o[0]){
case SRC_LLADDR:
case TARGET_LLADDR:
for(i=0; i<Eaddrlen; i += 2)
csum += (o[2+i]<<8 | o[3+i])^0xFFFF;
memmove(mac, o+2, Eaddrlen);
memmove(o+2, proxy, Eaddrlen);
for(i=0; i<Eaddrlen; i += 2)
csum += (o[2+i]<<8 | o[3+i]);
break;
}
o += o[1]*8;
}
while((c = csum >> 16) != 0)
csum = (csum & 0xFFFF) + c;
csum ^= 0xFFFF;
a[2] = csum>>8;
a[3] = csum;
break;
case UDP: /* for BOOTP */
if(a+42 > end
|| (a[0]<<8 | a[1]) != 68
|| (a[2]<<8 | a[3]) != 67
|| a[8] != 1
|| a[9] != 1
|| a[10] != Eaddrlen
|| (a[18]&0x80) != 0
|| memcmp(a+36, proxy, Eaddrlen) == 0)
return;
csum = (a[6]<<8 | a[7])^0xFFFF;
/* set the broadcast flag so response reaches us */
csum += (a[18]<<8)^0xFFFF;
a[18] |= 0x80;
csum += (a[18]<<8);
while((c = csum >> 16) != 0)
csum = (csum & 0xFFFF) + c;
csum ^= 0xFFFF;
a[6] = csum>>8;
a[7] = csum;
default:
return;
}
break;
case ETARP:
if(a+26 > end || memcmp(a, arp4, sizeof(arp4)) != 0 || (a[7] != 1 && a[7] != 2))
return;
v4tov6(ip, a+14+10*(upstream==0));
if(upstream){
memmove(mac, a+8, Eaddrlen);
memmove(a+8, proxy, Eaddrlen);
}
break;
}
Again:
h = ( (ip[IPaddrlen-1] ^ proxy[2])<<24 |
(ip[IPaddrlen-2] ^ proxy[3])<<16 |
(ip[IPaddrlen-3] ^ proxy[4])<<8 |
(ip[IPaddrlen-4] ^ proxy[5]) ) % nelem(t->tab);
te = &t->tab[h];
h &= 63;
if(upstream){
if((mac[0]&1) != 0 || memcmp(mac, proxy, Eaddrlen) == 0)
return;
for(i=0; te->valid && i<nelem(t->tab); i++){
if(memcmp(te->ip, ip, IPaddrlen) == 0)
break;
if(++te >= &t->tab[nelem(t->tab)])
te = t->tab;
}
memmove(te->mac, mac, Eaddrlen);
memmove(te->ip, ip, IPaddrlen);
te->valid = 1;
t->map |= 1ULL<<h;
if(targ != nil){
memmove(ip, targ, IPaddrlen);
targ = nil;
goto Again;
}
} else {
if((t->map>>h & 1) == 0)
return;
for(i=0; te->valid && i<nelem(t->tab); i++){
if(memcmp(te->ip, ip, IPaddrlen) == 0){
memmove(pkt->d, te->mac, Eaddrlen);
return;
}
if(++te >= &t->tab[nelem(t->tab)])
te = t->tab;
}
}
}