/* $Id: mbf_subr.c,v 1.1 2008/05/06 20:04:41 ryo Exp $ */ /* * Copyright (c) 2008 SHIMIZU Ryo * 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 #include #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("\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("\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; }