www.pudn.com > Firewall_PNE_3_3.zip > uipc_mbufutil.c, change:2009-03-16,size:4744b


/* uipc_mbufutil.c - mbuf utility routines used by Firewall */

/* Copyright 1992-2004 Wind River Systems, Inc. */
#include "copyright_wrs.h"

/*
modification history
--------------------
01c,13jul04,myz  move common header files to fw.h
01b,17mar04,myz always use byte access for EXTRACT_XXX and INSERT_XXX macros. 
01a,24nov03,myz copied from BSD/OS uipc_mbufutil.c 
*/

/*
 * These routines replace routines normally found in net/bpf_filter.c
 * They are extracted here so that IP Packet Filtering routines
 * can make use of them.
 * The naming convention is
 *
 *	m_xN - extract N bits
 *	m_sN - store N bits
 *
 * The calling convention is:
 *
 *	TYPE m_xN(struct mbuf *, int offset, int *err [, q128_t *value])
 *	int m_sN(struct mbuf *, int offset, TYPE value)
 *
 */

#include "fw.h"

#define EXTRACT_SHORT(p)\
	((u_short)\
		((u_short)*((u_char *)(p)+0)<<8|\
		 (u_short)*((u_char *)(p)+1)<<0))
#define EXTRACT_LONG(p)\
		((uint32_t)*((u_char *)(p)+0)<<24|\
		 (uint32_t)*((u_char *)(p)+1)<<16|\
		 (uint32_t)*((u_char *)(p)+2)<<8|\
		 (uint32_t)*((u_char *)(p)+3)<<0)
#define INSERT_SHORT(p,v)       (((p)[0] = v >> 8), ((p)[1] = v))
#define INSERT_LONG(p,v)        (((p)[0] = v >> 24), ((p)[1] = v >> 16), \
				 ((p)[2] = v >> 8), ((p)[0] = v))


unsigned long
m_x32(m, k, err)
	register struct mbuf *m;
	register int k;
	register int *err;
{
	register int len;
	register u_char *cp, *np;
	register struct mbuf *m0;

	len = m->m_len;
	while (k >= len) {
		k -= len;
		m = m->m_next;
		if (m == 0) {
			*err = 1;
			return (0);
		}
		len = m->m_len;
	}
	cp = mtod(m, u_char *) + k;
	if (len - k >= 4) {
		*err = 0;
		return (EXTRACT_LONG(cp));
	}
	m0 = m->m_next;
	if (m0 == 0 || m0->m_len + len - k < 4) {
		*err = 1;
		return (0);
	}
	np = mtod(m0, u_char *);
	*err = 0;
	switch (len - k) {
	case 1:
		return ((cp[0] << 24) | (np[0] << 16) | (np[1] << 8) | np[2]);

	case 2:
		return ((cp[0] << 24) | (cp[1] << 16) | (np[0] << 8) | np[1]);

	default:
		return ((cp[0] << 24) | (cp[1] << 16) | (cp[2] << 8) | np[0]);
	}
}

u_short
m_x16(m, k, err)
	register struct mbuf *m;
	register int k;
	register int *err;
{
	register int len;
	register u_char *cp;
	register struct mbuf *m0;

	len = m->m_len;
	while (k >= len) {
		k -= len;
		m = m->m_next;
		if (m == 0) {
			*err = 1;
			return (0);
		}
		len = m->m_len;
	}
	cp = mtod(m, u_char *) + k;
	if (len - k >= 2) {
		*err = 0;
		return (EXTRACT_SHORT(cp));
	}
	m0 = m->m_next;
	if (m0 == 0) {
		*err = 1;
		return (0);
	}
	*err = 0;
	return ((cp[0] << 8) | mtod(m0, u_char *)[0]);
}

u_char
m_x8(m, k, err)  
        register struct mbuf *m;
        register int k;
        register int *err;
{
        register int len;
        
        len = m->m_len;
        while (k >= len) {
                k -= len;
                m = m->m_next;
                if (m == 0) {
                        *err = 1;
                        return (0);
                }
                len = m->m_len;
        }
        *err = 0;
        return (mtod(m, u_char *)[k]);
}   

int
m_s32(m, k, v)
	register struct mbuf *m;
	register int k;
	register unsigned long v;
{
	register int len;
	register u_char *cp, *np;
	register struct mbuf *m0;

	len = m->m_len;
	while (k >= len) {
		k -= len;
		m = m->m_next;
		if (m == 0)
			return (-1);
		len = m->m_len;
	}
	cp = mtod(m, u_char *) + k;
	if (len - k >= 4) {
		INSERT_LONG(cp, v);
		return (0);
	}
	m0 = m->m_next;
	if (m0 == 0 || m0->m_len + len - k < 4)
		return (-1);
	np = mtod(m0, u_char *);
	switch (len - k) {

	case 1:
		cp[0] = v >> 24;
		np[0] = v >> 16;
		np[1] = v >> 8;
		np[2] = v;
		return (0);

	case 2:
		cp[0] = v >> 24;
		cp[1] = v >> 16;
		np[0] = v >> 8;
		np[1] = v;
		return (0);

	default:
		cp[0] = v >> 24;
		cp[1] = v >> 16;
		cp[2] = v >> 8;
		np[0] = v;
		return (0);
	}
	/* should never reach here */
        /* return (-1); */
}

int
m_s16(m, k, v)
	register struct mbuf *m;
	register int k;
	register unsigned int v;
{
	register int len;
	register u_char *cp;
	register struct mbuf *m0;

	len = m->m_len;
	while (k >= len) {
		k -= len;
		m = m->m_next;
		if (m == 0)
			return (-1);
		len = m->m_len;
	}
	cp = mtod(m, u_char *) + k;
	if (len - k >= 2) {
		INSERT_SHORT(cp, v);
		return (0);
	}
	m0 = m->m_next;
	if (m0 == 0)
		return (-1);
	cp[0] = v >> 8;
	mtod(m0, u_char *)[0] = v;
	return (0);
}

int
m_s8(m, k, v)
        register struct mbuf *m;
        register int k;
        register unsigned int v;
{
        register int len;
   
        len = m->m_len;
        while (k >= len) {
                k -= len;
                m = m->m_next;
                if (m == 0)
                        return (-1);
                len = m->m_len;
        }
        mtod(m, u_char *)[k] = v;
        return (0);
}