/* $Id: mbf_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 MBF_TEST_STANDALONE
#include "mbf_standalone.h"
#else
#include <sys/param.h>
#include <sys/mbuf.h>
#endif

#include "mbf.h"

#undef MBF_DEBUG

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

void
mbf_open(struct mbf *mbf, struct mbuf *m, unsigned int mode)
{
	switch (mode & MBFFLAGS_MODEMASK) {
	case MBFFLAGS_RDONLY:
		break;
	case MBFFLAGS_WRONLY:
	case MBFFLAGS_RDWR:
	default:
		panic("mbf_open: unsupported mode: 0x%x", mode);
		break;
	}

	mbf->mbf_m0 = mbf->mbf_cur = m;
	mbf->mbf_flags = (mode & MBFFLAGS_MODEMASK);
	mbf->mbf_size = 0;
	mbf->mbf_cur_base = 0;
	mbf->mbf_cur_offset = 0;
}

void
mbf_dump(struct mbf *mbf)
{
#ifdef MBF_DEBUG
	printf("<mbf_dump pos=%d>\n", mbf_tell(mbf));
	printf("	mbf_m0         = %p\n", mbf->mbf_m0);
	printf("	mbf_cur        = %p\n", mbf->mbf_cur);
	printf("	mbf_flags      = 0x%08x\n", mbf->mbf_flags);
	printf("	mbf_size       = %d\n", mbf->mbf_size);
	printf("	mbf_cur_base   = %d\n", mbf->mbf_cur_base);
	printf("	mbf_cur_offset = %d\n", mbf->mbf_cur_offset);
	printf("</mbf_dump>\n\n");
#endif
}


#define	MBF_LEFT(mbf)	((mbf)->mbf_cur->m_len - (mbf)->mbf_cur_offset)

int
mbf_seek(struct mbf *mbf, int offset, int whence)
{
	DPRINTF("mbf_seek(%p, %d, %d)\n", mbf, offset, whence);

	switch (whence) {
	case 2:	/* SEEK_END */
		if (offset >= 0) {
			DPRINTF("mbf_seek: seek to end\n");
			offset = 0x3fffffff;
		} else {
			offset = mbf_length(mbf) + offset - mbf_tell(mbf);
		}
		break;
	case 0:	/* SEEK_SET */
		if (offset < 0)
			offset = 0;
		offset -= mbf_tell(mbf);
		break;
	case 1:	/* SEEK_CUR */
		if ((mbf_tell(mbf) + offset) < 0)
			offset = -mbf_tell(mbf);
		break;
	default:
		return -1;
	}

	DPRINTF("mbf_seek: pos=%d, %+d -> %d\n", mbf_tell(mbf), offset,
	    mbf_tell(mbf) + offset);

	if (offset < 0) {
		/* seek backward if possible */
		offset = -offset;
		if (mbf->mbf_cur_offset >= offset) {
			DPRINTF("mbf_seek: seek backward mbuf=%p %d/%d -> mbuf=%p %d/%d\n",
			    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
			    mbf->mbf_cur, mbf->mbf_cur_offset - offset, mbf->mbf_cur->m_len);
			mbf->mbf_cur_offset -= offset;
			return mbf_tell(mbf);
		}

		/* cannot backward, trace from top of mbuf chain */
		DPRINTF("mbf_seek: cannot seek backward. trace from top\n");

		offset = mbf_tell(mbf) - offset;
		if (offset < 0)
			offset = 0;
		mbf->mbf_cur = mbf->mbf_m0;
		mbf->mbf_cur_base = 0;
		mbf->mbf_cur_offset = 0;
	}

	/* seek forward */
	while (offset > MBF_LEFT(mbf)) {
		if (mbf->mbf_cur->m_next == NULL) {
			DPRINTF("mbf_seek: seek forward mbuf=%p %d/%d -> mbuf=%p %d/%d (reach to end)\n",
			    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
			    mbf->mbf_cur, mbf->mbf_cur->m_len, mbf->mbf_cur->m_len);

			mbf->mbf_cur_offset = mbf->mbf_cur->m_len;
			mbf->mbf_size = mbf->mbf_cur_base + mbf->mbf_cur_offset;
			return mbf->mbf_size;
		}

		DPRINTF("mbf_seek: seek forward mbuf=%p %d/%d -> mbuf=%p %d/%d, next=%p %d/%d\n",
		    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
		    mbf->mbf_cur, mbf->mbf_cur->m_len, mbf->mbf_cur->m_len,
		    mbf->mbf_cur->m_next, 0, mbf->mbf_cur->m_next->m_len);

		offset -= MBF_LEFT(mbf);

		mbf->mbf_cur_base += mbf->mbf_cur->m_len;
		mbf->mbf_cur_offset = 0;
		mbf->mbf_cur = mbf->mbf_cur->m_next;
	}

	mbf->mbf_cur_offset += offset;
	DPRINTF("mbf_seek: seek forward mbuf=%p 0/%d -> %d/%d complete\n",
	    mbf->mbf_cur, mbf->mbf_cur->m_len,
	    mbf->mbf_cur_offset, mbf->mbf_cur->m_len);
	return mbf_tell(mbf);
}


