wifi: decode AP->AP (WDS / bridge) data frames.

2 files changed, 35 insertions, 20 deletions

diff --git a/sys/src/9/pc/wifi.c b/sys/src/9/pc/wifi.c
index 39959d9f8..da2175fd7 100644
--- a/sys/src/9/pc/wifi.c
+++ b/sys/src/9/pc/wifi.c
@@ -43,18 +43,18 @@ static Block* wifiencrypt(Wifi *, Wnode *, Block *);
 static uchar*
 srcaddr(Wifipkt *w)
 {
-	if((w->fc[1] & 3) == 0x02)
-		return w->a3;
-	else
+	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] & 3) == 0x01)
+	if((w->fc[1] & 0x01) != 0)
 		return w->a3;
-	else
-		return w->a1;
+	return w->a1;
 }
 
 int
@@ -69,6 +69,8 @@ wifihdrlen(Wifipkt *w)
 			if(w->fc[1] & 0x80)
 				n += 4;
 		}
+	if((w->fc[1] & 3) == 0x03)
+		n += Eaddrlen;
 	return n;
 }
 
@@ -78,10 +80,14 @@ wifiiq(Wifi *wifi, Block *b)
 	SNAP s;
 	Wifipkt h, *w;
 	Etherpkt *e;
+	int hdrlen;
 
 	if(BLEN(b) < WIFIHDRSIZE)
 		goto drop;
 	w = (Wifipkt*)b->rp;
+	hdrlen = wifihdrlen(w);
+	if(BLEN(b) < hdrlen)
+		goto drop;
 	if(w->fc[1] & 0x40){
 		/* encrypted */
 		qpass(wifi->iq, b);
@@ -96,9 +102,7 @@ wifiiq(Wifi *wifi, Block *b)
 	case 0x04:	/* control */
 		break;
 	case 0x08:	/* data */
-		if((w->fc[1] & 3) == 0x03)	/* AP->AP */
-			break;
-		b->rp += wifihdrlen(w);
+		b->rp += hdrlen;
 		switch(w->fc[0] & 0xf0){
 		default:
 			goto drop;
@@ -538,7 +542,8 @@ static void
 wifietheroq(Wifi *wifi, Block *b)
 {
 	Etherpkt e;
-	Wifipkt *w;
+	Wifipkt h;
+	int hdrlen;
 	Wnode *wn;
 	SNAP *s;
 
@@ -557,16 +562,21 @@ wifietheroq(Wifi *wifi, Block *b)
 	} else if(wn->status != Sassoc)
 		goto drop;
 
-	b = padblock(b, WIFIHDRSIZE + SNAPHDRSIZE);
-
-	w = (Wifipkt*)b->rp;
-	w->fc[0] = 0x08;	/* data */
-	w->fc[1] = 0x01;	/* STA->AP */
-	memmove(w->a1, wn->bssid, Eaddrlen);
-	memmove(w->a2, e.s, Eaddrlen);
-	memmove(w->a3, e.d, Eaddrlen);
+	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);
 
-	s = (SNAP*)(b->rp + WIFIHDRSIZE);
+	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;
@@ -998,7 +1008,7 @@ wifidecrypt(Wifi *, Wnode *wn, Block *b)
 	kid = b->rp[3]>>6;
 	if((b->rp[3] & 0x20) == 0)
 		goto drop;
-	if((dstaddr(w)[0] & 1) == 0)
+	if((w->a1[0] & 1) == 0)
 		kid = 4;	/* use peerwise key for non-unicast */
 
 	k = &wn->rxkey[kid];
@@ -1488,6 +1498,10 @@ setupCCMP(Wkey *k, Wifipkt *w, uvlong tsc, uchar nonce[13], uchar auth[32], AESs
 	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;
 }
diff --git a/sys/src/9/pc/wifi.h b/sys/src/9/pc/wifi.h
index 65c249605..bc451cf3d 100644
--- a/sys/src/9/pc/wifi.h
+++ b/sys/src/9/pc/wifi.h
@@ -74,6 +74,7 @@ struct Wifipkt
 	uchar	a2[Eaddrlen];
 	uchar	a3[Eaddrlen];
 	uchar	seq[2];
+	uchar	a4[Eaddrlen];
 };
 
 Wifi *wifiattach(Ether *ether, void (*transmit)(Wifi*, Wnode*, Block*));