/* Primitive mbuf allocate/free routines */

#ifdef TRACE
#include <stdio.h>
#endif

#include "machdep.h"
#include "mbuf.h"

/* Allocate mbuf with associated buffer of 'size' bytes */
struct mbuf *
alloc_mbuf(size)
register int16 size;
{
	register struct mbuf *bp;
	char *malloc();

	if((bp = (struct mbuf *)malloc((unsigned)(size + sizeof(struct mbuf)))) == NULLBUF)
		return NULLBUF;
	bp->next = bp->anext = NULLBUF;
	if(size != 0){
		bp->data = (char *)(bp + 1);
	} else {
		bp->data = NULLCHAR;
	}
	bp->cnt = 0;
	return bp;
}

/* Free all resources associated with mbuf
 * Return pointer to next mbuf in packet chain
 */
struct mbuf *
free_mbuf(bp)
register struct mbuf *bp;
{
	register struct mbuf *bp1 = NULLBUF;

	if(bp != NULLBUF){
		bp1 = bp->next;
		free((char *)bp);
	}
	return bp1;
}

/* Free packet (a chain of mbufs). Return pointer to next packet on queue,
 * if any
 */
struct mbuf *
free_p(bp)
register struct mbuf *bp;
{
	struct mbuf *abp;

	if(bp == NULLBUF)
		return NULLBUF;
	abp = bp->anext;
	while(bp != NULLBUF)
		bp = free_mbuf(bp);
	return abp;
}		
/* Free entire queue of packets (of mbufs) */
free_q(q)
struct mbuf **q;
{
	register struct mbuf *bp;

	while((bp = dequeue(q)) != NULLBUF)
		free_p(bp);
}

/* Count up the total number of bytes in an mbuf */
int16
len_mbuf(bp)
register struct mbuf *bp;
{
	int cnt;

	cnt = 0;
	while(bp != NULLBUF){
		cnt += bp->cnt;
		bp = bp->next;
	}
	return cnt;
}
/* Count up the number of packets in a queue */
int16
len_q(bp)
register struct mbuf *bp;
{
	register int cnt;

	for(cnt=0;bp != NULLBUF;cnt++,bp = bp->anext)
		;
	return cnt;
}
/* Duplicate/enqueue/dequeue operations based on mbufs */

/* Duplicate first 'cnt' bytes of packet starting at 'offset'.
 * This is done without copying data; only the headers are duplicated,
 * but without data segments of their own. The pointers are set up to
 * share the data segments of the original copy. The return pointer is
 * passed back through the first argument, and the return value is the
 * number of bytes actually duplicated.
 */
int16
dup_p(hp,bp,offset,cnt)
struct mbuf **hp;
register struct mbuf *bp;
register int16 offset;
register int16 cnt;
{
	register struct mbuf *cp;
	int16 tot;

	if(cnt == 0 || bp == NULLBUF || hp == (struct mbuf **)NULL){
		if(hp != (struct mbuf **)NULL)
			*hp = NULLBUF;
		return 0;
	}
	if((*hp = cp = alloc_mbuf(0)) == NULLBUF){
		return 0;
	}
	/* Skip over leading mbufs that are smaller than the offset */
	while(bp != NULLBUF && bp->cnt <= offset){
		offset -= bp->cnt;
		bp = bp->next;
	}
	if(bp == NULLBUF){
		free_mbuf(cp);
		*hp = NULLBUF;
		return 0;	/* Offset was too big */
	}
	tot = 0;
	for(;;){
		cp->data = bp->data + offset;
		cp->cnt = min(cnt,bp->cnt - offset);
		offset = 0;
		cnt -= cp->cnt;
		tot += cp->cnt;
		bp = bp->next;
		if(cnt == 0 || bp == NULLBUF || (cp->next = alloc_mbuf(0)) == NULLBUF)
			break;
		cp = cp->next;
	}
	return tot;
}
/* Copy first 'cnt' bytes of packet into a new, single mbuf */
struct mbuf *
copy_p(bp,cnt)
register struct mbuf *bp;
register int16 cnt;
{
	register struct mbuf *cp;
	register char *wp;
	register int16 n;

	if(bp == NULLBUF || cnt == 0 || (cp = alloc_mbuf(cnt)) == NULLBUF)
		return NULLBUF;
	wp = cp->data;
	while(cnt != 0 && bp != NULLBUF){
		n = min(cnt,bp->cnt);
		bcopy(bp->data,wp,n);
		wp += n;
		cp->cnt += n;
		cnt -= n;
		bp = bp->next;
	}
	return cp;
}
/* Copy and delete "cnt" bytes from beginning of packet. Return number of
 * bytes actually pulled off
 */
int16
pullup(bph,buf,cnt)
struct mbuf **bph;
char *buf;
int16 cnt;
{
	register struct mbuf *bp;
	int16 n,tot;

	tot = 0;
	if(bph == (struct mbuf **)NULL)
		return 0;
	while(*bph != NULLBUF && cnt != 0){
		bp = *bph;
		n = min(cnt,bp->cnt);
		if(buf != NULLCHAR){
			bcopy(bp->data,buf,n);
			buf += n;
		}
		tot += n;
		cnt -= n;
		bp->data += n;
		bp->cnt -= n;		
		if(bp->cnt == 0){
			*bph = free_mbuf(bp);
		}
	}
	return tot;
}
/* Append mbuf to end of mbuf chain */
int16
append(bph,bp)
struct mbuf **bph;
struct mbuf *bp;
{
	register struct mbuf *p;

