/* $Id: mbh_subr.c,v 1.1 2008/05/06 20:04:41 ryo Exp $ */

/*
 * Copyright (c) 2008 SHIMIZU Ryo <ryo@nerv.org>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifdef MBH_TEST_STANDALONE
#include "mbh_standalone.h"
#else
#include <sys/param.h>
#include <sys/mbuf.h>
#endif

#include "mbh.h"

#ifdef MBH_DEBUG
#define DPRINTF(format, args...)	printf(format, ## args)
#else
#define DPRINTF(args...)
#endif

#ifdef MBH_DEBUG
void
mbh_dump(struct mbh *mbh)
{
	printf("<mbh_dump>\n");
	printf("	mbh_m      = %p\n", mbh->mbh_m);
	printf("	mbh_offset = %d/%d\n", mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);
	printf("</mbh_dump>\n\n");
}
#endif

#define	MBH_LEFT(mbh)	((mbh)->mbh_m->m_len - (mbh)->mbh_offset)
#define DPRINTF(args...)

inline int32_t
mbh_peek_1(struct mbh *mbh)
{
	uint8_t *p;

	DPRINTF("mbh_peek_1: mbuf=%p offset=%d/%d\n",
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_peek_1: eof\n");
		mbh->mbh_eof = 1;
		return -1;
	}

	while (MBH_LEFT(mbh) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_peek_1: reach eof\n");
			mbh->mbh_eof = 1;
			return -1;
		}
	}
	p = mtod(mbh->mbh_m, uint8_t *) + mbh->mbh_offset;
	return *p;
}

int32_t
mbh_read_1(struct mbh *mbh)
{
	int32_t result;

	result = mbh_peek_1(mbh);
	if (mbh->mbh_eof == 0) {
		mbh->mbh_offset++;
		mbh->mbh_total_offset++;
	}
	return result;
}

inline int
mbh_poke_1(struct mbh *mbh, uint8_t data)
{
	uint8_t *p;

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_poke_1: reach eof\n");
		mbh->mbh_eof = 1;
		return 0;
	}

	while (MBH_LEFT(mbh) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_poke_1: reach eof\n");
			mbh->mbh_eof = 1;
			return 0;
		}
	}
	p = mtod(mbh->mbh_m, uint8_t *) + mbh->mbh_offset;
	*p = data;
	return 1;
}

inline int
mbh_write_1(struct mbh *mbh, uint8_t data)
{
	mbh_poke_1(mbh, data);
	if (mbh->mbh_eof == 0) {
		mbh->mbh_offset++;
		mbh->mbh_total_offset++;
		return 1;
	}
	return 0;
}

inline int32_t
mbh_read_2(struct mbh *mbh)
{
	int d0, d1;
	int l;
	struct {
		uint16_t data;
	} __attribute__((__packed__)) *p;

	DPRINTF("mbh_read_2: mbuf=%p offset=%d/%d\n",
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_read_2: eof\n");
		mbh->mbh_eof = 1;
		return -1;
	}

	while ((l = MBH_LEFT(mbh)) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_read_2: reach eof\n");
			mbh->mbh_eof = 1;
			return -1;
		}
	}

	if (l < 2) {
		DPRINTF("mbh_read_2: no more 2byte data: fallback to mbh_read_1\n");

		if ((d0 = mbh_read_1(mbh)) < 0) {
			DPRINTF("mbh_read_2: 1st mbh_read_1 return -1\n");
			return -1;
		}
		if ((d1 = mbh_read_1(mbh)) < 0) {
			DPRINTF("mbh_read_2: 2nd mbh_read_1 return -1\n");
			return -1;
		}
#if _BYTE_ORDER == _LITTLE_ENDIAN
		return (d1 << 8) + d0;
#else
		return (d0 << 8) + d1;
#endif
	}

	p = (void *)(mtod(mbh->mbh_m, char *) + mbh->mbh_offset);
	mbh->mbh_offset += 2;
	mbh->mbh_total_offset += 2;
	return p->data;
}

int32_t
mbh_peek_2(struct mbh *mbh)
{
	struct mbh mbh_tmp;

	mbh_tmp = *mbh;
	return mbh_read_2(&mbh_tmp);
}

inline int32_t
mbh_write_2(struct mbh *mbh, uint16_t data)
{
	int l;
	struct {
		uint16_t data;
	} __attribute__((__packed__)) *p;

	DPRINTF("mbh_read_2: mbuf=%p offset=%d/%d\n",
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_write_2: eof\n");
		mbh->mbh_eof = 1;
		return 0;
	}

	while ((l = MBH_LEFT(mbh)) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_write_2: reach eof\n");
			mbh->mbh_eof = 1;
			return 0;
		}
	}

	if (l < 2) {
		DPRINTF("mbh_write_2: no more 2byte data: fallback to mbh_read_1\n");

#if _BYTE_ORDER == _LITTLE_ENDIAN
		if (mbh_write_1(mbh, data) != 1) {
			DPRINTF("mbh_write_2: 1st mbh_write_1 doesn't return 1\n");
			return 0;
		}
		if (mbh_write_1(mbh, data >> 8) == 0) {
			DPRINTF("mbh_write_2: 2nd mbh_write_1 doesn't return 1\n");
			return 1;
		}
#else
		if (mbh_write_1(mbh, data >> 8) == 0) {
			DPRINTF("mbh_write_2: 1st mbh_write_1 doesn't return 1\n");
			return 0;
		}
		if (mbh_write_1(mbh, data) == 0) {
			DPRINTF("mbh_write_2: 2nd mbh_write_1 doesn't return 1\n");
			return 1;
		}
#endif
		return 2;
	}

	p = (void *)(mtod(mbh->mbh_m, char *) + mbh->mbh_offset);
	p->data = data;
	mbh->mbh_offset += 2;
	mbh->mbh_total_offset += 2;
	return 2;
}

int32_t
mbh_poke_2(struct mbh *mbh, uint16_t data)
{
	struct mbh mbh_tmp;

	mbh_tmp = *mbh;
	return mbh_write_2(&mbh_tmp, data);
}

uint32_t
mbh_read_4(struct mbh *mbh)
{
	int d1, d2;
	int l;
	struct {
		uint32_t data;
	} __attribute__((__packed__)) *p;

	DPRINTF("mbh_read_4: mbuf=%p offset=%d/%d\n",
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_read_4: eof\n");
		mbh->mbh_eof = 1;
		return -1;
	}

	while ((l = MBH_LEFT(mbh)) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_read_4: reach eof\n");
			mbh->mbh_eof = 1;
			return -1;
		}
	}

	if (l < 4) {
		DPRINTF("mbh_read_4: no more 4byte data: fallback to mbh_read_2\n");

		if ((d1 = mbh_read_2(mbh)) < 0) {
			DPRINTF("mbh_read_4: 1st mbh_read_2 return -1\n");
			return -1;
		}
		if ((d2 = mbh_read_2(mbh)) < 0) {
			DPRINTF("mbh_read_4: 2nd mbh_read_2 return -1\n");
			return -1;
		}

#if _BYTE_ORDER == _LITTLE_ENDIAN
		return (d2 << 16) + d1;
#else
		return (d1 << 16) + d2;
#endif
	}

	p = (void *)(mtod(mbh->mbh_m, char *) + mbh->mbh_offset);
	mbh->mbh_offset += 4;
	mbh->mbh_total_offset += 4;
	return p->data;
}

uint32_t
mbh_peek_4(struct mbh *mbh)
{
	struct mbh mbh_tmp;

	mbh_tmp = *mbh;
	return mbh_read_4(&mbh_tmp);
}

inline int32_t
mbh_write_4(struct mbh *mbh, uint32_t data)
{
	int l, rc;
	struct {
		uint32_t data;
	} __attribute__((__packed__)) *p;

	DPRINTF("mbh_write_4: mbuf=%p offset=%d/%d\n",
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_write_4: eof\n");
		mbh->mbh_eof = 1;
		return -1;
	}

	while ((l = MBH_LEFT(mbh)) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_write_4: reach eof\n");
			mbh->mbh_eof = 1;
			return -1;
		}
	}

	if (l < 4) {
		DPRINTF("mbh_write_4: no more 4byte data: fallback to mbh_write_2\n");

#if _BYTE_ORDER == _LITTLE_ENDIAN
		if ((rc = mbh_write_2(mbh, data)) != 2) {
			DPRINTF("mbh_write_4: 1st mbh_write_2 return %d\n", rc);
			return rc;
		}
		if ((rc = mbh_write_2(mbh, data >> 16)) != 2) {
			DPRINTF("mbh_write_4: 2nd mbh_write_2 return %d\n", rc);
			return 2 + rc;
		}
#else
		if ((rc = mbh_write_2(mbh, data >> 16)) != 2) {
			DPRINTF("mbh_write_4: 1st mbh_write_2 return %d\n", rc);
			return 0;
		}
		if ((rc = mbh_write_2(mbh, data)) != 2) {
			DPRINTF("mbh_write_4: 2nd mbh_write_2 return %d\n", rc);
			return 2 + rc;
		}
#endif
		return 4;
	}

	p = (void *)(mtod(mbh->mbh_m, char *) + mbh->mbh_offset);
	p->data = data;
	mbh->mbh_offset += 4;
	mbh->mbh_total_offset += 4;
	return p->data;
}

int32_t
mbh_poke_4(struct mbh *mbh, uint32_t data)
{
	struct mbh mbh_tmp;

	mbh_tmp = *mbh;
	return mbh_write_4(&mbh_tmp, data);
}

/*
 * the number of bytes which are forwarded is returned
 */