int32_t
mbf_read_1(struct mbf *mbf)
{
	uint8_t *p;
	if (mbf->mbf_cur == NULL)
		return -1;

	DPRINTF("mbf_read_1: mbf_cur_offset=%d, mbf_cur->m_len=%d\n",
	    mbf->mbf_cur_offset, mbf->mbf_cur->m_len);

	while (mbf->mbf_cur_offset >= mbf->mbf_cur->m_len) {
		if (mbf->mbf_cur->m_next == NULL) {
			DPRINTF("mbf_read_1: m=%p %d/%d -> m=%p (reach eof)\n",
			    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
			    mbf->mbf_cur->m_next);

			mbf->mbf_size = mbf->mbf_cur_base + mbf->mbf_cur->m_len;
			return -1;
		}

		DPRINTF("mbf_read_1: m=%p %d/%d -> m=%p %d/%d\n",
		    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
		    mbf->mbf_cur->m_next, 0, mbf->mbf_cur->m_next->m_len);

		mbf->mbf_cur_base += mbf->mbf_cur->m_len;
		mbf->mbf_cur_offset = 0;
		mbf->mbf_cur = mbf->mbf_cur->m_next;
	}
	p = mtod(mbf->mbf_cur, uint8_t *) + mbf->mbf_cur_offset++;
	DPRINTF("mbf_read_1: read 1byte: 0x%02x\n", *p);

	return *p;
}