	if(bph == (struct mbuf **)NULL || bp == NULLBUF)
		return;
	if(*bph == NULLBUF){
		/* First one on chain */
		*bph = bp;
	} else {
		for(p = *bph ; p->next != NULLBUF ; p = p->next)
			;
		p->next = bp;
	}
}
/* Append packet to end of packet queue */
void
enqueue(q,bp)
struct mbuf **q;
struct mbuf *bp;
{
	register struct mbuf *p;
	char i_state;

	if(q == (struct mbuf **)NULL || bp == NULLBUF)
		return;
	i_state = disable();
	if(*q == NULLBUF){
		/* List is empty, stick at front */
		*q = bp;
	} else {
		for(p = *q ; p->anext != NULLBUF ; p = p->anext)
			;
		p->anext = bp;
	}
	restore(i_state);
}
/* Unlink a packet from the head of the queue */
struct mbuf *
dequeue(q)
register struct mbuf **q;
{
	register struct mbuf *bp;
	char i_state;

	if(q == (struct mbuf **)NULL)
		return NULLBUF;
	i_state = disable();
	if((bp = *q) != NULLBUF){
		*q = bp->anext;
		bp->anext = NULLBUF;
	}
	restore(i_state);
	return bp;
}	

/* Copy user data into an mbuf */
struct mbuf *
qdata(data,cnt)
char *data;
int16 cnt;
{
	register struct mbuf *bp;

	if((bp = alloc_mbuf(cnt)) == NULLBUF)
		return NULLBUF;
	bcopy(data,bp->data,cnt);
	bp->cnt = cnt;
	return bp;
}
/* Copy mbuf data into user buffer */
int16
dqdata(bp,buf,cnt)
struct mbuf *bp;
char *buf;
unsigned cnt;
{
	unsigned n,tot;
	struct mbuf *bp1;

	if(buf == NULLCHAR)
		return 0;
	
	tot = 0;
	for(bp1 = bp;bp1 != NULLBUF; bp1 = bp1->next){
		n = min(bp1->cnt,cnt);
		bcopy(bp1->data,buf,n);
		cnt -= n;
		buf += n;
		tot += n;
	}
	free_p(bp);
	return tot;
}
#ifdef	TRACE
/* Comment this out if your library already has this function */
#define	isprint(c)	((c) >= ' ' && (c) < 0x7f)		/* Assumes ASCII */

/* Dump an mbuf in hex */
void
hexdump(bp)
struct mbuf *bp;
{
	register struct mbuf *tbp;
	int16 n;
	int16 address;
	void fmtline();

	if(bp == NULLBUF)
		return;
	tbp = copy_p(bp,len_mbuf(bp));
	address = 0;
	while(tbp->cnt != 0){
		n = min(tbp->cnt,16);
		fmtline(address,tbp->data,n);
		address += n;
		tbp->data += n;
		tbp->cnt -= n;
	}
	free_p(tbp);
	fflush(stdout);
}
/* Dump an mbuf in ascii */
void
asciidump(bp)
struct mbuf *bp;
{
	struct mbuf *tbp;
	char c;
	int16 tot;

	if(bp == NULLBUF)
		return;
	tbp = copy_p(bp,len_mbuf(bp));
	tot = 0;
	while(pullup(&tbp,&c,1) == 1){
		if((tot % 64) == 0)
			printf("%04x  ",tot);
		if(isprint(c))
			putchar(c);
		else
			putchar('.');
		tot++;
		if((tot % 64) == 0)
			printf("\r\n");
	}
	if((tot % 64) != 0)
		printf("\r\n");
	free_p(tbp);
	fflush(stdout);
}
/* Print a buffer up to 16 bytes long in formatted hex with ascii
 * translation, e.g.,
 * 0000: 30 31 32 33 34 35 36 37 38 39 3a 3b 3c 3d 3e 3f  0123456789:;<=>?
 */
void
fmtline(addr,buf,len)
int16 addr;
char *buf;
int16 len;
{
	char line[80];
	char *aptr,*cptr;
	int16 c;
	void ctohex();

	for(cptr = line;cptr < &line[80];cptr++)
		*cptr = ' ';
	ctohex(line,(int16)hibyte(addr));
	ctohex(line+2,(int16)lobyte(addr));
	aptr = &line[6];
	cptr = &line[55];
	while(len-- != 0){
		c = *buf++ & 0xff;
		ctohex(aptr,c);
		aptr += 3;
		c &= 0x7f;
		if(isprint(c)){
			*cptr++ = c;
		} else {
			*cptr++ = '.';
		}
	}
	*cptr++ = '\r';
	*cptr++ = '\n';
#ifdef	AMIGA
	*cptr = '\0';
	printf(line);
#else
	fwrite(line,1,(unsigned)(cptr-line),stdout);
#endif
}
/* Convert byte to two ascii-hex characters */
static
void
ctohex(buf,c)
char *buf;
int16 c;
{
	static char hex[] = "0123456789abcdef";

	buf[0] = hex[hinibble(c)];
	buf[1] = hex[lonibble(c)];
}
#endif