int
mbh_forward(struct mbh *mbh, int nbytes)
{
	int l, nbytes0;

	DPRINTF("mbh_forward(mbh=%p, nbytes=%d): mbuf=%p offset=%d/%d\n",
	    mbh, nbytes,
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_forward: eof\n");
		mbh->mbh_eof = 1;
		return 0;
	}

	nbytes0 = nbytes;
	while (nbytes > 0) {
		while ((l = MBH_LEFT(mbh)) <= 0) {
			mbh->mbh_offset -= mbh->mbh_m->m_len;
			if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
				DPRINTF("mbh_forward: reach eof\n");
				mbh->mbh_eof = 1;
				goto forward_done;
			}
		}

		DPRINTF("mbh_forward: left=%d, read %d bytes\n", l, nbytes);
		if (l > nbytes) {
			DPRINTF("mbh_forward: forward %d bytes (enough)\n", nbytes);
			mbh->mbh_offset += nbytes;
			mbh->mbh_total_offset += nbytes;
			nbytes = 0;
			break;
		}

		DPRINTF("mbh_forward: forward %d bytes (partly)\n", l);
		mbh->mbh_offset += l;
		mbh->mbh_total_offset += l;
		nbytes -= l;
	}
 forward_done:
	return nbytes0 - nbytes;
}

