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