home *** CD-ROM | disk | FTP | other *** search
/ Gold Fish 1 / GoldFishApril1994_CD1.img / d2xx / d225 / amigatcp / src / mbuf.c < prev    next >
C/C++ Source or Header  |  1989-06-24  |  7KB  |  412 lines
  1. /* Primitive mbuf allocate/free routines */
  2.  
  3. #ifdef TRACE
  4. #include <stdio.h>
  5. #endif
  6.  
  7. #include "machdep.h"
  8. #include "mbuf.h"
  9.  
  10. /* Allocate mbuf with associated buffer of 'size' bytes */
  11. struct mbuf *
  12. alloc_mbuf(size)
  13. register int16 size;
  14. {
  15.     register struct mbuf *bp;
  16.     char *malloc();
  17.  
  18.     if((bp = (struct mbuf *)malloc((unsigned)(size + sizeof(struct mbuf)))) == NULLBUF)
  19.         return NULLBUF;
  20.     bp->next = bp->anext = NULLBUF;
  21.     if(size != 0){
  22.         bp->data = (char *)(bp + 1);
  23.     } else {
  24.         bp->data = NULLCHAR;
  25.     }
  26.     bp->cnt = 0;
  27.     return bp;
  28. }
  29.  
  30. /* Free all resources associated with mbuf
  31.  * Return pointer to next mbuf in packet chain
  32.  */
  33. struct mbuf *
  34. free_mbuf(bp)
  35. register struct mbuf *bp;
  36. {
  37.     register struct mbuf *bp1 = NULLBUF;
  38.  
  39.     if(bp != NULLBUF){
  40.         bp1 = bp->next;
  41.         free((char *)bp);
  42.     }
  43.     return bp1;
  44. }
  45.  
  46. /* Free packet (a chain of mbufs). Return pointer to next packet on queue,
  47.  * if any
  48.  */
  49. struct mbuf *
  50. free_p(bp)
  51. register struct mbuf *bp;
  52. {
  53.     struct mbuf *abp;
  54.  
  55.     if(bp == NULLBUF)
  56.         return NULLBUF;
  57.     abp = bp->anext;
  58.     while(bp != NULLBUF)
  59.         bp = free_mbuf(bp);
  60.     return abp;
  61. }        
  62. /* Free entire queue of packets (of mbufs) */
  63. free_q(q)
  64. struct mbuf **q;
  65. {
  66.     register struct mbuf *bp;
  67.  
  68.     while((bp = dequeue(q)) != NULLBUF)
  69.         free_p(bp);
  70. }
  71.  
  72. /* Count up the total number of bytes in an mbuf */
  73. int16
  74. len_mbuf(bp)
  75. register struct mbuf *bp;
  76. {
  77.     int cnt;
  78.  
  79.     cnt = 0;
  80.     while(bp != NULLBUF){
  81.         cnt += bp->cnt;
  82.         bp = bp->next;
  83.     }
  84.     return cnt;
  85. }
  86. /* Count up the number of packets in a queue */
  87. int16
  88. len_q(bp)
  89. register struct mbuf *bp;
  90. {
  91.     register int cnt;
  92.  
  93.     for(cnt=0;bp != NULLBUF;cnt++,bp = bp->anext)
  94.         ;
  95.     return cnt;
  96. }
  97. /* Duplicate/enqueue/dequeue operations based on mbufs */
  98.  
  99. /* Duplicate first 'cnt' bytes of packet starting at 'offset'.
  100.  * This is done without copying data; only the headers are duplicated,
  101.  * but without data segments of their own. The pointers are set up to
  102.  * share the data segments of the original copy. The return pointer is
  103.  * passed back through the first argument, and the return value is the
  104.  * number of bytes actually duplicated.
  105.  */
  106. int16
  107. dup_p(hp,bp,offset,cnt)
  108. struct mbuf **hp;
  109. register struct mbuf *bp;
  110. register int16 offset;
  111. register int16 cnt;
  112. {
  113.     register struct mbuf *cp;
  114.     int16 tot;
  115.  
  116.     if(cnt == 0 || bp == NULLBUF || hp == (struct mbuf **)NULL){
  117.         if(hp != (struct mbuf **)NULL)
  118.             *hp = NULLBUF;
  119.         return 0;
  120.     }
  121.     if((*hp = cp = alloc_mbuf(0)) == NULLBUF){
  122.         return 0;
  123.     }
  124.     /* Skip over leading mbufs that are smaller than the offset */
  125.     while(bp != NULLBUF && bp->cnt <= offset){
  126.         offset -= bp->cnt;
  127.         bp = bp->next;
  128.     }
  129.     if(bp == NULLBUF){
  130.         free_mbuf(cp);
  131.         *hp = NULLBUF;
  132.         return 0;    /* Offset was too big */
  133.     }
  134.     tot = 0;
  135.     for(;;){
  136.         cp->data = bp->data + offset;
  137.         cp->cnt = min(cnt,bp->cnt - offset);
  138.         offset = 0;
  139.         cnt -= cp->cnt;
  140.         tot += cp->cnt;
  141.         bp = bp->next;
  142.         if(cnt == 0 || bp == NULLBUF || (cp->next = alloc_mbuf(0)) == NULLBUF)
  143.             break;
  144.         cp = cp->next;
  145.     }
  146.     return tot;
  147. }
  148. /* Copy first 'cnt' bytes of packet into a new, single mbuf */
  149. struct mbuf *
  150. copy_p(bp,cnt)
  151. register struct mbuf *bp;
  152. register int16 cnt;
  153. {
  154.     register struct mbuf *cp;
  155.     register char *wp;
  156.     register int16 n;
  157.  
  158.     if(bp == NULLBUF || cnt == 0 || (cp = alloc_mbuf(cnt)) == NULLBUF)
  159.         return NULLBUF;
  160.     wp = cp->data;
  161.     while(cnt != 0 && bp != NULLBUF){
  162.         n = min(cnt,bp->cnt);
  163.         bcopy(bp->data,wp,n);
  164.         wp += n;
  165.         cp->cnt += n;
  166.         cnt -= n;
  167.         bp = bp->next;
  168.     }
  169.     return cp;
  170. }
  171. /* Copy and delete "cnt" bytes from beginning of packet. Return number of
  172.  * bytes actually pulled off
  173.  */
  174. int16
  175. pullup(bph,buf,cnt)
  176. struct mbuf **bph;
  177. char *buf;
  178. int16 cnt;
  179. {
  180.     register struct mbuf *bp;
  181.     int16 n,tot;
  182.  
  183.     tot = 0;
  184.     if(bph == (struct mbuf **)NULL)
  185.         return 0;
  186.     while(*bph != NULLBUF && cnt != 0){
  187.         bp = *bph;
  188.         n = min(cnt,bp->cnt);
  189.         if(buf != NULLCHAR){
  190.             bcopy(bp->data,buf,n);
  191.             buf += n;
  192.         }
  193.         tot += n;
  194.         cnt -= n;
  195.         bp->data += n;
  196.         bp->cnt -= n;        
  197.         if(bp->cnt == 0){
  198.             *bph = free_mbuf(bp);
  199.         }
  200.     }
  201.     return tot;
  202. }
  203. /* Append mbuf to end of mbuf chain */
  204. int16
  205. append(bph,bp)
  206. struct mbuf **bph;
  207. struct mbuf *bp;
  208. {
  209.     register struct mbuf *p;
  210.  
  211.     if(bph == (struct mbuf **)NULL || bp == NULLBUF)
  212.         return;
  213.     if(*bph == NULLBUF){
  214.         /* First one on chain */
  215.         *bph = bp;
  216.     } else {
  217.         for(p = *bph ; p->next != NULLBUF ; p = p->next)
  218.             ;
  219.         p->next = bp;
  220.     }
  221. }
  222. /* Append packet to end of packet queue */
  223. void
  224. enqueue(q,bp)
  225. struct mbuf **q;
  226. struct mbuf *bp;
  227. {
  228.     register struct mbuf *p;
  229.     char i_state;
  230.  
  231.     if(q == (struct mbuf **)NULL || bp == NULLBUF)
  232.         return;
  233.     i_state = disable();
  234.     if(*q == NULLBUF){
  235.         /* List is empty, stick at front */
  236.         *q = bp;
  237.     } else {
  238.         for(p = *q ; p->anext != NULLBUF ; p = p->anext)
  239.             ;
  240.         p->anext = bp;
  241.     }
  242.     restore(i_state);
  243. }
  244. /* Unlink a packet from the head of the queue */
  245. struct mbuf *
  246. dequeue(q)
  247. register struct mbuf **q;
  248. {
  249.     register struct mbuf *bp;
  250.     char i_state;
  251.  
  252.     if(q == (struct mbuf **)NULL)
  253.         return NULLBUF;
  254.     i_state = disable();
  255.     if((bp = *q) != NULLBUF){
  256.         *q = bp->anext;
  257.         bp->anext = NULLBUF;
  258.     }
  259.     restore(i_state);
  260.     return bp;
  261. }    
  262.  
  263. /* Copy user data into an mbuf */
  264. struct mbuf *
  265. qdata(data,cnt)
  266. char *data;
  267. int16 cnt;
  268. {
  269.     register struct mbuf *bp;
  270.  
  271.     if((bp = alloc_mbuf(cnt)) == NULLBUF)
  272.         return NULLBUF;
  273.     bcopy(data,bp->data,cnt);
  274.     bp->cnt = cnt;
  275.     return bp;
  276. }
  277. /* Copy mbuf data into user buffer */
  278. int16
  279. dqdata(bp,buf,cnt)
  280. struct mbuf *bp;
  281. char *buf;
  282. unsigned cnt;
  283. {
  284.     unsigned n,tot;
  285.     struct mbuf *bp1;
  286.  
  287.     if(buf == NULLCHAR)
  288.         return 0;
  289.     
  290.     tot = 0;
  291.     for(bp1 = bp;bp1 != NULLBUF; bp1 = bp1->next){
  292.         n = min(bp1->cnt,cnt);
  293.         bcopy(bp1->data,buf,n);
  294.         cnt -= n;
  295.         buf += n;
  296.         tot += n;
  297.     }
  298.     free_p(bp);
  299.     return tot;
  300. }
  301. #ifdef    TRACE
  302. /* Comment this out if your library already has this function */
  303. #define    isprint(c)    ((c) >= ' ' && (c) < 0x7f)        /* Assumes ASCII */
  304.  
  305. /* Dump an mbuf in hex */
  306. void
  307. hexdump(bp)
  308. struct mbuf *bp;
  309. {
  310.     register struct mbuf *tbp;
  311.     int16 n;
  312.     int16 address;
  313.     void fmtline();
  314.  
  315.     if(bp == NULLBUF)
  316.         return;
  317.     tbp = copy_p(bp,len_mbuf(bp));
  318.     address = 0;
  319.     while(tbp->cnt != 0){
  320.         n = min(tbp->cnt,16);
  321.         fmtline(address,tbp->data,n);
  322.         address += n;
  323.         tbp->data += n;
  324.         tbp->cnt -= n;
  325.     }
  326.     free_p(tbp);
  327.     fflush(stdout);
  328. }
  329. /* Dump an mbuf in ascii */
  330. void
  331. asciidump(bp)
  332. struct mbuf *bp;
  333. {
  334.     struct mbuf *tbp;
  335.     char c;
  336.     int16 tot;
  337.  
  338.     if(bp == NULLBUF)
  339.         return;
  340.     tbp = copy_p(bp,len_mbuf(bp));
  341.     tot = 0;
  342.     while(pullup(&tbp,&c,1) == 1){
  343.         if((tot % 64) == 0)
  344.             printf("%04x  ",tot);
  345.         if(isprint(c))
  346.             putchar(c);
  347.         else
  348.             putchar('.');
  349.         tot++;
  350.         if((tot % 64) == 0)
  351.             printf("\r\n");
  352.     }
  353.     if((tot % 64) != 0)
  354.         printf("\r\n");
  355.     free_p(tbp);
  356.     fflush(stdout);
  357. }
  358. /* Print a buffer up to 16 bytes long in formatted hex with ascii
  359.  * translation, e.g.,
  360.  * 0000: 30 31 32 33 34 35 36 37 38 39 3a 3b 3c 3d 3e 3f  0123456789:;<=>?
  361.  */
  362. void
  363. fmtline(addr,buf,len)
  364. int16 addr;
  365. char *buf;
  366. int16 len;
  367. {
  368.     char line[80];
  369.     char *aptr,*cptr;
  370.     int16 c;
  371.     void ctohex();
  372.  
  373.     for(cptr = line;cptr < &line[80];cptr++)
  374.         *cptr = ' ';
  375.     ctohex(line,(int16)hibyte(addr));
  376.     ctohex(line+2,(int16)lobyte(addr));
  377.     aptr = &line[6];
  378.     cptr = &line[55];
  379.     while(len-- != 0){
  380.         c = *buf++ & 0xff;
  381.         ctohex(aptr,c);
  382.         aptr += 3;
  383.         c &= 0x7f;
  384.         if(isprint(c)){
  385.             *cptr++ = c;
  386.         } else {
  387.             *cptr++ = '.';
  388.         }
  389.     }
  390.     *cptr++ = '\r';
  391.     *cptr++ = '\n';
  392. #ifdef    AMIGA
  393.     *cptr = '\0';
  394.     printf(line);
  395. #else
  396.     fwrite(line,1,(unsigned)(cptr-line),stdout);
  397. #endif
  398. }
  399. /* Convert byte to two ascii-hex characters */
  400. static
  401. void
  402. ctohex(buf,c)
  403. char *buf;
  404. int16 c;
  405. {
  406.     static char hex[] = "0123456789abcdef";
  407.  
  408.     buf[0] = hex[hinibble(c)];
  409.     buf[1] = hex[lonibble(c)];
  410. }
  411. #endif
  412.