int
mbh_read(struct mbh *mbh, void *buf, int nbytes)
{
	char *p;
	int l, nbytes0;

	DPRINTF("mbh_read(mbh=%p, buf=%p, nbytes=%d): mbuf=%p offset=%d/%d\n",
	    mbh, buf, nbytes,
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_read: eof\n");
		mbh->mbh_eof = 1;
		return -1;
	}

	p = buf;
	nbytes0 = nbytes;	/* save original */
	while (nbytes > 0) {
		while ((l = MBH_LEFT(mbh)) <= 0) {
			mbh->mbh_offset -= mbh->mbh_m->m_len;
			if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
				DPRINTF("mbh_read: reach eof\n");
				mbh->mbh_eof = 1;
				goto read_done;
			}
		}

		DPRINTF("mbh_read: left=%d, read %d bytes\n", l, nbytes);
		if (l > nbytes) {
			DPRINTF("mbh_read: memcpy %d bytes (enough)\n", nbytes);
			memcpy(p, mtod(mbh->mbh_m, char *) + mbh->mbh_offset, nbytes);
			mbh->mbh_offset += nbytes;
			mbh->mbh_total_offset += nbytes;
			nbytes = 0;
			break;
		}

		DPRINTF("mbh_read: memcpy %d bytes (partly)\n", l);
		memcpy(p, mtod(mbh->mbh_m, char *) + mbh->mbh_offset, l);
		p += l;
		mbh->mbh_offset += l;
		mbh->mbh_total_offset += l;
		nbytes -= l;
	}
 read_done:
	return nbytes0 - nbytes;
}

int
mbh_peek(struct mbh *mbh, void *buf, int nbytes)
{
	struct mbh mbh_tmp;

	mbh_tmp = *mbh;
	return mbh_read(&mbh_tmp, buf, nbytes);
}

int
mbh_poke(struct mbh *mbh, void *buf, int nbytes)
{
	struct mbh mbh_tmp;

	mbh_tmp = *mbh;
	return mbh_write(&mbh_tmp, buf, nbytes);
}

