/*
* 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);
}
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);
}
/*
* encapsulate next IP packet on x's write queue in IP/ESP packet
* and initiate output of the result.
*/
static void
espkick(void *x)
{
int nexthdr, payload, pad, align;
uint8_t *auth;
Block *bp;
Conv *c = x;
Esp4hdr *eh4;
Esp6hdr *eh6;
Espcb *ecb;
Esptail *et;
Userhdr *uh;
Versdep vers;
getverslens(convipvers(c), &vers);
bp = qget(c->wq);
if(bp == nil)
return;
qlock(c);
ecb = c->ptcl;
if(ecb->header) {
/* make sure the message has a User header */
bp = pullupblock(bp, Userhdrlen);
if(bp == nil) {
qunlock(c);
return;
}
uh = (Userhdr*)bp->rp;
nexthdr = uh->nexthdr;
bp->rp += Userhdrlen;
} else {
nexthdr = 0; /* what should this be? */
}
payload = BLEN(bp) + ecb->espivlen;
/* Make space to fit ip header */
bp = padblock(bp, vers.hdrlen + ecb->espivlen);
getpktspiaddrs(bp->rp, &vers);
align = 4;
if(ecb->espblklen > align)
align = ecb->espblklen;
if(align % ecb->ahblklen != 0)
panic("espkick: ahblklen is important after all");
pad = (align-1) - (payload + Esptaillen-1)%align;
/*
* Make space for tail
* this is done by calling padblock with a negative size
* Padblock does not change bp->wp!
*/
bp = padblock(bp, -(pad+Esptaillen+ecb->ahlen));
bp->wp += pad+Esptaillen+ecb->ahlen;
et = (Esptail*)(bp->rp + vers.hdrlen + payload + pad);
/* fill in tail */
et->pad = pad;
et->nexthdr = nexthdr;
/* encrypt the payload */
ecb->cipher(ecb, bp->rp + vers.hdrlen, payload + pad + Esptaillen);
auth = bp->rp + vers.hdrlen + payload + pad + Esptaillen;
/* fill in head; construct a new IP header and an ESP header */
if (vers.version == V4) {
eh4 = (Esp4hdr *)bp->rp;
eh4->vihl = IP_VER4;
v6tov4(eh4->espsrc, c->laddr);
v6tov4(eh4->espdst, c->raddr);
eh4->espproto = IP_ESPPROTO;
eh4->frag[0] = 0;
eh4->frag[1] = 0;
hnputl(eh4->espspi, ecb->spi);
hnputl(eh4->espseq, ++ecb->seq);
} else {
eh6 = (Esp6hdr *)bp->rp;
eh6->vcf[0] = IP_VER6;
ipmove(eh6->src, c->laddr);
ipmove(eh6->dst, c->raddr);
eh6->proto = IP_ESPPROTO;
hnputl(eh6->espspi, ecb->spi);
hnputl(eh6->espseq, ++ecb->seq);
}
/* compute secure hash */
ecb->auth(ecb, bp->rp + vers.iphdrlen, (vers.hdrlen - vers.iphdrlen) +
payload + pad + Esptaillen, auth);
qunlock(c);
/* print("esp: pass down: %uld\n", BLEN(bp)); */
if (vers.version == V4)
ipoput4(c->p->f, bp, 0, c->ttl, c->tos, c);
else
ipoput6(c->p->f, bp, 0, c->ttl, c->tos, c);
}
static void
espkick(void *x)
{
Conv *c = x;
Esphdr *eh;
Esptail *et;
Userhdr *uh;
Espcb *ecb;
Block *bp;
int nexthdr;
int payload;
int pad;
int align;
uchar *auth;
bp = qget(c->wq);
if(bp == nil)
return;
qlock(c);
ecb = c->ptcl;
if(ecb->header) {
/* make sure the message has a User header */
bp = pullupblock(bp, UserhdrSize);
if(bp == nil) {
qunlock(c);
return;
}
uh = (Userhdr*)bp->rp;
nexthdr = uh->nexthdr;
bp->rp += UserhdrSize;
} else {
nexthdr = 0; // what should this be?
}
payload = BLEN(bp) + ecb->espivlen;
/* Make space to fit ip header */
bp = padblock(bp, EsphdrSize + ecb->espivlen);
align = 4;
if(ecb->espblklen > align)
align = ecb->espblklen;
if(align % ecb->ahblklen != 0)
panic("espkick: ahblklen is important after all");
pad = (align-1) - (payload + EsptailSize-1)%align;
/*
* Make space for tail
* this is done by calling padblock with a negative size
* Padblock does not change bp->wp!
*/
bp = padblock(bp, -(pad+EsptailSize+ecb->ahlen));
bp->wp += pad+EsptailSize+ecb->ahlen;
eh = (Esphdr *)(bp->rp);
et = (Esptail*)(bp->rp + EsphdrSize + payload + pad);
// fill in tail
et->pad = pad;
et->nexthdr = nexthdr;
ecb->cipher(ecb, bp->rp+EsphdrSize, payload+pad+EsptailSize);
auth = bp->rp + EsphdrSize + payload + pad + EsptailSize;
// fill in head
eh->vihl = IP_VER4;
hnputl(eh->espspi, ecb->spi);
hnputl(eh->espseq, ++ecb->seq);
v6tov4(eh->espsrc, c->laddr);
v6tov4(eh->espdst, c->raddr);
eh->espproto = IP_ESPPROTO;
eh->frag[0] = 0;
eh->frag[1] = 0;
ecb->auth(ecb, bp->rp+IphdrSize, (EsphdrSize-IphdrSize)+payload+pad+EsptailSize, auth);
qunlock(c);
//print("esp: pass down: %uld\n", BLEN(bp));
ipoput4(c->p->f, bp, 0, c->ttl, c->tos, c);
}
