void run_read(CQ qp, RPQ rside, RPQ wside)
{
/* more data in queue? run more reader threads */
if (qsize(qp) > 0)
{
/* run more readers */
start_reader(rside);
}
/* some space freed? run some writer threads */
if (!rpqempty(wside) && !qfull(qp))
Run(rpqtop(wside));
}
int
kprint(char *fmt, ...)
{
va_list arg;
char buf[PRINTSIZE];
int n;
va_start(arg, fmt);
n = vseprint(buf, buf+sizeof(buf), fmt, arg) - buf;
va_end(arg);
if(qfull(klogq))
qflush(klogq);
return qproduce(klogq, buf, n);
}
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);
}
/*
* 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);
}
/*
* process a rcvd reliable packet. return -1 if not to be passed to user process,
* 0 therwise.
*
* called with ucb locked.
*/
int
reliput(Conv *c, Block *bp, uint8_t *addr, uint16_t port)
{
Block *nbp;
Rudpcb *ucb;
Rudppriv *upriv;
Udphdr *uh;
Reliable *r;
Rudphdr *rh;
uint32_t seq, ack, sgen, agen, ackreal;
int rv = -1;
/* get fields */
uh = (Udphdr*)(bp->rp);
rh = (Rudphdr*)uh;
seq = nhgetl(rh->relseq);
sgen = nhgetl(rh->relsgen);
ack = nhgetl(rh->relack);
agen = nhgetl(rh->relagen);
upriv = c->p->priv;
ucb = (Rudpcb*)c->ptcl;
r = relstate(ucb, addr, port, "input");
DPRINT("rcvd %lud/%lud, %lud/%lud, r->sndgen = %lud\n",
seq, sgen, ack, agen, r->sndgen);
/* if acking an incorrect generation, ignore */
if(ack && agen != r->sndgen)
goto out;
/* Look for a hangup */
if(sgen == Hangupgen) {
if(agen == r->sndgen)
relforget(c, addr, port, 0);
goto out;
}
/* make sure we're not talking to a new remote side */
if(r->rcvgen != sgen){
if(seq != 0 && seq != 1)
goto out;
/* new connection */
if(r->rcvgen != 0){
DPRINT("new con r->rcvgen = %lud, sgen = %lud\n", r->rcvgen, sgen);
relhangup(c, r);
}
r->rcvgen = sgen;
}
/* dequeue acked packets */
if(ack && agen == r->sndgen){
ackreal = 0;
while(r->unacked != nil && INSEQ(ack, r->ackrcvd, r->sndseq)){
nbp = r->unacked;
r->unacked = nbp->list;
DPRINT("%lud/%lud acked, r->sndgen = %lud\n",
ack, agen, r->sndgen);
freeb(nbp);
r->ackrcvd = NEXTSEQ(r->ackrcvd);
ackreal = 1;
}
/* flow control */
if(UNACKED(r) < Maxunacked/8 && r->blocked)
wakeup(&r->vous);
/*
* retransmit next packet if the acked packet
* was transmitted more than once
*/
if(ackreal && r->unacked != nil){
r->timeout = 0;
if(r->xmits > 1){
r->xmits = 1;
relrexmit(c, r);
}
}
}
/* no message or input queue full */
if(seq == 0 || qfull(c->rq))
goto out;
/* refuse out of order delivery */
if(seq != NEXTSEQ(r->rcvseq)){
relsendack(c, r, 0); /* tell him we got it already */
upriv->orders++;
DPRINT("out of sequence %lud not %lud\n", seq, NEXTSEQ(r->rcvseq));
goto out;
}
r->rcvseq = seq;
rv = 0;
out:
relput(r);
return rv;
}
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);
}