int
mbh_write(struct mbh *mbh, void *buf, int nbytes)
{
	char *p;
	int l, nbytes0;

	DPRINTF("mbh_write(mbh=%p, buf=%p, nbytes=%d): mbuf=%p offset=%d/%d\n",
	    mbh, buf, nbytes,
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_write: eof\n");
		mbh->mbh_eof = 1;
		return -1;
	}

	p = buf;
	nbytes0 = nbytes;	/* save original */
	while (nbytes > 0) {
		while ((l = MBH_LEFT(mbh)) <= 0) {
			mbh->mbh_offset -= mbh->mbh_m->m_len;
			if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
				DPRINTF("mbh_write: reach eof\n");
				mbh->mbh_eof = 1;
				goto write_done;
			}
		}

		DPRINTF("mbh_write: left=%d, read %d bytes\n", l, nbytes);
		if (l > nbytes) {
			DPRINTF("mbh_write: memcpy %d bytes (enough)\n", nbytes);
			memcpy(mtod(mbh->mbh_m, char *) + mbh->mbh_offset, p, nbytes);
			mbh->mbh_offset += nbytes;
			mbh->mbh_total_offset += nbytes;
			nbytes = 0;
			break;
		}

		DPRINTF("mbh_write: memcpy %d bytes (partly)\n", l);
		memcpy(mtod(mbh->mbh_m, char *) + mbh->mbh_offset, p, l);
		p += l;
		mbh->mbh_offset += l;
		mbh->mbh_total_offset += l;
		nbytes -= l;
	}
 write_done:
	return nbytes0 - nbytes;
}


/*
 * return mbuf data pointer in mbuf if continuous,
 * or return copyed buffer
 */
void *
mbh_peekbuf(struct mbh *mbh, void *tmpbuf, int nbytes)
{
	int l;
	char *p;

	DPRINTF("mbh_peekbuf(mbh=%p, tmpbuf=%p, nbytes=%d): mbuf=%p offset=%d/%d\n",
	    mbh, tmpbuf, nbytes,
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_peekbuf: eof\n");
		mbh->mbh_eof = 1;
		return NULL;
	}

	while ((l = MBH_LEFT(mbh)) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_getbuf: reach eof\n");
			mbh->mbh_eof = 1;
			return NULL;
		}
	}

	if (l > nbytes) {
		DPRINTF("mbh_getbuf: left=%d, %d bytes data is continuous in mbuf\n", l, nbytes);
		p = mtod(mbh->mbh_m, char *) + mbh->mbh_offset;
		return p;
	}

	DPRINTF("mbh_peekbuf: left=%d, %d bytes data is not continuous. fallback to mbh_read\n", l, nbytes);
	if (mbh_peek(mbh, tmpbuf, nbytes) != nbytes)
		return NULL;
	return tmpbuf;
}

/*
 * return mbuf data pointer in mbuf if continuous,
 * or return copyed buffer
 */
void *
mbh_readbuf(struct mbh *mbh, void *tmpbuf, int nbytes)
{
	int l;
	char *p;

	DPRINTF("mbh_getbuf(mbh=%p, tmpbuf=%p, nbytes=%d): mbuf=%p offset=%d/%d\n",
	    mbh, tmpbuf, nbytes,
	    mbh->mbh_m, mbh->mbh_offset, mbh->mbh_m ? mbh->mbh_m->m_len : 0);

	if (mbh->mbh_m == NULL) {
		DPRINTF("mbh_getbuf: eof\n");
		mbh->mbh_eof = 1;
		return NULL;
	}

	while ((l = MBH_LEFT(mbh)) <= 0) {
		mbh->mbh_offset -= mbh->mbh_m->m_len;
		if ((mbh->mbh_m = mbh->mbh_m->m_next) == NULL) {
			DPRINTF("mbh_getbuf: reach eof\n");
			mbh->mbh_eof = 1;
			return NULL;
		}
	}

	if (l > nbytes) {
		DPRINTF("mbh_getbuf: left=%d, %d bytes data is continuous in mbuf\n", l, nbytes);
		p = mtod(mbh->mbh_m, char *) + mbh->mbh_offset;
		mbh->mbh_offset += nbytes;
		mbh->mbh_total_offset += nbytes;
		return p;
	}

	DPRINTF("mbh_getbuf: left=%d, %d bytes data is not continuous. fallback to mbh_read\n", l, nbytes);
	if (mbh_read(mbh, tmpbuf, nbytes) != nbytes)
		return NULL;
	return tmpbuf;
}