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 *
packetAlloc(void)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
packetFree(Packet * p)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 *
packetDup(Packet * p,int offset,int n)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 *
packetSplit(Packet * p,int n)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
packetConsume(Packet * p,uchar * buf,int n)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
packetTrim(Packet * p,int offset,int n)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 *
packetHeader(Packet * p,int n)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 *
packetTrailer(Packet * p,int n)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
packetPrefix(Packet * p,uchar * buf,int n)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
packetAppend(Packet * p,uchar * buf,int n)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
packetConcat(Packet * p,Packet * pp)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 *
packetPeek(Packet * p,uchar * buf,int offset,int n)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
packetCopy(Packet * p,uchar * buf,int offset,int n)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
packetFragments(Packet * p,IOchunk * io,int nio,int offset)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
packetStats(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
packetSize(Packet * p)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
packetAllocatedSize(Packet * p)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
packetSha1(Packet * p,uchar sha1[VtScoreSize])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
packetCmp(Packet * pkt0,Packet * pkt1)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 *
fragAlloc(Packet * p,int n,int pos,Frag * next)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 *
fragDup(Packet * p,Frag * f)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
fragFree(Frag * f)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 *
memAlloc(int n,int pos)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
memFree(Mem * m)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
memHead(Mem * m,uchar * rp,int n)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
memTail(Mem * m,uchar * wp,int n)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 }
849