#include <u.h>#include <libc.h>#include <venti.h>#include "packet.h" static Frag *fragAlloc(Packet*, int n, int pos, Frag *next);static Frag *fragDup(Packet*, Frag*);static void fragFree(Frag*); static Mem *memAlloc(int, int);static void memFree(Mem*);static int memHead(Mem *m, uchar *rp, int n);static int memTail(Mem *m, uchar *wp, int n); static char EPacketSize[] = "bad packet size";static char EPacketOffset[] = "bad packet offset";static char EBadSize[] = "bad size"; static struct { Lock lk; Packet *packet; int npacket; Frag *frag; int nfrag; Mem *bigMem; int nbigMem; Mem *smallMem; int nsmallMem;} freeList; #define FRAGSIZE(f) ((f)->wp - (f)->rp)#define FRAGASIZE(f) ((f)->mem->ep - (f)->mem->bp) #define NOTFREE(p) assert((p)->size>=0) Packet *packetAlloc(void){ Packet *p; lock(&freeList.lk); p = freeList.packet; if(p != nil) freeList.packet = p->next; else freeList.npacket++; unlock(&freeList.lk); if(p == nil) p = vtMemBrk(sizeof(Packet)); else assert(p->size == -1); p->size = 0; p->asize = 0; p->first = nil; p->last = nil; p->next = nil; return p;} voidpacketFree(Packet *p){ Frag *f, *ff; if(0)fprint(2, "packetFree %p\n", p); NOTFREE(p); p->size = -1; for(f=p->first; f!=nil; f=ff) { ff = f->next; fragFree(f); } p->first = nil; p->last = nil; lock(&freeList.lk); p->next = freeList.packet; freeList.packet = p; unlock(&freeList.lk);} Packet *packetDup(Packet *p, int offset, int n){ Frag *f, *ff; Packet *pp; NOTFREE(p); if(offset < 0 || n < 0 || offset+n > p->size) { vtSetError(EBadSize); return nil; } pp = packetAlloc(); if(n == 0) return pp; pp->size = n; /* skip offset */ for(f=p->first; offset >= FRAGSIZE(f); f=f->next) offset -= FRAGSIZE(f); /* first frag */ ff = fragDup(pp, f); ff->rp += offset; pp->first = ff; n -= FRAGSIZE(ff); pp->asize += FRAGASIZE(ff); /* the remaining */ while(n > 0) { f = f->next; ff->next = fragDup(pp, f); ff = ff->next; n -= FRAGSIZE(ff); pp->asize += FRAGASIZE(ff); } /* fix up last frag: note n <= 0 */ ff->wp += n; ff->next = nil; pp->last = ff; return pp;} Packet *packetSplit(Packet *p, int n){ Packet *pp; Frag *f, *ff; NOTFREE(p); if(n < 0 || n > p->size) { vtSetError(EPacketSize); return nil; } pp = packetAlloc(); if(n == 0) return pp; pp->size = n; p->size -= n; ff = nil; for(f=p->first; n > 0 && n >= FRAGSIZE(f); f=f->next) { n -= FRAGSIZE(f); p->asize -= FRAGASIZE(f); pp->asize += FRAGASIZE(f); ff = f; } /* split shared frag */ if(n > 0) { ff = f; f = fragDup(pp, ff); pp->asize += FRAGASIZE(ff); ff->next = nil; ff->wp = ff->rp + n; f->rp += n; } pp->first = p->first; pp->last = ff; p->first = f; return pp;} intpacketConsume(Packet *p, uchar *buf, int n){ NOTFREE(p); if(buf && !packetCopy(p, buf, 0, n)) return 0; return packetTrim(p, n, p->size-n);} intpacketTrim(Packet *p, int offset, int n){ Frag *f, *ff; NOTFREE(p); if(offset < 0 || offset > p->size) { vtSetError(EPacketOffset); return 0; } if(n < 0 || offset + n > p->size) { vtSetError(EPacketOffset); return 0; } p->size = n; /* easy case */ if(n == 0) { for(f=p->first; f != nil; f=ff) { ff = f->next; fragFree(f); } p->first = p->last = nil; p->asize = 0; return 1; } /* free before offset */ for(f=p->first; offset >= FRAGSIZE(f); f=ff) { p->asize -= FRAGASIZE(f); offset -= FRAGSIZE(f); ff = f->next; fragFree(f); } /* adjust frag */ f->rp += offset; p->first = f; /* skip middle */ for(; n > 0 && n > FRAGSIZE(f); f=f->next) n -= FRAGSIZE(f); /* adjust end */ f->wp = f->rp + n; p->last = f; ff = f->next; f->next = nil; /* free after */ for(f=ff; f != nil; f=ff) { p->asize -= FRAGASIZE(f); ff = f->next; fragFree(f); } return 1;} uchar *packetHeader(Packet *p, int n){ Frag *f; Mem *m; NOTFREE(p); if(n <= 0 || n > MaxFragSize) { vtSetError(EPacketSize); return 0; } p->size += n; /* try and fix in current frag */ f = p->first; if(f != nil) { m = f->mem; if(n <= f->rp - m->bp) if(m->ref == 1 || memHead(m, f->rp, n)) { f->rp -= n; return f->rp; } } /* add frag to front */ f = fragAlloc(p, n, PEnd, p->first); p->asize += FRAGASIZE(f); if(p->first == nil) p->last = f; p->first = f; return f->rp;} uchar *packetTrailer(Packet *p, int n){ Mem *m; Frag *f; NOTFREE(p); if(n <= 0 || n > MaxFragSize) { vtSetError(EPacketSize); return 0; } p->size += n; /* try and fix in current frag */ if(p->first != nil) { f = p->last; m = f->mem; if(n <= m->ep - f->wp) if(m->ref == 1 || memTail(m, f->wp, n)) { f->wp += n; return f->wp - n; } } /* add frag to end */ f = fragAlloc(p, n, (p->first == nil)?PMiddle:PFront, nil); p->asize += FRAGASIZE(f); if(p->first == nil) p->first = f; else p->last->next = f; p->last = f; return f->rp;} intpacketPrefix(Packet *p, uchar *buf, int n){ Frag *f; int nn; Mem *m; NOTFREE(p); if(n <= 0) return 1; p->size += n; /* try and fix in current frag */ f = p->first; if(f != nil) { m = f->mem; nn = f->rp - m->bp; if(nn > n) nn = n; if(m->ref == 1 || memHead(m, f->rp, nn)) { f->rp -= nn; n -= nn; memmove(f->rp, buf+n, nn); } } while(n > 0) { nn = n; if(nn > MaxFragSize) nn = MaxFragSize; f = fragAlloc(p, nn, PEnd, p->first); p->asize += FRAGASIZE(f); if(p->first == nil) p->last = f; p->first = f; n -= nn; memmove(f->rp, buf+n, nn); } return 1;} intpacketAppend(Packet *p, uchar *buf, int n){ Frag *f; int nn; Mem *m; NOTFREE(p); if(n <= 0) return 1; p->size += n; /* try and fix in current frag */ if(p->first != nil) { f = p->last; m = f->mem; nn = m->ep - f->wp; if(nn > n) nn = n; if(m->ref == 1 || memTail(m, f->wp, nn)) { memmove(f->wp, buf, nn); f->wp += nn; buf += nn; n -= nn; } } while(n > 0) { nn = n; if(nn > MaxFragSize) nn = MaxFragSize; f = fragAlloc(p, nn, (p->first == nil)?PMiddle:PFront, nil); p->asize += FRAGASIZE(f); if(p->first == nil) p->first = f; else p->last->next = f; p->last = f; memmove(f->rp, buf, nn); buf += nn; n -= nn; } return 1;} intpacketConcat(Packet *p, Packet *pp){ NOTFREE(p); NOTFREE(pp); if(pp->size == 0) return 1; p->size += pp->size; p->asize += pp->asize; if(p->first != nil) p->last->next = pp->first; else p->first = pp->first; p->last = pp->last; pp->size = 0; pp->asize = 0; pp->first = nil; pp->last = nil; return 1;} uchar *packetPeek(Packet *p, uchar *buf, int offset, int n){ Frag *f; int nn; uchar *b; NOTFREE(p); if(n == 0) return buf; if(offset < 0 || offset >= p->size) { vtSetError(EPacketOffset); return 0; } if(n < 0 || offset + n > p->size) { vtSetError(EPacketSize); return 0; } /* skip up to offset */ for(f=p->first; offset >= FRAGSIZE(f); f=f->next) offset -= FRAGSIZE(f); /* easy case */ if(offset + n <= FRAGSIZE(f)) return f->rp + offset; for(b=buf; n>0; n -= nn) { nn = FRAGSIZE(f) - offset; if(nn > n) nn = n; memmove(b, f->rp+offset, nn); offset = 0; f = f->next; b += nn; } return buf;} intpacketCopy(Packet *p, uchar *buf, int offset, int n){ uchar *b; NOTFREE(p); b = packetPeek(p, buf, offset, n); if(b == nil) return 0; if(b != buf) memmove(buf, b, n); return 1;} intpacketFragments(Packet *p, IOchunk *io, int nio, int offset){ Frag *f; int size; IOchunk *eio; NOTFREE(p); if(p->size == 0 || nio <= 0) return 0; if(offset < 0 || offset > p->size) { vtSetError(EPacketOffset); return -1; } for(f=p->first; offset >= FRAGSIZE(f); f=f->next) offset -= FRAGSIZE(f); size = 0; eio = io + nio; for(; f != nil && io < eio; f=f->next) { io->addr = f->rp + offset; io->len = f->wp - (f->rp + offset); Lines 1–500 of 849. Load more
#include <u.h> #include <libc.h> #include <venti.h> #include "packet.h" static Frag *fragAlloc(Packet*, int n, int pos, Frag *next); static Frag *fragDup(Packet*, Frag*); static void fragFree(Frag*); static Mem *memAlloc(int, int); static void memFree(Mem*); static int memHead(Mem *m, uchar *rp, int n); static int memTail(Mem *m, uchar *wp, int n); static char EPacketSize[] = "bad packet size"; static char EPacketOffset[] = "bad packet offset"; static char EBadSize[] = "bad size"; static struct { Lock lk; Packet *packet; int npacket; Frag *frag; int nfrag; Mem *bigMem; int nbigMem; Mem *smallMem; int nsmallMem; } freeList; #define FRAGSIZE(f) ((f)->wp - (f)->rp) #define FRAGASIZE(f) ((f)->mem->ep - (f)->mem->bp) #define NOTFREE(p) assert((p)->size>=0) Packet * packetAlloc(void) { Packet *p; lock(&freeList.lk); p = freeList.packet; if(p != nil) freeList.packet = p->next; else freeList.npacket++; unlock(&freeList.lk); if(p == nil) p = vtMemBrk(sizeof(Packet)); else assert(p->size == -1); p->size = 0; p->asize = 0; p->first = nil; p->last = nil; p->next = nil; return p; } void packetFree(Packet *p) { Frag *f, *ff; if(0)fprint(2, "packetFree %p\n", p); NOTFREE(p); p->size = -1; for(f=p->first; f!=nil; f=ff) { ff = f->next; fragFree(f); } p->first = nil; p->last = nil; lock(&freeList.lk); p->next = freeList.packet; freeList.packet = p; unlock(&freeList.lk); } Packet * packetDup(Packet *p, int offset, int n) { Frag *f, *ff; Packet *pp; NOTFREE(p); if(offset < 0 || n < 0 || offset+n > p->size) { vtSetError(EBadSize); return nil; } pp = packetAlloc(); if(n == 0) return pp; pp->size = n; /* skip offset */ for(f=p->first; offset >= FRAGSIZE(f); f=f->next) offset -= FRAGSIZE(f); /* first frag */ ff = fragDup(pp, f); ff->rp += offset; pp->first = ff; n -= FRAGSIZE(ff); pp->asize += FRAGASIZE(ff); /* the remaining */ while(n > 0) { f = f->next; ff->next = fragDup(pp, f); ff = ff->next; n -= FRAGSIZE(ff); pp->asize += FRAGASIZE(ff); } /* fix up last frag: note n <= 0 */ ff->wp += n; ff->next = nil; pp->last = ff; return pp; } Packet * packetSplit(Packet *p, int n) { Packet *pp; Frag *f, *ff; NOTFREE(p); if(n < 0 || n > p->size) { vtSetError(EPacketSize); return nil; } pp = packetAlloc(); if(n == 0) return pp; pp->size = n; p->size -= n; ff = nil; for(f=p->first; n > 0 && n >= FRAGSIZE(f); f=f->next) { n -= FRAGSIZE(f); p->asize -= FRAGASIZE(f); pp->asize += FRAGASIZE(f); ff = f; } /* split shared frag */ if(n > 0) { ff = f; f = fragDup(pp, ff); pp->asize += FRAGASIZE(ff); ff->next = nil; ff->wp = ff->rp + n; f->rp += n; } pp->first = p->first; pp->last = ff; p->first = f; return pp; } int packetConsume(Packet *p, uchar *buf, int n) { NOTFREE(p); if(buf && !packetCopy(p, buf, 0, n)) return 0; return packetTrim(p, n, p->size-n); } int packetTrim(Packet *p, int offset, int n) { Frag *f, *ff; NOTFREE(p); if(offset < 0 || offset > p->size) { vtSetError(EPacketOffset); return 0; } if(n < 0 || offset + n > p->size) { vtSetError(EPacketOffset); return 0; } p->size = n; /* easy case */ if(n == 0) { for(f=p->first; f != nil; f=ff) { ff = f->next; fragFree(f); } p->first = p->last = nil; p->asize = 0; return 1; } /* free before offset */ for(f=p->first; offset >= FRAGSIZE(f); f=ff) { p->asize -= FRAGASIZE(f); offset -= FRAGSIZE(f); ff = f->next; fragFree(f); } /* adjust frag */ f->rp += offset; p->first = f; /* skip middle */ for(; n > 0 && n > FRAGSIZE(f); f=f->next) n -= FRAGSIZE(f); /* adjust end */ f->wp = f->rp + n; p->last = f; ff = f->next; f->next = nil; /* free after */ for(f=ff; f != nil; f=ff) { p->asize -= FRAGASIZE(f); ff = f->next; fragFree(f); } return 1; } uchar * packetHeader(Packet *p, int n) { Frag *f; Mem *m; NOTFREE(p); if(n <= 0 || n > MaxFragSize) { vtSetError(EPacketSize); return 0; } p->size += n; /* try and fix in current frag */ f = p->first; if(f != nil) { m = f->mem; if(n <= f->rp - m->bp) if(m->ref == 1 || memHead(m, f->rp, n)) { f->rp -= n; return f->rp; } } /* add frag to front */ f = fragAlloc(p, n, PEnd, p->first); p->asize += FRAGASIZE(f); if(p->first == nil) p->last = f; p->first = f; return f->rp; } uchar * packetTrailer(Packet *p, int n) { Mem *m; Frag *f; NOTFREE(p); if(n <= 0 || n > MaxFragSize) { vtSetError(EPacketSize); return 0; } p->size += n; /* try and fix in current frag */ if(p->first != nil) { f = p->last; m = f->mem; if(n <= m->ep - f->wp) if(m->ref == 1 || memTail(m, f->wp, n)) { f->wp += n; return f->wp - n; } } /* add frag to end */ f = fragAlloc(p, n, (p->first == nil)?PMiddle:PFront, nil); p->asize += FRAGASIZE(f); if(p->first == nil) p->first = f; else p->last->next = f; p->last = f; return f->rp; } int packetPrefix(Packet *p, uchar *buf, int n) { Frag *f; int nn; Mem *m; NOTFREE(p); if(n <= 0) return 1; p->size += n; /* try and fix in current frag */ f = p->first; if(f != nil) { m = f->mem; nn = f->rp - m->bp; if(nn > n) nn = n; if(m->ref == 1 || memHead(m, f->rp, nn)) { f->rp -= nn; n -= nn; memmove(f->rp, buf+n, nn); } } while(n > 0) { nn = n; if(nn > MaxFragSize) nn = MaxFragSize; f = fragAlloc(p, nn, PEnd, p->first); p->asize += FRAGASIZE(f); if(p->first == nil) p->last = f; p->first = f; n -= nn; memmove(f->rp, buf+n, nn); } return 1; } int packetAppend(Packet *p, uchar *buf, int n) { Frag *f; int nn; Mem *m; NOTFREE(p); if(n <= 0) return 1; p->size += n; /* try and fix in current frag */ if(p->first != nil) { f = p->last; m = f->mem; nn = m->ep - f->wp; if(nn > n) nn = n; if(m->ref == 1 || memTail(m, f->wp, nn)) { memmove(f->wp, buf, nn); f->wp += nn; buf += nn; n -= nn; } } while(n > 0) { nn = n; if(nn > MaxFragSize) nn = MaxFragSize; f = fragAlloc(p, nn, (p->first == nil)?PMiddle:PFront, nil); p->asize += FRAGASIZE(f); if(p->first == nil) p->first = f; else p->last->next = f; p->last = f; memmove(f->rp, buf, nn); buf += nn; n -= nn; } return 1; } int packetConcat(Packet *p, Packet *pp) { NOTFREE(p); NOTFREE(pp); if(pp->size == 0) return 1; p->size += pp->size; p->asize += pp->asize; if(p->first != nil) p->last->next = pp->first; else p->first = pp->first; p->last = pp->last; pp->size = 0; pp->asize = 0; pp->first = nil; pp->last = nil; return 1; } uchar * packetPeek(Packet *p, uchar *buf, int offset, int n) { Frag *f; int nn; uchar *b; NOTFREE(p); if(n == 0) return buf; if(offset < 0 || offset >= p->size) { vtSetError(EPacketOffset); return 0; } if(n < 0 || offset + n > p->size) { vtSetError(EPacketSize); return 0; } /* skip up to offset */ for(f=p->first; offset >= FRAGSIZE(f); f=f->next) offset -= FRAGSIZE(f); /* easy case */ if(offset + n <= FRAGSIZE(f)) return f->rp + offset; for(b=buf; n>0; n -= nn) { nn = FRAGSIZE(f) - offset; if(nn > n) nn = n; memmove(b, f->rp+offset, nn); offset = 0; f = f->next; b += nn; } return buf; } int packetCopy(Packet *p, uchar *buf, int offset, int n) { uchar *b; NOTFREE(p); b = packetPeek(p, buf, offset, n); if(b == nil) return 0; if(b != buf) memmove(buf, b, n); return 1; } int packetFragments(Packet *p, IOchunk *io, int nio, int offset) { Frag *f; int size; IOchunk *eio; NOTFREE(p); if(p->size == 0 || nio <= 0) return 0; if(offset < 0 || offset > p->size) { vtSetError(EPacketOffset); return -1; } for(f=p->first; offset >= FRAGSIZE(f); f=f->next) offset -= FRAGSIZE(f); size = 0; eio = io + nio; for(; f != nil && io < eio; f=f->next) { io->addr = f->rp + offset; io->len = f->wp - (f->rp + offset);