123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857 |
- #include "u.h"
- #include "../port/lib.h"
- #include "mem.h"
- #include "dat.h"
- #include "fns.h"
- #include "../port/error.h"
- #include "ip.h"
- #define DPRINT if(0)print
- typedef struct Ipmuxrock Ipmuxrock;
- typedef struct Ipmux Ipmux;
- typedef struct Ip4hdr Ip4hdr;
- typedef struct Ip6hdr Ip6hdr;
- enum
- {
- IPHDR = 20, /* sizeof(Ip4hdr) */
- };
- struct Ip4hdr
- {
- uchar vihl; /* Version and header length */
- uchar tos; /* Type of service */
- uchar length[2]; /* packet length */
- uchar id[2]; /* ip->identification */
- uchar frag[2]; /* Fragment information */
- uchar ttl; /* Time to live */
- uchar proto; /* Protocol */
- uchar cksum[2]; /* Header checksum */
- uchar src[4]; /* IP source */
- uchar dst[4]; /* IP destination */
- uchar data[1]; /* start of data */
- };
- Ip4hdr *ipoff = 0;
- struct Ip6hdr
- {
- uchar vcf[4]; /* version, class label, and flow label */
- uchar ploadlen[2]; /* payload length */
- uchar proto; /* next header, i.e. proto */
- uchar ttl; /* hop limit, i.e. ttl */
- uchar src[16]; /* IP source */
- uchar dst[16]; /* IP destination */
- };
- enum
- {
- Tproto,
- Tdata,
- Tiph,
- Tdst,
- Tsrc,
- Tifc,
- Cother = 0,
- Cbyte, /* single byte */
- Cmbyte, /* single byte with mask */
- Cshort, /* single short */
- Cmshort, /* single short with mask */
- Clong, /* single long */
- Cmlong, /* single long with mask */
- Cifc,
- Cmifc,
- };
- char *ftname[] =
- {
- [Tproto] "proto",
- [Tdata] "data",
- [Tiph] "iph",
- [Tdst] "dst",
- [Tsrc] "src",
- [Tifc] "ifc",
- };
- /*
- * a node in the decision tree
- */
- struct Ipmux
- {
- Ipmux *yes;
- Ipmux *no;
- uchar type; /* type of field(Txxxx) */
- uchar ctype; /* tupe of comparison(Cxxxx) */
- uchar len; /* length in bytes of item to compare */
- uchar n; /* number of items val points to */
- short off; /* offset of comparison */
- short eoff; /* end offset of comparison */
- uchar skiphdr; /* should offset start after ipheader */
- uchar *val;
- uchar *mask;
- uchar *e; /* val+n*len*/
- int ref; /* so we can garbage collect */
- Conv *conv;
- };
- /*
- * someplace to hold per conversation data
- */
- struct Ipmuxrock
- {
- Ipmux *chain;
- };
- static int ipmuxsprint(Ipmux*, int, char*, int);
- static void ipmuxkick(void *x);
- static char*
- skipwhite(char *p)
- {
- while(*p == ' ' || *p == '\t')
- p++;
- return p;
- }
- static char*
- follows(char *p, char c)
- {
- char *f;
- f = strchr(p, c);
- if(f == nil)
- return nil;
- *f++ = 0;
- f = skipwhite(f);
- if(*f == 0)
- return nil;
- return f;
- }
- static Ipmux*
- parseop(char **pp)
- {
- char *p = *pp;
- int type, off, end, len;
- Ipmux *f;
- p = skipwhite(p);
- if(strncmp(p, "dst", 3) == 0){
- type = Tdst;
- off = (ulong)(ipoff->dst);
- len = IPv4addrlen;
- p += 3;
- }
- else if(strncmp(p, "src", 3) == 0){
- type = Tsrc;
- off = (ulong)(ipoff->src);
- len = IPv4addrlen;
- p += 3;
- }
- else if(strncmp(p, "ifc", 3) == 0){
- type = Tifc;
- off = -IPv4addrlen;
- len = IPv4addrlen;
- p += 3;
- }
- else if(strncmp(p, "proto", 5) == 0){
- type = Tproto;
- off = (ulong)&(ipoff->proto);
- len = 1;
- p += 5;
- }
- else if(strncmp(p, "data", 4) == 0 || strncmp(p, "iph", 3) == 0){
- if(strncmp(p, "data", 4) == 0) {
- type = Tdata;
- p += 4;
- }
- else {
- type = Tiph;
- p += 3;
- }
- p = skipwhite(p);
- if(*p != '[')
- return nil;
- p++;
- off = strtoul(p, &p, 0);
- if(off < 0 || off > (64-IPHDR))
- return nil;
- p = skipwhite(p);
- if(*p != ':')
- end = off;
- else {
- p++;
- p = skipwhite(p);
- end = strtoul(p, &p, 0);
- if(end < off)
- return nil;
- p = skipwhite(p);
- }
- if(*p != ']')
- return nil;
- p++;
- len = end - off + 1;
- }
- else
- return nil;
- f = smalloc(sizeof(*f));
- f->type = type;
- f->len = len;
- f->off = off;
- f->val = nil;
- f->mask = nil;
- f->n = 1;
- f->ref = 1;
- if(type == Tdata)
- f->skiphdr = 1;
- else
- f->skiphdr = 0;
- return f;
- }
- static int
- htoi(char x)
- {
- if(x >= '0' && x <= '9')
- x -= '0';
- else if(x >= 'a' && x <= 'f')
- x -= 'a' - 10;
- else if(x >= 'A' && x <= 'F')
- x -= 'A' - 10;
- else
- x = 0;
- return x;
- }
- static int
- hextoi(char *p)
- {
- return (htoi(p[0])<<4) | htoi(p[1]);
- }
- static void
- parseval(uchar *v, char *p, int len)
- {
- while(*p && len-- > 0){
- *v++ = hextoi(p);
- p += 2;
- }
- }
- static Ipmux*
- parsemux(char *p)
- {
- int n, nomask;
- Ipmux *f;
- char *val;
- char *mask;
- char *vals[20];
- uchar *v;
- /* parse operand */
- f = parseop(&p);
- if(f == nil)
- return nil;
- /* find value */
- val = follows(p, '=');
- if(val == nil)
- goto parseerror;
- /* parse mask */
- mask = follows(p, '&');
- if(mask != nil){
- switch(f->type){
- case Tsrc:
- case Tdst:
- case Tifc:
- f->mask = smalloc(f->len);
- v4parseip(f->mask, mask);
- break;
- case Tdata:
- case Tiph:
- f->mask = smalloc(f->len);
- parseval(f->mask, mask, f->len);
- break;
- default:
- goto parseerror;
- }
- nomask = 0;
- } else {
- nomask = 1;
- f->mask = smalloc(f->len);
- memset(f->mask, 0xff, f->len);
- }
- /* parse vals */
- f->n = getfields(val, vals, sizeof(vals)/sizeof(char*), 1, "|");
- if(f->n == 0)
- goto parseerror;
- f->val = smalloc(f->n*f->len);
- v = f->val;
- for(n = 0; n < f->n; n++){
- switch(f->type){
- case Tsrc:
- case Tdst:
- case Tifc:
- v4parseip(v, vals[n]);
- break;
- case Tproto:
- case Tdata:
- case Tiph:
- parseval(v, vals[n], f->len);
- break;
- }
- v += f->len;
- }
- f->eoff = f->off + f->len;
- f->e = f->val + f->n*f->len;
- f->ctype = Cother;
- if(f->n == 1){
- switch(f->len){
- case 1:
- f->ctype = nomask ? Cbyte : Cmbyte;
- break;
- case 2:
- f->ctype = nomask ? Cshort : Cmshort;
- break;
- case 4:
- if(f->type == Tifc)
- f->ctype = nomask ? Cifc : Cmifc;
- else
- f->ctype = nomask ? Clong : Cmlong;
- break;
- }
- }
- return f;
- parseerror:
- if(f->mask)
- free(f->mask);
- if(f->val)
- free(f->val);
- free(f);
- return nil;
- }
- /*
- * Compare relative ordering of two ipmuxs. This doesn't compare the
- * values, just the fields being looked at.
- *
- * returns: <0 if a is a more specific match
- * 0 if a and b are matching on the same fields
- * >0 if b is a more specific match
- */
- static int
- ipmuxcmp(Ipmux *a, Ipmux *b)
- {
- int n;
- /* compare types, lesser ones are more important */
- n = a->type - b->type;
- if(n != 0)
- return n;
- /* compare offsets, call earlier ones more specific */
- n = (a->off+((int)a->skiphdr)*(ulong)ipoff->data) -
- (b->off+((int)b->skiphdr)*(ulong)ipoff->data);
- if(n != 0)
- return n;
- /* compare match lengths, longer ones are more specific */
- n = b->len - a->len;
- if(n != 0)
- return n;
- /*
- * if we get here we have two entries matching
- * the same bytes of the record. Now check
- * the mask for equality. Longer masks are
- * more specific.
- */
- if(a->mask != nil && b->mask == nil)
- return -1;
- if(a->mask == nil && b->mask != nil)
- return 1;
- if(a->mask != nil && b->mask != nil){
- n = memcmp(b->mask, a->mask, a->len);
- if(n != 0)
- return n;
- }
- return 0;
- }
- /*
- * Compare the values of two ipmuxs. We're assuming that ipmuxcmp
- * returned 0 comparing them.
- */
- static int
- ipmuxvalcmp(Ipmux *a, Ipmux *b)
- {
- int n;
- n = b->len*b->n - a->len*a->n;
- if(n != 0)
- return n;
- return memcmp(a->val, b->val, a->len*a->n);
- }
- /*
- * add onto an existing ipmux chain in the canonical comparison
- * order
- */
- static void
- ipmuxchain(Ipmux **l, Ipmux *f)
- {
- for(; *l; l = &(*l)->yes)
- if(ipmuxcmp(f, *l) < 0)
- break;
- f->yes = *l;
- *l = f;
- }
- /*
- * copy a tree
- */
- static Ipmux*
- ipmuxcopy(Ipmux *f)
- {
- Ipmux *nf;
- if(f == nil)
- return nil;
- nf = smalloc(sizeof *nf);
- *nf = *f;
- nf->no = ipmuxcopy(f->no);
- nf->yes = ipmuxcopy(f->yes);
- nf->val = smalloc(f->n*f->len);
- nf->e = nf->val + f->len*f->n;
- memmove(nf->val, f->val, f->n*f->len);
- return nf;
- }
- static void
- ipmuxfree(Ipmux *f)
- {
- if(f->val != nil)
- free(f->val);
- free(f);
- }
- static void
- ipmuxtreefree(Ipmux *f)
- {
- if(f == nil)
- return;
- if(f->no != nil)
- ipmuxfree(f->no);
- if(f->yes != nil)
- ipmuxfree(f->yes);
- ipmuxfree(f);
- }
- /*
- * merge two trees
- */
- static Ipmux*
- ipmuxmerge(Ipmux *a, Ipmux *b)
- {
- int n;
- Ipmux *f;
- if(a == nil)
- return b;
- if(b == nil)
- return a;
- n = ipmuxcmp(a, b);
- if(n < 0){
- f = ipmuxcopy(b);
- a->yes = ipmuxmerge(a->yes, b);
- a->no = ipmuxmerge(a->no, f);
- return a;
- }
- if(n > 0){
- f = ipmuxcopy(a);
- b->yes = ipmuxmerge(b->yes, a);
- b->no = ipmuxmerge(b->no, f);
- return b;
- }
- if(ipmuxvalcmp(a, b) == 0){
- a->yes = ipmuxmerge(a->yes, b->yes);
- a->no = ipmuxmerge(a->no, b->no);
- a->ref++;
- ipmuxfree(b);
- return a;
- }
- a->no = ipmuxmerge(a->no, b);
- return a;
- }
- /*
- * remove a chain from a demux tree. This is like merging accept that
- * we remove instead of insert.
- */
- static int
- ipmuxremove(Ipmux **l, Ipmux *f)
- {
- int n, rv;
- Ipmux *ft;
- if(f == nil)
- return 0; /* we've removed it all */
- if(*l == nil)
- return -1;
- ft = *l;
- n = ipmuxcmp(ft, f);
- if(n < 0){
- /* *l is maching an earlier field, descend both paths */
- rv = ipmuxremove(&ft->yes, f);
- rv += ipmuxremove(&ft->no, f);
- return rv;
- }
- if(n > 0){
- /* f represents an earlier field than *l, this should be impossible */
- return -1;
- }
- /* if we get here f and *l are comparing the same fields */
- if(ipmuxvalcmp(ft, f) != 0){
- /* different values mean mutually exclusive */
- return ipmuxremove(&ft->no, f);
- }
- /* we found a match */
- if(--(ft->ref) == 0){
- /*
- * a dead node implies the whole yes side is also dead.
- * since our chain is constrained to be on that side,
- * we're done.
- */
- ipmuxtreefree(ft->yes);
- *l = ft->no;
- ipmuxfree(ft);
- return 0;
- }
- /*
- * free the rest of the chain. it is constrained to match the
- * yes side.
- */
- return ipmuxremove(&ft->yes, f->yes);
- }
- /*
- * connection request is a semi separated list of filters
- * e.g. proto=17;dat[0:4]=11aa22bb;ifc=135.104.9.2&255.255.255.0
- *
- * there's no protection against overlapping specs.
- */
- static char*
- ipmuxconnect(Conv *c, char **argv, int argc)
- {
- int i, n;
- char *field[10];
- Ipmux *mux, *chain;
- Ipmuxrock *r;
- Fs *f;
- f = c->p->f;
- if(argc != 2)
- return Ebadarg;
- n = getfields(argv[1], field, nelem(field), 1, ";");
- if(n <= 0)
- return Ebadarg;
- chain = nil;
- mux = nil;
- for(i = 0; i < n; i++){
- mux = parsemux(field[i]);
- if(mux == nil){
- ipmuxtreefree(chain);
- return Ebadarg;
- }
- ipmuxchain(&chain, mux);
- }
- if(chain == nil)
- return Ebadarg;
- mux->conv = c;
- /* save a copy of the chain so we can later remove it */
- mux = ipmuxcopy(chain);
- r = (Ipmuxrock*)(c->ptcl);
- r->chain = chain;
- /* add the chain to the protocol demultiplexor tree */
- wlock(f);
- f->ipmux->priv = ipmuxmerge(f->ipmux->priv, mux);
- wunlock(f);
- Fsconnected(c, nil);
- return nil;
- }
- static int
- ipmuxstate(Conv *c, char *state, int n)
- {
- Ipmuxrock *r;
-
- r = (Ipmuxrock*)(c->ptcl);
- return ipmuxsprint(r->chain, 0, state, n);
- }
- static void
- ipmuxcreate(Conv *c)
- {
- Ipmuxrock *r;
- c->rq = qopen(64*1024, Qmsg, 0, c);
- c->wq = qopen(64*1024, Qkick, ipmuxkick, c);
- r = (Ipmuxrock*)(c->ptcl);
- r->chain = nil;
- }
- static char*
- ipmuxannounce(Conv*, char**, int)
- {
- return "ipmux does not support announce";
- }
- static void
- ipmuxclose(Conv *c)
- {
- Ipmuxrock *r;
- Fs *f = c->p->f;
- r = (Ipmuxrock*)(c->ptcl);
- qclose(c->rq);
- qclose(c->wq);
- qclose(c->eq);
- ipmove(c->laddr, IPnoaddr);
- ipmove(c->raddr, IPnoaddr);
- c->lport = 0;
- c->rport = 0;
- wlock(f);
- ipmuxremove(&(c->p->priv), r->chain);
- wunlock(f);
- ipmuxtreefree(r->chain);
- r->chain = nil;
- }
- /*
- * takes a fully formed ip packet and just passes it down
- * the stack
- */
- static void
- ipmuxkick(void *x)
- {
- Conv *c = x;
- Block *bp;
- struct Ip6hdr *ih6;
- bp = qget(c->wq);
- if(bp == nil)
- return;
- else {
- Ip4hdr *ih4 = (Ip4hdr*)(bp->rp);
- if((ih4->vihl)&0xF0 != 0x60)
- ipoput4(c->p->f, bp, 0, ih4->ttl, ih4->tos, nil);
- else {
- ih6 = (struct Ip6hdr*)ih4;
- ipoput6(c->p->f, bp, 0, ih6->ttl, 0, nil);
- }
- }
- }
- static void
- ipmuxiput(Proto *p, Ipifc *ifc, Block *bp)
- {
- int len, hl;
- Fs *f = p->f;
- uchar *m, *h, *v, *e, *ve, *hp;
- Conv *c;
- Ipmux *mux;
- Ip4hdr *ip;
- Ip6hdr *ip6;
- ip = (Ip4hdr*)bp->rp;
- hl = (ip->vihl&0x0F)<<2;
- if(p->priv == nil)
- goto nomatch;
- h = bp->rp;
- len = BLEN(bp);
- /* run the v4 filter */
- rlock(f);
- c = nil;
- mux = f->ipmux->priv;
- while(mux != nil){
- if(mux->eoff > len){
- mux = mux->no;
- continue;
- }
- hp = h + mux->off + ((int)mux->skiphdr)*hl;
- switch(mux->ctype){
- case Cbyte:
- if(*mux->val == *hp)
- goto yes;
- break;
- case Cmbyte:
- if((*hp & *mux->mask) == *mux->val)
- goto yes;
- break;
- case Cshort:
- if(*((ushort*)mux->val) == *(ushort*)hp)
- goto yes;
- break;
- case Cmshort:
- if((*(ushort*)hp & (*((ushort*)mux->mask))) == *((ushort*)mux->val))
- goto yes;
- break;
- case Clong:
- if(*((ulong*)mux->val) == *(ulong*)hp)
- goto yes;
- break;
- case Cmlong:
- if((*(ulong*)hp & (*((ulong*)mux->mask))) == *((ulong*)mux->val))
- goto yes;
- break;
- case Cifc:
- if(*((ulong*)mux->val) == *(ulong*)(ifc->lifc->local + IPv4off))
- goto yes;
- break;
- case Cmifc:
- if((*(ulong*)(ifc->lifc->local + IPv4off) & (*((ulong*)mux->mask))) == *((ulong*)mux->val))
- goto yes;
- break;
- default:
- v = mux->val;
- for(e = mux->e; v < e; v = ve){
- m = mux->mask;
- hp = h + mux->off;
- for(ve = v + mux->len; v < ve; v++){
- if((*hp++ & *m++) != *v)
- break;
- }
- if(v == ve)
- goto yes;
- }
- }
- mux = mux->no;
- continue;
- yes:
- if(mux->conv != nil)
- c = mux->conv;
- mux = mux->yes;
- }
- runlock(f);
- if(c != nil){
- /* tack on interface address */
- bp = padblock(bp, IPaddrlen);
- ipmove(bp->rp, ifc->lifc->local);
- bp = concatblock(bp);
- if(bp != nil)
- if(qpass(c->rq, bp) < 0)
- print("Q");
- return;
- }
- nomatch:
- /* doesn't match any filter, hand it to the specific protocol handler */
- ip = (Ip4hdr*)bp->rp;
- if((ip->vihl&0xF0)==0x40) {
- p = f->t2p[ip->proto];
- } else {
- ip6 = (Ip6hdr*)bp->rp;
- p = f->t2p[ip6->proto];
- }
- if(p && p->rcv)
- (*p->rcv)(p, ifc, bp);
- else
- freeblist(bp);
- return;
- }
- static int
- ipmuxsprint(Ipmux *mux, int level, char *buf, int len)
- {
- int i, j, n;
- uchar *v;
- n = 0;
- for(i = 0; i < level; i++)
- n += snprint(buf+n, len-n, " ");
- if(mux == nil){
- n += snprint(buf+n, len-n, "\n");
- return n;
- }
- n += snprint(buf+n, len-n, "h[%d:%d]&",
- mux->off+((int)mux->skiphdr)*((int)ipoff->data),
- mux->off+(((int)mux->skiphdr)*((int)ipoff->data))+mux->len-1);
- for(i = 0; i < mux->len; i++)
- n += snprint(buf+n, len - n, "%2.2ux", mux->mask[i]);
- n += snprint(buf+n, len-n, "=");
- v = mux->val;
- for(j = 0; j < mux->n; j++){
- for(i = 0; i < mux->len; i++)
- n += snprint(buf+n, len - n, "%2.2ux", *v++);
- n += snprint(buf+n, len-n, "|");
- }
- n += snprint(buf+n, len-n, "\n");
- level++;
- n += ipmuxsprint(mux->no, level, buf+n, len-n);
- n += ipmuxsprint(mux->yes, level, buf+n, len-n);
- return n;
- }
- static int
- ipmuxstats(Proto *p, char *buf, int len)
- {
- int n;
- Fs *f = p->f;
- rlock(f);
- n = ipmuxsprint(p->priv, 0, buf, len);
- runlock(f);
- return n;
- }
- void
- ipmuxinit(Fs *f)
- {
- Proto *ipmux;
- ipmux = smalloc(sizeof(Proto));
- ipmux->priv = nil;
- ipmux->name = "ipmux";
- ipmux->connect = ipmuxconnect;
- ipmux->announce = ipmuxannounce;
- ipmux->state = ipmuxstate;
- ipmux->create = ipmuxcreate;
- ipmux->close = ipmuxclose;
- ipmux->rcv = ipmuxiput;
- ipmux->ctl = nil;
- ipmux->advise = nil;
- ipmux->stats = ipmuxstats;
- ipmux->ipproto = -1;
- ipmux->nc = 64;
- ipmux->ptclsize = sizeof(Ipmuxrock);
- f->ipmux = ipmux; /* hack for Fsrcvpcol */
- Fsproto(f, ipmux);
- }
|