sys/src/liboventi/packet.c

   1 #include <u.h>
   2 #include <libc.h>
   3 #include <oventi.h>
   4 #include "packet.h"
   5
   6 static Frag *fragAlloc(Packet*, int n, int pos, Frag *next);
   7 static Frag *fragDup(Packet*, Frag*);
   8 static void fragFree(Frag*);
   9
  10 static Mem *memAlloc(int, int);
  11 static void memFree(Mem*);
  12 static int memHead(Mem *m, uchar *rp, int n);
  13 static int memTail(Mem *m, uchar *wp, int n);
  14
  15 static char EPacketSize[] = "bad packet size";
  16 static char EPacketOffset[] = "bad packet offset";
  17 static char EBadSize[] = "bad size";
  18
  19 static struct {
  20         Lock lk;
  21         Packet *packet;
  22         int npacket;
  23         Frag *frag;
  24         int nfrag;
  25         Mem *bigMem;
  26         int nbigMem;
  27         Mem *smallMem;
  28         int nsmallMem;
  29 } freeList;
  30
  31 #define FRAGSIZE(f) ((f)->wp - (f)->rp)
  32 #define FRAGASIZE(f) ((f)->mem->ep - (f)->mem->bp)
  33
  34 #define NOTFREE(p) assert((p)->size>=0)
  35
  36 Packet *
  37 packetAlloc(void)
  38 {
  39         Packet *p;
  40
  41         lock(&freeList.lk);
  42         p = freeList.packet;
  43         if(p != nil)
  44                 freeList.packet = p->next;
  45         else
  46                 freeList.npacket++;
  47         unlock(&freeList.lk);
  48
  49         if(p == nil)
  50                 p = vtMemBrk(sizeof(Packet));
  51         else
  52                 assert(p->size == -1);
  53         p->size = 0;
  54         p->asize = 0;
  55         p->first = nil;
  56         p->last = nil;
  57         p->next = nil;
  58
  59         return p;
  60 }
  61
  62 void
  63 packetFree(Packet *p)
  64 {
  65         Frag *f, *ff;
  66
  67 if(0)fprint(2, "packetFree %p\n", p);
  68
  69         NOTFREE(p);
  70         p->size = -1;
  71
  72         for(f=p->first; f!=nil; f=ff) {
  73                 ff = f->next;
  74                 fragFree(f);
  75         }
  76         p->first = nil;
  77         p->last = nil;
  78
  79         lock(&freeList.lk);
  80         p->next = freeList.packet;
  81         freeList.packet = p;
  82         unlock(&freeList.lk);
  83 }
  84
  85 Packet *
  86 packetDup(Packet *p, int offset, int n)
  87 {
  88         Frag *f, *ff;
  89         Packet *pp;
  90
  91         NOTFREE(p);
  92         if(offset < 0 || n < 0 || offset+n > p->size) {
  93                 vtSetError(EBadSize);
  94                 return nil;
  95         }
  96
  97         pp = packetAlloc();
  98         if(n == 0)
  99                 return pp;
 100
 101         pp->size = n;
 102
 103         /* skip offset */
 104         for(f=p->first; offset >= FRAGSIZE(f); f=f->next)
 105                 offset -= FRAGSIZE(f);
 106
 107         /* first frag */
 108         ff = fragDup(pp, f);
 109         ff->rp += offset;
 110         pp->first = ff;
 111         n -= FRAGSIZE(ff);
 112         pp->asize += FRAGASIZE(ff);
 113
 114         /* the remaining */
 115         while(n > 0) {
 116                 f = f->next;
 117                 ff->next = fragDup(pp, f);
 118                 ff = ff->next;
 119                 n -= FRAGSIZE(ff);
 120                 pp->asize += FRAGASIZE(ff);
 121         }
 122
 123         /* fix up last frag: note n <= 0 */
 124         ff->wp += n;
 125         ff->next = nil;
 126         pp->last = ff;
 127
 128         return pp;
 129 }
 130
 131 Packet *
 132 packetSplit(Packet *p, int n)
 133 {
 134         Packet *pp;
 135         Frag *f, *ff;
 136
 137         NOTFREE(p);
 138         if(n < 0 || n > p->size) {
 139                 vtSetError(EPacketSize);
 140                 return nil;
 141         }
 142
 143         pp = packetAlloc();
 144         if(n == 0)
 145                 return pp;
 146
 147         pp->size = n;
 148         p->size -= n;
 149         ff = nil;
 150         for(f=p->first; n > 0 && n >= FRAGSIZE(f); f=f->next) {
 151                 n -= FRAGSIZE(f);
 152                 p->asize -= FRAGASIZE(f);
 153                 pp->asize += FRAGASIZE(f);
 154                 ff = f;
 155         }
 156
 157         /* split shared frag */
 158         if(n > 0) {
 159                 ff = f;
 160                 f = fragDup(pp, ff);
 161                 pp->asize += FRAGASIZE(ff);
 162                 ff->next = nil;
 163                 ff->wp = ff->rp + n;
 164                 f->rp += n;
 165         }
 166
 167         pp->first = p->first;
 168         pp->last = ff;
 169         p->first = f;
 170         return pp;
 171 }
 172
 173 int
 174 packetConsume(Packet *p, uchar *buf, int n)
 175 {
 176         NOTFREE(p);
 177         if(buf && !packetCopy(p, buf, 0, n))
 178                 return 0;
 179         return packetTrim(p, n, p->size-n);
 180 }
 181
 182 int
 183 packetTrim(Packet *p, int offset, int n)
 184 {
 185         Frag *f, *ff;
 186
 187         NOTFREE(p);
 188         if(offset < 0 || offset > p->size) {
 189                 vtSetError(EPacketOffset);
 190                 return 0;
 191         }
 192
 193         if(n < 0 || offset + n > p->size) {
 194                 vtSetError(EPacketOffset);
 195                 return 0;
 196         }
 197
 198         p->size = n;
 199
 200         /* easy case */
 201         if(n == 0) {
 202                 for(f=p->first; f != nil; f=ff) {
 203                         ff = f->next;
 204                         fragFree(f);
 205                 }
 206                 p->first = p->last = nil;
 207                 p->asize = 0;
 208                 return 1;
 209         }
 210
 211         /* free before offset */
 212         for(f=p->first; offset >= FRAGSIZE(f); f=ff) {
 213                 p->asize -= FRAGASIZE(f);
 214                 offset -= FRAGSIZE(f);
 215                 ff = f->next;
 216                 fragFree(f);
 217         }
 218
 219         /* adjust frag */
 220         f->rp += offset;
 221         p->first = f;
 222
 223         /* skip middle */
 224         for(; n > 0 && n > FRAGSIZE(f); f=f->next)
 225                 n -= FRAGSIZE(f);
 226
 227         /* adjust end */
 228         f->wp = f->rp + n;
 229         p->last = f;
 230         ff = f->next;
 231         f->next = nil;
 232
 233         /* free after */
 234         for(f=ff; f != nil; f=ff) {
 235                 p->asize -= FRAGASIZE(f);
 236                 ff = f->next;
 237                 fragFree(f);
 238         }
 239         return 1;
 240 }
 241
 242 uchar *
 243 packetHeader(Packet *p, int n)
 244 {
 245         Frag *f;
 246         Mem *m;
 247
 248         NOTFREE(p);
 249         if(n <= 0 || n > MaxFragSize) {
 250                 vtSetError(EPacketSize);
 251                 return 0;
 252         }
 253
 254         p->size += n;
 255
 256         /* try and fix in current frag */
 257         f = p->first;
 258         if(f != nil) {
 259                 m = f->mem;
 260                 if(n <= f->rp - m->bp)
 261                 if(m->ref == 1 || memHead(m, f->rp, n)) {
 262                         f->rp -= n;
 263                         return f->rp;
 264                 }
 265         }
 266
 267         /* add frag to front */
 268         f = fragAlloc(p, n, PEnd, p->first);
 269         p->asize += FRAGASIZE(f);
 270         if(p->first == nil)
 271                 p->last = f;
 272         p->first = f;
 273         return f->rp;
 274 }
 275
 276 uchar *
 277 packetTrailer(Packet *p, int n)
 278 {
 279         Mem *m;
 280         Frag *f;
 281
 282         NOTFREE(p);
 283         if(n <= 0 || n > MaxFragSize) {
 284                 vtSetError(EPacketSize);
 285                 return 0;
 286         }
 287
 288         p->size += n;
 289
 290         /* try and fix in current frag */
 291         if(p->first != nil) {
 292                 f = p->last;
 293                 m = f->mem;
 294                 if(n <= m->ep - f->wp)
 295                 if(m->ref == 1 || memTail(m, f->wp, n)) {
 296                         f->wp += n;
 297                         return f->wp - n;
 298                 }
 299         }
 300
 301         /* add frag to end */
 302         f = fragAlloc(p, n, (p->first == nil)?PMiddle:PFront, nil);
 303         p->asize += FRAGASIZE(f);
 304         if(p->first == nil)
 305                 p->first = f;
 306         else
 307                 p->last->next = f;
 308         p->last = f;
 309         return f->rp;
 310 }
 311
 312 int
 313 packetPrefix(Packet *p, uchar *buf, int n)
 314 {
 315         Frag *f;
 316         int nn;
 317         Mem *m;
 318
 319         NOTFREE(p);
 320         if(n <= 0)
 321                 return 1;
 322
 323         p->size += n;
 324
 325         /* try and fix in current frag */
 326         f = p->first;
 327         if(f != nil) {
 328                 m = f->mem;
 329                 nn = f->rp - m->bp;
 330                 if(nn > n)
 331                         nn = n;
 332                 if(m->ref == 1 || memHead(m, f->rp, nn)) {
 333                         f->rp -= nn;
 334                         n -= nn;
 335                         memmove(f->rp, buf+n, nn);
 336                 }
 337         }
 338
 339         while(n > 0) {
 340                 nn = n;
 341                 if(nn > MaxFragSize)
 342                         nn = MaxFragSize;
 343                 f = fragAlloc(p, nn, PEnd, p->first);
 344                 p->asize += FRAGASIZE(f);
 345                 if(p->first == nil)
 346                         p->last = f;
 347                 p->first = f;
 348                 n -= nn;
 349                 memmove(f->rp, buf+n, nn);
 350         }
 351         return 1;
 352 }
 353
 354 int
 355 packetAppend(Packet *p, uchar *buf, int n)
 356 {
 357         Frag *f;
 358         int nn;
 359         Mem *m;
 360
 361         NOTFREE(p);
 362         if(n <= 0)
 363                 return 1;
 364
 365         p->size += n;
 366         /* try and fix in current frag */
 367         if(p->first != nil) {
 368                 f = p->last;
 369                 m = f->mem;
 370                 nn = m->ep - f->wp;
 371                 if(nn > n)
 372                         nn = n;
 373                 if(m->ref == 1 || memTail(m, f->wp, nn)) {
 374                         memmove(f->wp, buf, nn);
 375                         f->wp += nn;
 376                         buf += nn;
 377                         n -= nn;
 378                 }
 379         }
 380
 381         while(n > 0) {
 382                 nn = n;
 383                 if(nn > MaxFragSize)
 384                         nn = MaxFragSize;
 385                 f = fragAlloc(p, nn, (p->first == nil)?PMiddle:PFront, nil);
 386                 p->asize += FRAGASIZE(f);
 387                 if(p->first == nil)
 388                         p->first = f;
 389                 else
 390                         p->last->next = f;
 391                 p->last = f;
 392                 memmove(f->rp, buf, nn);
 393                 buf += nn;
 394                 n -= nn;
 395         }
 396         return 1;
 397 }
 398
 399 int
 400 packetConcat(Packet *p, Packet *pp)
 401 {
 402         NOTFREE(p);
 403         NOTFREE(pp);
 404         if(pp->size == 0)
 405                 return 1;
 406         p->size += pp->size;
 407         p->asize += pp->asize;
 408
 409         if(p->first != nil)
 410                 p->last->next = pp->first;
 411         else
 412                 p->first = pp->first;
 413         p->last = pp->last;
 414         pp->size = 0;
 415         pp->asize = 0;
 416         pp->first = nil;
 417         pp->last = nil;
 418         return 1;
 419 }
 420
 421 uchar *
 422 packetPeek(Packet *p, uchar *buf, int offset, int n)
 423 {
 424         Frag *f;
 425         int nn;
 426         uchar *b;
 427
 428         NOTFREE(p);
 429         if(n == 0)
 430                 return buf;
 431
 432         if(offset < 0 || offset >= p->size) {
 433                 vtSetError(EPacketOffset);
 434                 return 0;
 435         }
 436
 437         if(n < 0 || offset + n > p->size) {
 438                 vtSetError(EPacketSize);
 439                 return 0;
 440         }
 441
 442         /* skip up to offset */
 443         for(f=p->first; offset >= FRAGSIZE(f); f=f->next)
 444                 offset -= FRAGSIZE(f);
 445
 446         /* easy case */
 447         if(offset + n <= FRAGSIZE(f))
 448                 return f->rp + offset;
 449
 450         for(b=buf; n>0; n -= nn) {
 451                 nn = FRAGSIZE(f) - offset;
 452                 if(nn > n)
 453                         nn = n;
 454                 memmove(b, f->rp+offset, nn);
 455                 offset = 0;
 456                 f = f->next;
 457                 b += nn;
 458         }
 459
 460         return buf;
 461 }
 462
 463 int
 464 packetCopy(Packet *p, uchar *buf, int offset, int n)
 465 {
 466         uchar *b;
 467
 468         NOTFREE(p);
 469         b = packetPeek(p, buf, offset, n);
 470         if(b == nil)
 471                 return 0;
 472         if(b != buf)
 473                 memmove(buf, b, n);
 474         return 1;
 475 }
 476
 477 int
 478 packetFragments(Packet *p, IOchunk *io, int nio, int offset)
 479 {
 480         Frag *f;
 481         int size;
 482         IOchunk *eio;
 483
 484         NOTFREE(p);
 485         if(p->size == 0 || nio <= 0)
 486                 return 0;
 487
 488         if(offset < 0 || offset > p->size) {
 489                 vtSetError(EPacketOffset);
 490                 return -1;
 491         }
 492
 493         for(f=p->first; offset >= FRAGSIZE(f); f=f->next)
 494                 offset -= FRAGSIZE(f);
 495
 496         size = 0;
 497         eio = io + nio;
 498         for(; f != nil && io < eio; f=f->next) {
 499                 io->addr = f->rp + offset;
 500                 io->len = f->wp - (f->rp + offset);
 501                 offset = 0;
 502                 size += io->len;
 503                 io++;
 504         }
 505
 506         return size;
 507 }
 508
 509 void
 510 packetStats(void)
 511 {
 512         Packet *p;
 513         Frag *f;
 514         Mem *m;
 515
 516         int np, nf, nsm, nbm;
 517
 518         lock(&freeList.lk);
 519         np = 0;
 520         for(p=freeList.packet; p; p=p->next)
 521                 np++;
 522         nf = 0;
 523         for(f=freeList.frag; f; f=f->next)
 524                 nf++;
 525         nsm = 0;
 526         for(m=freeList.smallMem; m; m=m->next)
 527                 nsm++;
 528         nbm = 0;
 529         for(m=freeList.bigMem; m; m=m->next)
 530                 nbm++;
 531
 532         fprint(2, "packet: %d/%d frag: %d/%d small mem: %d/%d big mem: %d/%d\n",
 533                 np, freeList.npacket,
 534                 nf, freeList.nfrag,
 535                 nsm, freeList.nsmallMem,
 536                 nbm, freeList.nbigMem);
 537
 538         unlock(&freeList.lk);
 539 }
 540
 541
 542 int
 543 packetSize(Packet *p)
 544 {
 545         NOTFREE(p);
 546         if(0) {
 547                 Frag *f;
 548                 int size = 0;
 549
 550                 for(f=p->first; f; f=f->next)
 551                         size += FRAGSIZE(f);
 552                 if(size != p->size)
 553                         fprint(2, "packetSize %d %d\n", size, p->size);
 554                 assert(size == p->size);
 555         }
 556         return p->size;
 557 }
 558
 559 int
 560 packetAllocatedSize(Packet *p)
 561 {
 562         NOTFREE(p);
 563         if(0) {
 564                 Frag *f;
 565                 int asize = 0;
 566
 567                 for(f=p->first; f; f=f->next)
 568                         asize += FRAGASIZE(f);
 569                 if(asize != p->asize)
 570                         fprint(2, "packetAllocatedSize %d %d\n", asize, p->asize);
 571                 assert(asize == p->asize);
 572         }
 573         return p->asize;
 574 }
 575
 576 void
 577 packetSha1(Packet *p, uchar sha1[VtScoreSize])
 578 {
 579         Frag *f;
 580         VtSha1 *s;
 581         int size;
 582
 583         NOTFREE(p);
 584         s = vtSha1Alloc();
 585         size = p->size;
 586         for(f=p->first; f; f=f->next) {
 587                 vtSha1Update(s, f->rp, FRAGSIZE(f));
 588                 size -= FRAGSIZE(f);
 589         }
 590         assert(size == 0);
 591         vtSha1Final(s, sha1);
 592         vtSha1Free(s);
 593 }
 594
 595 int
 596 packetCmp(Packet *pkt0, Packet *pkt1)
 597 {
 598         Frag *f0, *f1;
 599         int n0, n1, x;
 600
 601         NOTFREE(pkt0);
 602         NOTFREE(pkt1);
 603         f0 = pkt0->first;
 604         f1 = pkt1->first;
 605
 606         if(f0 == nil)
 607                 return (f1 == nil)?0:-1;
 608         if(f1 == nil)
 609                 return 1;
 610         n0 = FRAGSIZE(f0);
 611         n1 = FRAGSIZE(f1);
 612
 613         for(;;) {
 614                 if(n0 < n1) {
 615                         x = memcmp(f0->wp - n0, f1->wp - n1, n0);
 616                         if(x != 0)
 617                                 return x;
 618                         n1 -= n0;
 619                         f0 = f0->next;
 620                         if(f0 == nil)
 621                                 return -1;
 622                         n0 = FRAGSIZE(f0);
 623                 } else if (n0 > n1) {
 624                         x = memcmp(f0->wp - n0, f1->wp - n1, n1);
 625                         if(x != 0)
 626                                 return x;
 627                         n0 -= n1;
 628                         f1 = f1->next;
 629                         if(f1 == nil)
 630                                 return 1;
 631                         n1 = FRAGSIZE(f1);
 632                 } else { /* n0 == n1 */
 633                         x = memcmp(f0->wp - n0, f1->wp - n1, n0);
 634                         if(x != 0)
 635                                 return x;
 636                         f0 = f0->next;
 637                         f1 = f1->next;
 638                         if(f0 == nil)
 639                                 return (f1 == nil)?0:-1;
 640                         if(f1 == nil)
 641                                 return 1;
 642                         n0 = FRAGSIZE(f0);
 643                         n1 = FRAGSIZE(f1);
 644                 }
 645         }
 646 }
 647
 648
 649 static Frag *
 650 fragAlloc(Packet *p, int n, int pos, Frag *next)
 651 {
 652         Frag *f, *ef;
 653         Mem *m;
 654
 655         /* look for local frag */
 656         f = &p->local[0];
 657         ef = &p->local[NLocalFrag];
 658         for(;f<ef; f++) {
 659                 if(f->state == FragLocalFree) {
 660                         f->state = FragLocalAlloc;
 661                         goto Found;
 662                 }
 663         }
 664         lock(&freeList.lk);
 665         f = freeList.frag;
 666         if(f != nil)
 667                 freeList.frag = f->next;
 668         else
 669                 freeList.nfrag++;
 670         unlock(&freeList.lk);
 671
 672         if(f == nil) {
 673                 f = vtMemBrk(sizeof(Frag));
 674                 f->state = FragGlobal;
 675         }
 676
 677 Found:
 678         if(n == 0)
 679                 return f;
 680
 681         if(pos == PEnd && next == nil)
 682                 pos = PMiddle;
 683         m = memAlloc(n, pos);
 684         f->mem = m;
 685         f->rp = m->rp;
 686         f->wp = m->wp;
 687         f->next = next;
 688
 689         return f;
 690 }
 691
 692 static Frag *
 693 fragDup(Packet *p, Frag *f)
 694 {
 695         Frag *ff;
 696         Mem *m;
 697
 698         m = f->mem;
 699
 700         /*
 701          * m->rp && m->wp can be out of date when ref == 1
 702          * also, potentially reclaims space from previous frags
 703          */
 704         if(m->ref == 1) {
 705                 m->rp = f->rp;
 706                 m->wp = f->wp;
 707         }
 708
 709         ff = fragAlloc(p, 0, 0, nil);
 710         *ff = *f;
 711         lock(&m->lk);
 712         m->ref++;
 713         unlock(&m->lk);
 714         return ff;
 715 }
 716
 717
 718 static void
 719 fragFree(Frag *f)
 720 {
 721         memFree(f->mem);
 722
 723         if(f->state == FragLocalAlloc) {
 724                 f->state = FragLocalFree;
 725                 return;
 726         }
 727
 728         lock(&freeList.lk);
 729         f->next = freeList.frag;
 730         freeList.frag = f;
 731         unlock(&freeList.lk);
 732 }
 733
 734 static Mem *
 735 memAlloc(int n, int pos)
 736 {
 737         Mem *m;
 738         int nn;
 739
 740         if(n < 0 || n > MaxFragSize) {
 741                 vtSetError(EPacketSize);
 742                 return 0;
 743         }
 744         if(n <= SmallMemSize) {
 745                 lock(&freeList.lk);
 746                 m = freeList.smallMem;
 747                 if(m != nil)
 748                         freeList.smallMem = m->next;
 749                 else
 750                         freeList.nsmallMem++;
 751                 unlock(&freeList.lk);
 752                 nn = SmallMemSize;
 753         } else {
 754                 lock(&freeList.lk);
 755                 m = freeList.bigMem;
 756                 if(m != nil)
 757                         freeList.bigMem = m->next;
 758                 else
 759                         freeList.nbigMem++;
 760                 unlock(&freeList.lk);
 761                 nn = BigMemSize;
 762         }
 763
 764         if(m == nil) {
 765                 m = vtMemBrk(sizeof(Mem));
 766                 m->bp = vtMemBrk(nn);
 767                 m->ep = m->bp + nn;
 768         }
 769         assert(m->ref == 0);
 770         m->ref = 1;
 771
 772         switch(pos) {
 773         default:
 774                 assert(0);
 775         case PFront:
 776                 m->rp = m->bp;
 777                 break;
 778         case PMiddle:
 779                 /* leave a little bit at end */
 780                 m->rp = m->ep - n - 32;
 781                 break;
 782         case PEnd:
 783                 m->rp = m->ep - n;
 784                 break;
 785         }
 786         /* check we did not blow it */
 787         if(m->rp < m->bp)
 788                 m->rp = m->bp;
 789         m->wp = m->rp + n;
 790         assert(m->rp >= m->bp && m->wp <= m->ep);
 791         return m;
 792 }
 793
 794 static void
 795 memFree(Mem *m)
 796 {
 797         lock(&m->lk);
 798         m->ref--;
 799         if(m->ref > 0) {
 800                 unlock(&m->lk);
 801                 return;
 802         }
 803         unlock(&m->lk);
 804         assert(m->ref == 0);
 805
 806         switch(m->ep - m->bp) {
 807         default:
 808                 assert(0);
 809         case SmallMemSize:
 810                 lock(&freeList.lk);
 811                 m->next = freeList.smallMem;
 812                 freeList.smallMem = m;
 813                 unlock(&freeList.lk);
 814                 break;
 815         case BigMemSize:
 816                 lock(&freeList.lk);
 817                 m->next = freeList.bigMem;
 818                 freeList.bigMem = m;
 819                 unlock(&freeList.lk);
 820                 break;
 821         }
 822 }
 823
 824 static int
 825 memHead(Mem *m, uchar *rp, int n)
 826 {
 827         lock(&m->lk);
 828         if(m->rp != rp) {
 829                 unlock(&m->lk);
 830                 return 0;
 831         }
 832         m->rp -= n;
 833         unlock(&m->lk);
 834         return 1;
 835 }
 836
 837 static int
 838 memTail(Mem *m, uchar *wp, int n)
 839 {
 840         lock(&m->lk);
 841         if(m->wp != wp) {
 842                 unlock(&m->lk);
 843                 return 0;
 844         }
 845         m->wp += n;
 846         unlock(&m->lk);
 847         return 1;
 848 }