home *** CD-ROM | disk | FTP | other *** search
/ Media Share 9 / MEDIASHARE_09.ISO / hamradio / s920603.zip / MBUF.C < prev    next >
C/C++ Source or Header  |  1992-05-01  |  13KB  |  596 lines
  1. /* mbuf (message buffer) primitives
  2.  * Copyright 1991 Phil Karn, KA9Q
  3.  */
  4. #include <stdio.h>
  5. #include <dos.h>    /* TEMP */
  6. #include "global.h"
  7. #include "mbuf.h"
  8. #include "proc.h"
  9.  
  10. /* Interrupt buffer pool */
  11. int Intqlen;            /* Number of free mbufs on Intq */
  12. struct mbuf *Intq;        /* Mbuf pool for interrupt handlers */
  13. struct mbuf *Garbq;        /* List of buffers freed at interrupt time */
  14. long Ibuffail;            /* Allocate failures */
  15. long Pushdowns;            /* Total calls to pushdown() */
  16. long Pushalloc;            /* Calls to pushalloc() that call malloc */
  17.  
  18. void
  19. refiq()
  20. {
  21.     register struct mbuf *bp;
  22.     char i_state;
  23.     int32 dma_abs;    /* TEMP */
  24.     int16 dma_page;    /* TEMP */
  25.  
  26.     /* Empty the garbage */
  27.     if(Garbq != NULLBUF){
  28.         i_state = dirps();
  29.         bp = Garbq;
  30.         Garbq = NULLBUF;
  31.         restore(i_state);
  32.         free_p(bp);
  33.     }
  34.     /* Replenish interrupt buffer pool */
  35.     while(Intqlen < Nibufs){
  36.         if((bp = alloc_mbuf(Ibufsize)) == NULLBUF)
  37.             break;
  38. #ifndef    notdef        /* Temp hack to satisfy PI DMA requirements */
  39.         dma_abs = ((long)FP_SEG(bp->data) << 4) + (long)FP_OFF(bp->data);
  40.         dma_page = dma_abs >> 16;
  41.         if(((dma_abs+Ibufsize) >> 16) != dma_page){
  42.             i_state = dirps();
  43.             bp->next = Garbq;
  44.             Garbq = bp;
  45.             restore(i_state);
  46.             continue;
  47.         }
  48. #endif
  49.  
  50.         i_state = dirps();
  51.         bp->next = Intq;
  52.         Intq = bp;
  53.         Intqlen++;
  54.         restore(i_state);
  55.  
  56.     }
  57. }
  58.  
  59. void
  60. iqstat()
  61. {
  62.     printf("Intqlen %u Ibufsize %u Ibuffail %lu\n",
  63.         Intqlen,Ibufsize,Ibuffail);
  64. }
  65. /* Allocate mbuf with associated buffer of 'size' bytes. If interrupts
  66.  * are enabled, use the regular heap. If they're off, use the special
  67.  * interrupt buffer pool.
  68.  */
  69. struct mbuf *
  70. alloc_mbuf(size)
  71. register int16 size;
  72. {
  73.     register struct mbuf *bp;
  74.  
  75.     if(istate()){
  76.         /* Interrupts are enabled, use the heap normally */
  77.         bp = (struct mbuf *)malloc((unsigned)(size + sizeof(struct mbuf)));
  78.         if(bp == NULLBUF)
  79.             return NULLBUF;
  80.         /* Clear just the header portion */
  81.         memset((char *)bp,0,sizeof(struct mbuf));
  82.         if((bp->size = size) != 0)
  83.             bp->data = (char *)(bp + 1);
  84.         bp->refcnt++;
  85.     } else {
  86.         /* Interrupts are off, use special interrupt buffer pool */
  87.         if(size > Ibufsize || Intq == NULLBUF){
  88.             Ibuffail++;
  89.             return NULLBUF;
  90.         }
  91.         bp = Intq;
  92.         Intq = bp->next;
  93.         bp->next = NULLBUF;
  94.         Intqlen--;
  95.     }
  96.     return bp;
  97. }
  98. /* Allocate mbuf, waiting if memory is unavailable */
  99. struct mbuf *
  100. ambufw(size)
  101. int16 size;
  102. {
  103.     register struct mbuf *bp;
  104.  
  105.     bp = (struct mbuf *)mallocw((unsigned)(size + sizeof(struct mbuf)));
  106.  
  107.     /* Clear just the header portion */
  108.     memset((char *)bp,0,sizeof(struct mbuf));
  109.     if((bp->size = size) != 0)
  110.         bp->data = (char *)(bp + 1);
  111.     bp->refcnt++;
  112.     return bp;
  113. }
  114.  
  115. /* Decrement the reference pointer in an mbuf. If it goes to zero,
  116.  * free all resources associated with mbuf.
  117.  * Return pointer to next mbuf in packet chain
  118.  */
  119. struct mbuf *
  120. free_mbuf(bp)
  121. register struct mbuf *bp;
  122. {
  123.     struct mbuf *bpnext;
  124.  
  125.     if(bp == NULLBUF)
  126.         return NULLBUF;
  127.  
  128.     bpnext = bp->next;
  129.     if(bp->dup != NULLBUF){
  130.         free_mbuf(bp->dup);    /* Follow indirection */
  131.         bp->dup = NULLBUF;
  132.     }
  133.     /* Decrement reference count. If it has gone to zero, free it. */
  134.     if(--bp->refcnt <= 0){
  135.         if(istate()){
  136.             free((char *)bp);
  137.         } else {
  138.             /* If the interrupt pool isn't full and this buffer
  139.              * appears to have come from it, put it back.
  140.              * Otherwise put it on the garbage list where it
  141.              * will be freed by refiq() later with interrupts
  142.              * enabled.
  143.              *
  144.              * This test handles the common special case of
  145.              * an interrupt handler allocating a buffer and
  146.              * then freeing it before returning (e.g., due to
  147.              * a receive abort or CRC failure).
  148.              */
  149.             bp->refcnt = 1;    /* Adjust */
  150.             if(bp->size == Ibufsize && Intqlen < Nibufs){
  151.                 bp->next = Intq;
  152.                 bp->anext = NULLBUF;
  153.                 bp->data = (char *)(bp + 1);
  154.                 bp->cnt = 0;
  155.                 Intq = bp;
  156.                 Intqlen++;
  157.             } else {
  158.                 bp->next = Garbq;
  159.                 Garbq = bp;
  160.             }
  161.         }
  162.     }
  163.     return bpnext;
  164. }
  165.  
  166. /* Free packet (a chain of mbufs). Return pointer to next packet on queue,
  167.  * if any
  168.  */
  169. struct mbuf *
  170. free_p(bp)
  171. register struct mbuf *bp;
  172. {
  173.     register struct mbuf *abp;
  174.  
  175.     if(bp == NULLBUF)
  176.         return NULLBUF;
  177.     abp = bp->anext;
  178.     while(bp != NULLBUF)
  179.         bp = free_mbuf(bp);
  180.     return abp;
  181. }        
  182. /* Free entire queue of packets (of mbufs) */
  183. void
  184. free_q(q)
  185. struct mbuf **q;
  186. {
  187.     register struct mbuf *bp;
  188.  
  189.     while((bp = dequeue(q)) != NULLBUF)
  190.         free_p(bp);
  191. }
  192.  
  193. /* Count up the total number of bytes in a packet */
  194. int16
  195. len_p(bp)
  196. register struct mbuf *bp;
  197. {
  198.     register int16 cnt = 0;
  199.  
  200.     while(bp != NULLBUF){
  201.         cnt += bp->cnt;
  202.         bp = bp->next;
  203.     }
  204.     return cnt;
  205. }
  206. /* Count up the number of packets in a queue */
  207. int16
  208. len_q(bp)
  209. register struct mbuf *bp;
  210. {
  211.     register int16 cnt;
  212.  
  213.     for(cnt=0;bp != NULLBUF;cnt++,bp = bp->anext)
  214.         ;
  215.     return cnt;
  216. }
  217. /* Trim mbuf to specified length by lopping off end */
  218. void
  219. trim_mbuf(bpp,length)
  220. struct mbuf **bpp;
  221. int16 length;
  222. {
  223.     register int16 tot = 0;
  224.     register struct mbuf *bp;
  225.  
  226.     if(bpp == NULLBUFP || *bpp == NULLBUF)
  227.         return;    /* Nothing to trim */
  228.  
  229.     if(length == 0){
  230.         /* Toss the whole thing */
  231.         free_p(*bpp);
  232.         *bpp = NULLBUF;
  233.         return;
  234.     }
  235.     /* Find the point at which to trim. If length is greater than
  236.      * the packet, we'll just fall through without doing anything
  237.      */
  238.     for( bp = *bpp; bp != NULLBUF; bp = bp->next){
  239.         if(tot + bp->cnt < length){
  240.             tot += bp->cnt;
  241.         } else {
  242.             /* Cut here */
  243.             bp->cnt = length - tot;
  244.             free_p(bp->next);
  245.             bp->next = NULLBUF;
  246.             break;
  247.         }
  248.     }
  249. }
  250. /* Duplicate/enqueue/dequeue operations based on mbufs */
  251.  
  252. /* Duplicate first 'cnt' bytes of packet starting at 'offset'.
  253.  * This is done without copying data; only the headers are duplicated,
  254.  * but without data segments of their own. The pointers are set up to
  255.  * share the data segments of the original copy. The return pointer is
  256.  * passed back through the first argument, and the return value is the
  257.  * number of bytes actually duplicated.
  258.  */
  259. int16
  260. dup_p(hp,bp,offset,cnt)
  261. struct mbuf **hp;
  262. register struct mbuf *bp;
  263. register int16 offset;
  264. register int16 cnt;
  265. {
  266.     register struct mbuf *cp;
  267.     int16 tot;
  268.  
  269.     if(cnt == 0 || bp == NULLBUF || hp == NULLBUFP){
  270.         if(hp != NULLBUFP)
  271.             *hp = NULLBUF;
  272.         return 0;
  273.     }
  274.     if((*hp = cp = alloc_mbuf(0)) == NULLBUF){
  275.         return 0;
  276.     }
  277.     /* Skip over leading mbufs that are smaller than the offset */
  278.     while(bp != NULLBUF && bp->cnt <= offset){
  279.         offset -= bp->cnt;
  280.         bp = bp->next;
  281.     }
  282.     if(bp == NULLBUF){
  283.         free_mbuf(cp);
  284.         *hp = NULLBUF;
  285.         return 0;    /* Offset was too big */
  286.     }
  287.     tot = 0;
  288.     for(;;){
  289.         /* Make sure we get the original, "real" buffer (i.e. handle the
  290.          * case of duping a dupe)
  291.          */
  292.         if(bp->dup != NULLBUF)
  293.             cp->dup = bp->dup;
  294.         else
  295.             cp->dup = bp;
  296.  
  297.         /* Increment the duplicated buffer's reference count */
  298.         cp->dup->refcnt++;
  299.  
  300.         cp->data = bp->data + offset;
  301.         cp->cnt = min(cnt,bp->cnt - offset);
  302.         offset = 0;
  303.         cnt -= cp->cnt;
  304.         tot += cp->cnt;
  305.         bp = bp->next;
  306.         if(cnt == 0 || bp == NULLBUF || (cp->next = alloc_mbuf(0)) == NULLBUF)
  307.             break;
  308.         cp = cp->next;
  309.     }
  310.     return tot;
  311. }
  312. /* Copy first 'cnt' bytes of packet into a new, single mbuf */
  313. struct mbuf *
  314. copy_p(bp,cnt)
  315. register struct mbuf *bp;
  316. register int16 cnt;
  317. {
  318.     register struct mbuf *cp;
  319.     register char *wp;
  320.     register int16 n;
  321.  
  322.     if(bp == NULLBUF || cnt == 0 || (cp = alloc_mbuf(cnt)) == NULLBUF)
  323.         return NULLBUF;
  324.     wp = cp->data;
  325.     while(cnt != 0 && bp != NULLBUF){
  326.         n = min(cnt,bp->cnt);
  327.         memcpy(wp,bp->data,n);
  328.         wp += n;
  329.         cp->cnt += n;
  330.         cnt -= n;
  331.         bp = bp->next;
  332.     }
  333.     return cp;
  334. }
  335. /* Copy and delete "cnt" bytes from beginning of packet. Return number of
  336.  * bytes actually pulled off
  337.  */
  338. int16
  339. pullup(bph,buf,cnt)
  340. struct mbuf **bph;
  341. char *buf;
  342. int16 cnt;
  343. {
  344.     register struct mbuf *bp;
  345.     int16 n,tot;
  346.  
  347.     tot = 0;
  348.     if(bph == NULLBUFP)
  349.         return 0;
  350.     while(cnt != 0 && (bp = *bph) != NULLBUF){
  351.         n = min(cnt,bp->cnt);
  352.         if(buf != NULLCHAR){
  353.             if(n == 1)    /* Common case optimization */
  354.                 *buf = *bp->data;
  355.             else if(n > 1)
  356.                 memcpy(buf,bp->data,n);
  357.             buf += n;
  358.         }
  359.         tot += n;
  360.         cnt -= n;
  361.         bp->data += n;
  362.         bp->cnt -= n;        
  363.         if(bp->cnt == 0){
  364.             /* If this is the last mbuf of a packet but there
  365.              * are others on the queue, return a pointer to
  366.              * the next on the queue. This allows pullups to
  367.              * to work on a packet queue
  368.              */
  369.             if(bp->next == NULLBUF && bp->anext != NULLBUF){
  370.                 *bph = bp->anext;
  371.                 free_mbuf(bp);
  372.             } else
  373.                 *bph = free_mbuf(bp);
  374.         }
  375.     }
  376.     return tot;
  377. }
  378. /* Append mbuf to end of mbuf chain */
  379. void
  380. append(bph,bp)
  381. struct mbuf **bph;
  382. struct mbuf *bp;
  383. {
  384.     register struct mbuf *p;
  385.  
  386.     if(bph == NULLBUFP || bp == NULLBUF)
  387.         return;
  388.     if(*bph == NULLBUF){
  389.         /* First one on chain */
  390.         *bph = bp;
  391.     } else {
  392.         for(p = *bph ; p->next != NULLBUF ; p = p->next)
  393.             ;
  394.         p->next = bp;
  395.     }
  396. }
  397. /* Insert specified amount of contiguous new space at the beginning of an
  398.  * mbuf chain. If enough space is available in the first mbuf, no new space
  399.  * is allocated. Otherwise a mbuf of the appropriate size is allocated and
  400.  * tacked on the front of the chain.
  401.  *
  402.  * This operation is the logical inverse of pullup(), hence the name.
  403.  */
  404. struct mbuf *
  405. pushdown(bp,size)
  406. register struct mbuf *bp;
  407. int16 size;
  408. {
  409.     register struct mbuf *nbp;
  410.  
  411.     Pushdowns++;
  412.     /* Check that bp is real, that it hasn't been duplicated, and
  413.      * that it itself isn't a duplicate before checking to see if
  414.      * there's enough space at its front.
  415.      */
  416.     if(bp != NULLBUF && bp->refcnt == 1 && bp->dup == NULLBUF
  417.      && bp->data - (char *)(bp+1) >= size){
  418.         /* No need to alloc new mbuf, just adjust this one */
  419.         bp->data -= size;
  420.         bp->cnt += size;
  421.     } else {
  422.         nbp = ambufw(size);
  423.         nbp->next = bp;
  424.         nbp->cnt = size;
  425.         bp = nbp;
  426.         Pushalloc++;
  427.     }
  428.     return bp;
  429. }
  430. /* Append packet to end of packet queue */
  431. void
  432. enqueue(q,bp)
  433. struct mbuf **q;
  434. struct mbuf *bp;
  435. {
  436.     register struct mbuf *p;
  437.     char i_state;
  438.  
  439.     if(q == NULLBUFP || bp == NULLBUF)
  440.         return;
  441.     i_state = dirps();
  442.     if(*q == NULLBUF){
  443.         /* List is empty, stick at front */
  444.         *q = bp;
  445.     } else {
  446.         for(p = *q ; p->anext != NULLBUF ; p = p->anext)
  447.             ;
  448.         p->anext = bp;
  449.     }
  450.     restore(i_state);
  451.     psignal(q,1);
  452. }
  453. /* Unlink a packet from the head of the queue */
  454. struct mbuf *
  455. dequeue(q)
  456. register struct mbuf **q;
  457. {
  458.     register struct mbuf *bp;
  459.     char i_state;
  460.  
  461.     if(q == NULLBUFP)
  462.         return NULLBUF;
  463.     i_state = dirps();
  464.     if((bp = *q) != NULLBUF){
  465.         *q = bp->anext;
  466.         bp->anext = NULLBUF;
  467.     }
  468.     restore(i_state);
  469.     return bp;
  470. }    
  471.  
  472. /* Copy user data into an mbuf */
  473. struct mbuf *
  474. qdata(data,cnt)
  475. char *data;
  476. int16 cnt;
  477. {
  478.     register struct mbuf *bp;
  479.  
  480.     bp = ambufw(cnt);
  481.     memcpy(bp->data,data,cnt);
  482.     bp->cnt = cnt;
  483.     return bp;
  484. }
  485. /* Copy mbuf data into user buffer */
  486. int16
  487. dqdata(bp,buf,cnt)
  488. struct mbuf *bp;
  489. char *buf;
  490. unsigned cnt;
  491. {
  492.     int16 tot;
  493.     unsigned n;
  494.     struct mbuf *bp1;
  495.  
  496.     if(buf == NULLCHAR)
  497.         return 0;
  498.     
  499.     tot = 0;
  500.     for(bp1 = bp;bp1 != NULLBUF; bp1 = bp1->next){
  501.         n = min(bp1->cnt,cnt);
  502.         memcpy(buf,bp1->data,n);
  503.         cnt -= n;
  504.         buf += n;
  505.         tot += n;
  506.     }
  507.     free_p(bp);
  508.     return tot;
  509. }
  510. /* Pull a 32-bit integer in host order from buffer in network byte order.
  511.  * On error, return 0. Note that this is indistinguishable from a normal
  512.  * return.
  513.  */
  514. int32
  515. pull32(bpp)
  516. struct mbuf **bpp;
  517. {
  518.     char buf[4];
  519.  
  520.     if(pullup(bpp,buf,4) != 4){
  521.         /* Return zero if insufficient buffer */
  522.         return 0;
  523.     }
  524.     return get32(buf);
  525. }
  526. /* Pull a 16-bit integer in host order from buffer in network byte order.
  527.  * Return -1 on error
  528.  */
  529. long
  530. pull16(bpp)
  531. struct mbuf **bpp;
  532. {
  533.     char buf[2];
  534.  
  535.     if(pullup(bpp,buf,2) != 2){
  536.         return -1;        /* Nothing left */
  537.     }
  538.     return get16(buf);
  539. }
  540. /* Pull single character from mbuf */
  541. int
  542. pullchar(bpp)
  543. struct mbuf **bpp;
  544. {
  545.     char c;
  546.  
  547.     if(pullup(bpp,&c,1) != 1)
  548.         return -1;        /* Nothing left */
  549.     return (int)uchar(c);
  550. }
  551. int
  552. write_p(fp,bp)
  553. FILE *fp;
  554. struct mbuf *bp;
  555. {
  556.     while(bp != NULLBUF){
  557.         if(fwrite(bp->data,1,bp->cnt,fp) != bp->cnt)
  558.             return -1;
  559.         bp = bp->next;
  560.     }
  561.     return 0;
  562. }
  563. /* Reclaim unused space in a mbuf chain. If the argument is a chain of mbufs
  564.  * and/or it appears to have wasted space, copy it to a single new mbuf and
  565.  * free the old mbuf(s). But refuse to move mbufs that merely
  566.  * reference other mbufs, or that have other headers referencing them.
  567.  *
  568.  * Be extremely careful that there aren't any other pointers to
  569.  * (or into) this mbuf, since we have no way of detecting them here.
  570.  * This function is meant to be called only when free memory is in
  571.  * short supply.
  572.  */
  573. void
  574. mbuf_crunch(bpp)
  575. struct mbuf **bpp;
  576. {
  577.     register struct mbuf *bp = *bpp;
  578.     struct mbuf *nbp;
  579.  
  580.     if(bp->refcnt > 1 || bp->dup != NULLBUF){
  581.         /* Can't crunch, there are other refs */
  582.         return;
  583.     }
  584.     if(bp->next == NULLBUF && bp->cnt == bp->size){
  585.         /* Nothing to be gained by crunching */
  586.         return;
  587.     }
  588.     if((nbp = copy_p(bp,len_p(bp))) == NULLBUF){
  589.         /* Copy failed due to lack of (contiguous) space */
  590.         return;
  591.     }
  592.     nbp->anext = bp->anext;
  593.     free_p(bp);
  594.     *bpp = nbp;
  595. }
  596.