void
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);
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->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);
etheriq(wifi->ether, b, 1);
return;
}
drop:
freeb(b);
}
static void
loopbackbwrite(Ipifc *ifc, Block *bp, int, uchar*)
{
LB *lb;
lb = ifc->arg;
if(qpass(lb->q, bp) < 0)
ifc->outerr++;
ifc->out++;
}
static void
loopbackbwrite(struct Ipifc *ifc, struct block *bp, int unused_int,
uint8_t * unused_uint8_p_t)
{
LB *lb;
lb = ifc->arg;
if (qpass(lb->q, bp) < 0)
ifc->outerr++;
ifc->out++;
}
/*
* User level routing. Ip packets we don't know what to do with
* come here.
*/
void useriprouter(struct Fs *f, struct Ipifc *ifc, struct block *bp)
{
qlock(&(&f->iprouter)->qlock);
if (f->iprouter.q != NULL) {
bp = padblock(bp, IPaddrlen);
if (bp == NULL)
return;
ipmove(bp->rp, ifc->lifc->local);
qpass(f->iprouter.q, bp);
} else
freeb(bp);
qunlock(&(&f->iprouter)->qlock);
}
/* send the base station scan info to any readers */
static void
w_scaninfo(Ether* ether, Ctlr *ctlr, int len)
{
int i, j;
Netfile **ep, *f, **fp;
Block *bp;
WScan *wsp;
ushort *scanbuf;
scanbuf = malloc(len*2);
if(scanbuf == nil)
return;
for (i = 0; i < len ; i++)
scanbuf[i] = csr_ins(ctlr, WR_Data1);
/* calculate number of samples */
len /= 25;
if(len == 0)
goto out;
i = ether->scan;
ep = ðer->f[Ntypes];
for(fp = ether->f; fp < ep && i > 0; fp++){
f = *fp;
if(f == nil || f->scan == 0)
continue;
bp = iallocb(100*len);
if(bp == nil)
break;
for(j = 0; j < len; j++){
wsp = (WScan*)(&scanbuf[j*25]);
if(wsp->ssid_len > 32)
wsp->ssid_len = 32;
bp->wp = (uchar*)seprint((char*)bp->wp, (char*)bp->lim,
"ssid=%.*s;bssid=%E;signal=%d;noise=%d;chan=%d%s\n",
wsp->ssid_len, wsp->ssid, wsp->bssid, wsp->signal,
wsp->noise, wsp->chan, (wsp->capinfo&(1<<4))?";wep":"");
}
qpass(f->in, bp);
i--;
}
out:
free(scanbuf);
}
void udpiput(struct Proto *udp, struct Ipifc *ifc, struct block *bp)
{
int len;
Udp4hdr *uh4;
Udp6hdr *uh6;
struct conv *c;
Udpcb *ucb;
uint8_t raddr[IPaddrlen], laddr[IPaddrlen];
uint16_t rport, lport;
Udppriv *upriv;
struct Fs *f;
int version;
int ottl, oviclfl, olen;
uint8_t *p;
upriv = udp->priv;
f = udp->f;
upriv->ustats.udpInDatagrams++;
uh4 = (Udp4hdr *) (bp->rp);
version = ((uh4->vihl & 0xF0) == IP_VER6) ? V6 : V4;
/*
* Put back pseudo header for checksum
* (remember old values for icmpnoconv())
*/
switch (version) {
case V4:
ottl = uh4->Unused;
uh4->Unused = 0;
len = nhgets(uh4->udplen);
olen = nhgets(uh4->udpplen);
hnputs(uh4->udpplen, len);
v4tov6(raddr, uh4->udpsrc);
v4tov6(laddr, uh4->udpdst);
lport = nhgets(uh4->udpdport);
rport = nhgets(uh4->udpsport);
if (!(bp->flag & Budpck) &&
(uh4->udpcksum[0] || uh4->udpcksum[1]) &&
ptclcsum(bp, UDP4_PHDR_OFF, len + UDP4_PHDR_SZ)) {
upriv->ustats.udpInErrors++;
netlog(f, Logudp, "udp: checksum error %I\n",
raddr);
printd("udp: checksum error %I\n", raddr);
freeblist(bp);
return;
}
uh4->Unused = ottl;
hnputs(uh4->udpplen, olen);
break;
case V6:
uh6 = (Udp6hdr *) (bp->rp);
len = nhgets(uh6->udplen);
oviclfl = nhgetl(uh6->viclfl);
olen = nhgets(uh6->len);
ottl = uh6->hoplimit;
ipmove(raddr, uh6->udpsrc);
ipmove(laddr, uh6->udpdst);
lport = nhgets(uh6->udpdport);
rport = nhgets(uh6->udpsport);
memset(uh6, 0, 8);
hnputl(uh6->viclfl, len);
uh6->hoplimit = IP_UDPPROTO;
if (ptclcsum(bp, UDP6_PHDR_OFF, len + UDP6_PHDR_SZ)) {
upriv->ustats.udpInErrors++;
netlog(f, Logudp, "udp: checksum error %I\n", raddr);
printd("udp: checksum error %I\n", raddr);
freeblist(bp);
return;
}
hnputl(uh6->viclfl, oviclfl);
hnputs(uh6->len, olen);
uh6->nextheader = IP_UDPPROTO;
uh6->hoplimit = ottl;
break;
default:
panic("udpiput: version %d", version);
return; /* to avoid a warning */
}
qlock(&udp->qlock);
c = iphtlook(&upriv->ht, raddr, rport, laddr, lport);
if (c == NULL) {
/* no converstation found */
upriv->ustats.udpNoPorts++;
qunlock(&udp->qlock);
netlog(f, Logudp, "udp: no conv %I!%d -> %I!%d\n", raddr, rport,
laddr, lport);
switch (version) {
case V4:
icmpnoconv(f, bp);
break;
case V6:
icmphostunr(f, ifc, bp, icmp6_port_unreach, 0);
break;
default:
panic("udpiput2: version %d", version);
}
freeblist(bp);
return;
}
ucb = (Udpcb *) c->ptcl;
if (c->state == Announced) {
if (ucb->headers == 0) {
/* create a new conversation */
if (ipforme(f, laddr) != Runi) {
switch (version) {
case V4:
v4tov6(laddr, ifc->lifc->local);
break;
case V6:
ipmove(laddr, ifc->lifc->local);
break;
default:
panic("udpiput3: version %d", version);
}
}
c = Fsnewcall(c, raddr, rport, laddr, lport, version);
if (c == NULL) {
qunlock(&udp->qlock);
freeblist(bp);
return;
}
iphtadd(&upriv->ht, c);
ucb = (Udpcb *) c->ptcl;
}
}
qlock(&c->qlock);
qunlock(&udp->qlock);
/*
* Trim the packet down to data size
*/
len -= UDP_UDPHDR_SZ;
switch (version) {
case V4:
bp = trimblock(bp, UDP4_IPHDR_SZ + UDP_UDPHDR_SZ, len);
break;
case V6:
bp = trimblock(bp, UDP6_IPHDR_SZ + UDP_UDPHDR_SZ, len);
break;
default:
bp = NULL;
panic("udpiput4: version %d", version);
}
if (bp == NULL) {
qunlock(&c->qlock);
netlog(f, Logudp, "udp: len err %I.%d -> %I.%d\n", raddr, rport,
laddr, lport);
upriv->lenerr++;
return;
}
netlog(f, Logudpmsg, "udp: %I.%d -> %I.%d l %d\n", raddr, rport,
laddr, lport, len);
switch (ucb->headers) {
case 7:
/* pass the src address */
bp = padblock(bp, UDP_USEAD7);
p = bp->rp;
ipmove(p, raddr);
p += IPaddrlen;
ipmove(p, laddr);
p += IPaddrlen;
ipmove(p, ifc->lifc->local);
p += IPaddrlen;
hnputs(p, rport);
p += 2;
hnputs(p, lport);
break;
case 6:
/* pass the src address */
bp = padblock(bp, UDP_USEAD6);
p = bp->rp;
ipmove(p, raddr);
p += IPaddrlen;
ipmove(p, ipforme(f, laddr) == Runi ? laddr : ifc->lifc->local);
p += IPaddrlen;
hnputs(p, rport);
p += 2;
hnputs(p, lport);
break;
}
if (bp->next)
bp = concatblock(bp);
if (qfull(c->rq)) {
qunlock(&c->qlock);
netlog(f, Logudp, "udp: qfull %I.%d -> %I.%d\n", raddr, rport,
laddr, lport);
freeblist(bp);
return;
}
qpass(c->rq, bp);
qunlock(&c->qlock);
}
/*
* move blocks between queues if they are ready.
* schedule an interrupt for the next interesting time.
*
* must be called with the link ilocked.
*/
static void
pushlink(Link *link, vlong now)
{
Block *bp;
vlong tout, tin;
/*
* put another block in the link queue
*/
ilock(link);
if(link->iq == nil || link->oq == nil){
iunlock(link);
return;
}
timerdel(&link->ci);
/*
* put more blocks into the xmit queue
* use the time the last packet was supposed to go out
* as the start time for the next packet, rather than
* the current time. this more closely models a network
* device which can queue multiple output packets.
*/
tout = link->tout;
if(!tout)
tout = now;
while(tout <= now){
bp = qget(link->oq);
if(bp == nil){
tout = 0;
break;
}
/*
* can't send the packet before it gets queued
*/
tin = gtime(bp->rp);
if(tin > tout)
tout = tin;
tout = tout + (BLEN(bp) - Tmsize) * link->delayn;
/*
* drop packets
*/
if(link->droprate && nrand(link->droprate) == 0)
link->drops++;
else{
ptime(bp->rp, tout + link->delay0ns);
if(link->tq == nil)
link->tq = bp;
else
link->tqtail->next = bp;
link->tqtail = bp;
}
}
/*
* record the next time a packet can be sent,
* but don't schedule an interrupt if none is waiting
*/
link->tout = tout;
if(!qcanread(link->oq))
tout = 0;
/*
* put more blocks into the receive queue
*/
tin = 0;
while(bp = link->tq){
tin = gtime(bp->rp);
if(tin > now)
break;
bp->rp += Tmsize;
link->tq = bp->next;
bp->next = nil;
if(!link->indrop)
qpassnolim(link->iq, bp);
else if(qpass(link->iq, bp) < 0)
link->soverflows++;
tin = 0;
}
if(bp == nil && qisclosed(link->oq) && !qcanread(link->oq) && !qisclosed(link->iq))
qhangup(link->iq, nil);
link->tin = tin;
if(!tin || tin > tout && tout)
tin = tout;
link->ci.ns = tin - now;
if(tin){
if(tin < now)
panic("loopback unfinished business");
timeradd(&link->ci);
}
iunlock(link);
}
/*
* decapsulate IP packet from IP/ESP packet in bp and
* pass the result up the spi's Conv's read queue.
*/
void
espiput(Proto *esp, Ipifc *ipifc, Block *bp)
{
Mach *m = machp();
int payload, nexthdr;
uint8_t *auth, *espspi;
Conv *c;
Espcb *ecb;
Esptail *et;
Fs *f;
Userhdr *uh;
Versdep vers;
f = esp->f;
getverslens(pktipvers(f, &bp), &vers);
bp = pullupblock(bp, vers.hdrlen + Esptaillen);
if(bp == nil) {
netlog(f, Logesp, "esp: short packet\n");
return;
}
getpktspiaddrs(bp->rp, &vers);
qlock(esp);
/* Look for a conversation structure for this port */
c = convlookup(esp, vers.spi);
if(c == nil) {
qunlock(esp);
netlog(f, Logesp, "esp: no conv %I -> %I!%lud\n", vers.raddr,
vers.laddr, vers.spi);
icmpnoconv(f, bp);
freeblist(bp);
return;
}
qlock(c);
qunlock(esp);
ecb = c->ptcl;
/* too hard to do decryption/authentication on block lists */
if(bp->next)
bp = concatblock(bp);
if(BLEN(bp) < vers.hdrlen + ecb->espivlen + Esptaillen + ecb->ahlen) {
qunlock(c);
netlog(f, Logesp, "esp: short block %I -> %I!%lud\n", vers.raddr,
vers.laddr, vers.spi);
freeb(bp);
return;
}
auth = bp->wp - ecb->ahlen;
espspi = vers.version == V4? ((Esp4hdr*)bp->rp)->espspi:
((Esp6hdr*)bp->rp)->espspi;
/* compute secure hash and authenticate */
if(!ecb->auth(ecb, espspi, auth - espspi, auth)) {
qunlock(c);
print("esp: bad auth %I -> %I!%ld\n", vers.raddr, vers.laddr, vers.spi);
netlog(f, Logesp, "esp: bad auth %I -> %I!%lud\n", vers.raddr,
vers.laddr, vers.spi);
freeb(bp);
return;
}
payload = BLEN(bp) - vers.hdrlen - ecb->ahlen;
if(payload <= 0 || payload % 4 != 0 || payload % ecb->espblklen != 0) {
qunlock(c);
netlog(f, Logesp, "esp: bad length %I -> %I!%lud payload=%d BLEN=%lud\n",
vers.raddr, vers.laddr, vers.spi, payload, BLEN(bp));
freeb(bp);
return;
}
/* decrypt payload */
if(!ecb->cipher(ecb, bp->rp + vers.hdrlen, payload)) {
qunlock(c);
print("esp: cipher failed %I -> %I!%ld: %s\n", vers.raddr, vers.laddr, vers.spi, m->externup->errstr);
netlog(f, Logesp, "esp: cipher failed %I -> %I!%lud: %s\n",
vers.raddr, vers.laddr, vers.spi, m->externup->errstr);
freeb(bp);
return;
}
payload -= Esptaillen;
et = (Esptail*)(bp->rp + vers.hdrlen + payload);
payload -= et->pad + ecb->espivlen;
nexthdr = et->nexthdr;
if(payload <= 0) {
qunlock(c);
netlog(f, Logesp, "esp: short packet after decrypt %I -> %I!%lud\n",
vers.raddr, vers.laddr, vers.spi);
freeb(bp);
return;
}
/* trim packet */
bp->rp += vers.hdrlen + ecb->espivlen; /* toss original IP & ESP hdrs */
bp->wp = bp->rp + payload;
if(ecb->header) {
/* assume Userhdrlen < Esp4hdrlen < Esp6hdrlen */
bp->rp -= Userhdrlen;
uh = (Userhdr*)bp->rp;
memset(uh, 0, Userhdrlen);
uh->nexthdr = nexthdr;
}
/* ingress filtering here? */
if(qfull(c->rq)){
netlog(f, Logesp, "esp: qfull %I -> %I.%uld\n", vers.raddr,
vers.laddr, vers.spi);
freeblist(bp);
}else {
// print("esp: pass up: %uld\n", BLEN(bp));
qpass(c->rq, bp); /* pass packet up the read queue */
}
qunlock(c);
}
static void
greiput(Proto *gre, Ipifc*, Block *bp)
{
int len;
GREhdr *ghp;
Conv *c, **p;
ushort eproto;
uchar raddr[IPaddrlen];
GREpriv *gpriv;
gpriv = gre->priv;
ghp = (GREhdr*)(bp->rp);
v4tov6(raddr, ghp->src);
eproto = nhgets(ghp->eproto);
qlock(gre);
/* Look for a conversation structure for this port and address */
c = nil;
for(p = gre->conv; *p; p++) {
c = *p;
if(c->inuse == 0)
continue;
if(c->rport == eproto &&
(gpriv->raw || ipcmp(c->raddr, raddr) == 0))
break;
}
if(*p == nil) {
qunlock(gre);
freeblist(bp);
return;
}
qunlock(gre);
/*
* Trim the packet down to data size
*/
len = nhgets(ghp->len) - GRE_IPONLY;
if(len < GRE_IPPLUSGRE){
freeblist(bp);
return;
}
bp = trimblock(bp, GRE_IPONLY, len);
if(bp == nil){
gpriv->lenerr++;
return;
}
/*
* Can't delimit packet so pull it all into one block.
*/
if(qlen(c->rq) > 64*1024)
freeblist(bp);
else{
bp = concatblock(bp);
if(bp == 0)
panic("greiput");
qpass(c->rq, bp);
}
}
void
rudpiput(Proto *rudp, Ipifc *ifc, Block *bp)
{
int len, olen, ottl;
Udphdr *uh;
Conv *c;
Rudpcb *ucb;
uint8_t raddr[IPaddrlen], laddr[IPaddrlen];
uint16_t rport, lport;
Rudppriv *upriv;
Fs *f;
uint8_t *p;
upriv = rudp->priv;
f = rudp->f;
upriv->ustats.rudpInDatagrams++;
uh = (Udphdr*)(bp->rp);
/* Put back pseudo header for checksum
* (remember old values for icmpnoconv())
*/
ottl = uh->Unused;
uh->Unused = 0;
len = nhgets(uh->udplen);
olen = nhgets(uh->udpplen);
hnputs(uh->udpplen, len);
v4tov6(raddr, uh->udpsrc);
v4tov6(laddr, uh->udpdst);
lport = nhgets(uh->udpdport);
rport = nhgets(uh->udpsport);
if(nhgets(uh->udpcksum)) {
if(ptclcsum(bp, UDP_IPHDR, len+UDP_PHDRSIZE)) {
upriv->ustats.rudpInErrors++;
upriv->csumerr++;
netlog(f, Logrudp, "rudp: checksum error %I\n", raddr);
DPRINT("rudp: checksum error %I\n", raddr);
freeblist(bp);
return;
}
}
qlock(&rudp->ql);
c = iphtlook(&upriv->ht, raddr, rport, laddr, lport);
if(c == nil){
/* no conversation found */
upriv->ustats.rudpNoPorts++;
qunlock(&rudp->ql);
netlog(f, Logudp, "udp: no conv %I!%d -> %I!%d\n", raddr, rport,
laddr, lport);
uh->Unused = ottl;
hnputs(uh->udpplen, olen);
icmpnoconv(f, bp);
freeblist(bp);
return;
}
ucb = (Rudpcb*)c->ptcl;
qlock(&ucb->ql);
qunlock(&rudp->ql);
if(reliput(c, bp, raddr, rport) < 0){
qunlock(&ucb->ql);
freeb(bp);
return;
}
/*
* Trim the packet down to data size
*/
len -= (UDP_RHDRSIZE-UDP_PHDRSIZE);
bp = trimblock(bp, UDP_IPHDR+UDP_RHDRSIZE, len);
if(bp == nil) {
netlog(f, Logrudp, "rudp: len err %I.%d -> %I.%d\n",
raddr, rport, laddr, lport);
DPRINT("rudp: len err %I.%d -> %I.%d\n",
raddr, rport, laddr, lport);
upriv->lenerr++;
return;
}
netlog(f, Logrudpmsg, "rudp: %I.%d -> %I.%d l %d\n",
raddr, rport, laddr, lport, len);
switch(ucb->headers){
case 7:
/* pass the src address */
bp = padblock(bp, UDP_USEAD7);
p = bp->rp;
ipmove(p, raddr); p += IPaddrlen;
ipmove(p, laddr); p += IPaddrlen;
ipmove(p, ifc->lifc->local); p += IPaddrlen;
hnputs(p, rport); p += 2;
hnputs(p, lport);
break;
default:
/* connection oriented rudp */
if(ipcmp(c->raddr, IPnoaddr) == 0){
/* save the src address in the conversation */
ipmove(c->raddr, raddr);
c->rport = rport;
/* reply with the same ip address (if not broadcast) */
if(ipforme(f, laddr) == Runi)
ipmove(c->laddr, laddr);
else
v4tov6(c->laddr, ifc->lifc->local);
}
break;
}
if(bp->next)
bp = concatblock(bp);
if(qfull(c->rq)) {
netlog(f, Logrudp, "rudp: qfull %I.%d -> %I.%d\n", raddr, rport,
laddr, lport);
freeblist(bp);
}
else
qpass(c->rq, bp);
qunlock(&ucb->ql);
}
void
espiput(Proto *esp, Ipifc*, Block *bp)
{
Esphdr *eh;
Esptail *et;
Userhdr *uh;
Conv *c;
Espcb *ecb;
uchar raddr[IPaddrlen], laddr[IPaddrlen];
Fs *f;
uchar *auth;
ulong spi;
int payload, nexthdr;
f = esp->f;
bp = pullupblock(bp, EsphdrSize+EsptailSize);
if(bp == nil) {
netlog(f, Logesp, "esp: short packet\n");
return;
}
eh = (Esphdr*)(bp->rp);
spi = nhgetl(eh->espspi);
v4tov6(raddr, eh->espsrc);
v4tov6(laddr, eh->espdst);
qlock(esp);
/* Look for a conversation structure for this port */
c = convlookup(esp, spi);
if(c == nil) {
qunlock(esp);
netlog(f, Logesp, "esp: no conv %I -> %I!%d\n", raddr,
laddr, spi);
icmpnoconv(f, bp);
freeblist(bp);
return;
}
qlock(c);
qunlock(esp);
ecb = c->ptcl;
// too hard to do decryption/authentication on block lists
if(bp->next)
bp = concatblock(bp);
if(BLEN(bp) < EsphdrSize + ecb->espivlen + EsptailSize + ecb->ahlen) {
qunlock(c);
netlog(f, Logesp, "esp: short block %I -> %I!%d\n", raddr,
laddr, spi);
freeb(bp);
return;
}
eh = (Esphdr*)(bp->rp);
auth = bp->wp - ecb->ahlen;
if(!ecb->auth(ecb, eh->espspi, auth-eh->espspi, auth)) {
qunlock(c);
print("esp: bad auth %I -> %I!%ld\n", raddr, laddr, spi);
netlog(f, Logesp, "esp: bad auth %I -> %I!%d\n", raddr,
laddr, spi);
freeb(bp);
return;
}
payload = BLEN(bp)-EsphdrSize-ecb->ahlen;
if(payload<=0 || payload%4 != 0 || payload%ecb->espblklen!=0) {
qunlock(c);
netlog(f, Logesp, "esp: bad length %I -> %I!%d payload=%d BLEN=%d\n", raddr,
laddr, spi, payload, BLEN(bp));
freeb(bp);
return;
}
if(!ecb->cipher(ecb, bp->rp+EsphdrSize, payload)) {
qunlock(c);
print("esp: cipher failed %I -> %I!%ld: %r\n", raddr, laddr, spi);
netlog(f, Logesp, "esp: cipher failed %I -> %I!%d: %r\n", raddr,
laddr, spi);
freeb(bp);
return;
}
payload -= EsptailSize;
et = (Esptail*)(bp->rp + EsphdrSize + payload);
payload -= et->pad + ecb->espivlen;
nexthdr = et->nexthdr;
if(payload <= 0) {
qunlock(c);
netlog(f, Logesp, "esp: short packet after decrypt %I -> %I!%d\n", raddr,
laddr, spi);
freeb(bp);
return;
}
// trim packet
bp->rp += EsphdrSize + ecb->espivlen;
bp->wp = bp->rp + payload;
if(ecb->header) {
// assume UserhdrSize < EsphdrSize
bp->rp -= UserhdrSize;
uh = (Userhdr*)bp->rp;
memset(uh, 0, UserhdrSize);
uh->nexthdr = nexthdr;
}
if(qfull(c->rq)){
netlog(f, Logesp, "esp: qfull %I -> %I.%uld\n", raddr,
laddr, spi);
freeblist(bp);
}else {
//print("esp: pass up: %uld\n", BLEN(bp));
qpass(c->rq, bp);
}
qunlock(c);
}