/* $Id: mbh_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 MBH_TEST_STANDALONE #include "mbh_standalone.h" #else #include #include #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("\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("\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; }