int32_t
mbf_read_2(struct mbf *mbf)
{
	int d1, d2;
	struct {
		uint16_t data;
	} __attribute__((__packed__)) *p;

	if (mbf->mbf_cur == NULL)
		return -1;

	DPRINTF("mbf_read_2: mbf_cur_offset=%d, mbf_cur->m_len=%d\n",
	    mbf->mbf_cur_offset, mbf->mbf_cur->m_len);

	while (mbf->mbf_cur_offset >= mbf->mbf_cur->m_len) {
		if (mbf->mbf_cur->m_next == NULL) {
			DPRINTF("mbf_read_2: m=%p %d/%d -> m=%p (reach eof)\n",
			    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
			    mbf->mbf_cur->m_next);

			mbf->mbf_size = mbf->mbf_cur_base + mbf->mbf_cur->m_len;
			return -1;
		}

		DPRINTF("mbf_read_2: m=%p %d/%d -> m=%p %d/%d\n",
		    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
		    mbf->mbf_cur->m_next, 0, mbf->mbf_cur->m_next->m_len);

		mbf->mbf_cur_base += mbf->mbf_cur->m_len;
		mbf->mbf_cur_offset = 0;
		mbf->mbf_cur = mbf->mbf_cur->m_next;
	}

	if (MBF_LEFT(mbf) < 2) {
		DPRINTF("mbf_read_2: no more 2byte data: fallback to mbf_read_1\n");

		if ((d1 = mbf_read_1(mbf)) < 0) {
			DPRINTF("mbf_read_2: 1st mbf_read_1 return -1\n");
			return -1;
		}
		if ((d2 = mbf_read_1(mbf)) < 0) {
			DPRINTF("mbf_read_2: 2nd mbf_read_1 return -1\n");
			return -1;
		}

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

	p = (void *)(mtod(mbf->mbf_cur, char *) + mbf->mbf_cur_offset);
	mbf->mbf_cur_offset += 2;
	return p->data;
}

uint32_t
mbf_read_4(struct mbf *mbf)
{
	int d1, d2;
	struct {
		uint32_t data;
	} __attribute__((__packed__)) *p;

	if (mbf->mbf_cur == NULL)
		return -1;

	DPRINTF("mbf_read_4: mbf_cur_offset=%d, mbf_cur->m_len=%d\n",
	    mbf->mbf_cur_offset, mbf->mbf_cur->m_len);

	while (mbf->mbf_cur_offset >= mbf->mbf_cur->m_len) {
		if (mbf->mbf_cur->m_next == NULL) {
			DPRINTF("mbf_read_4: m=%p %d/%d -> m=%p (reach eof)\n",
			    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
			    mbf->mbf_cur->m_next);

			mbf->mbf_size = mbf->mbf_cur_base + mbf->mbf_cur->m_len;
			return -1;
		}

		DPRINTF("mbf_read_4: m=%p %d/%d -> m=%p %d/%d\n",
		    mbf->mbf_cur, mbf->mbf_cur_offset, mbf->mbf_cur->m_len,
		    mbf->mbf_cur->m_next, 0, mbf->mbf_cur->m_next->m_len);

		mbf->mbf_cur_base += mbf->mbf_cur->m_len;
		mbf->mbf_cur_offset = 0;
		mbf->mbf_cur = mbf->mbf_cur->m_next;
	}

	if (MBF_LEFT(mbf) < 4) {
		DPRINTF("mbf_read_4: no more 4byte data: fallback to mbf_read_2\n");

		if ((d1 = mbf_read_2(mbf)) < 0)
			return -1;
		if ((d2 = mbf_read_2(mbf)) < 0)
			return -1;

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

	p = (void *)(mtod(mbf->mbf_cur, char *) + mbf->mbf_cur_offset);
	mbf->mbf_cur_offset += 4;
	return p->data;
}

int
mbf_read(struct mbf *mbf, void *buf, int nbytes)
{
	char *p = buf;
	int l;

	while (nbytes > 0) {
		while ((l = MBF_LEFT(mbf)) <= 0) {
			if (mbf->mbf_cur->m_next == NULL) {
				DPRINTF("mbf_read: reach eof\n");
				goto read_done;
			}
			mbf->mbf_cur_base += mbf->mbf_cur->m_len;
			mbf->mbf_cur_offset = 0;
			mbf->mbf_cur = mbf->mbf_cur->m_next;
		}

		DPRINTF("mbf_read: left=%d, read %d bytes\n", l, nbytes);
		if (l > nbytes) {
			DPRINTF("mbf_read: memcpy %d bytes (enough)\n", nbytes);
			memcpy(p, mtod(mbf->mbf_cur, char *) + mbf->mbf_cur_offset, nbytes);
			p += nbytes;
			mbf->mbf_cur_offset += nbytes;
			break;
		}

		DPRINTF("mbf_read: memcpy %d bytes (partly)\n", l);
		memcpy(p, mtod(mbf->mbf_cur, char *) + mbf->mbf_cur_offset, l);
		p += l;
		mbf->mbf_cur_offset += l;
		nbytes -= l;
	}
read_done:
	return p - (char*)buf;
}


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

	while ((l = MBF_LEFT(mbf)) <= 0) {
		if (mbf->mbf_cur->m_next == NULL) {
			DPRINTF("mbf_getbuf: reach eof\n");
			return NULL;
		}
		mbf->mbf_cur_base += mbf->mbf_cur->m_len;
		mbf->mbf_cur_offset = 0;
		mbf->mbf_cur = mbf->mbf_cur->m_next;
	}

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

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