usr/src/uts/common/io/pfmod.c

/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License (the "License").
 * You may not use this file except in compliance with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */
/*
 * Copyright 2006 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */

#pragma ident	"%Z%%M%	%I%	%E% SMI"

/*
 * STREAMS Packet Filter Module
 *
 * This module applies a filter to messages arriving on its read
 * queue, passing on messages that the filter accepts adn discarding
 * the others.  It supports ioctls for setting the filter.
 *
 * On the write side, the module simply passes everything through
 * unchanged.
 *
 * Based on SunOS 4.x version.  This version has minor changes:
 *	- general SVR4 porting stuff
 * 	- change name and prefixes from "nit" buffer to streams buffer
 *	- multithreading assumes configured as D_MTQPAIR
 */

#include <sys/types.h>
#include <sys/sysmacros.h>
#include <sys/errno.h>
#include <sys/debug.h>
#include <sys/time.h>
#include <sys/stropts.h>
#include <sys/stream.h>
#include <sys/conf.h>
#include <sys/ddi.h>
#include <sys/sunddi.h>
#include <sys/kmem.h>
#include <sys/strsun.h>
#include <sys/pfmod.h>
#include <sys/modctl.h>

/*
 * Expanded version of the Packetfilt structure that includes
 * some additional fields that aid filter execution efficiency.
 */
struct epacketfilt {
	struct Pf_ext_packetfilt	pf;
#define	pf_Priority	pf.Pf_Priority
#define	pf_FilterLen	pf.Pf_FilterLen
#define	pf_Filter	pf.Pf_Filter
	/* pointer to word immediately past end of filter */
	ushort_t		*pf_FilterEnd;
	/* length in bytes of packet prefix the filter examines */
	ushort_t		pf_PByteLen;
};

/*
 * (Internal) packet descriptor for FilterPacket
 */
struct packdesc {
	ushort_t	*pd_hdr;	/* header starting address */
	uint_t		pd_hdrlen;	/* header length in shorts */
	ushort_t	*pd_body;	/* body starting address */
	uint_t		pd_bodylen;	/* body length in shorts */
};


/*
 * Function prototypes.
 */
static	int	pfopen(queue_t *, dev_t *, int, int, cred_t *);
static	int	pfclose(queue_t *);
static void	pfioctl(queue_t *wq, mblk_t *mp);
static	int	FilterPacket(struct packdesc *, struct epacketfilt *);
/*
 * To save instructions, since STREAMS ignores the return value
 * from these functions, they are defined as void here. Kind of icky, but...
 */
static void	pfwput(queue_t *, mblk_t *);
static void	pfrput(queue_t *, mblk_t *);

static struct module_info pf_minfo = {
	22,		/* mi_idnum */
	"pfmod",	/* mi_idname */
	0,		/* mi_minpsz */
	INFPSZ,		/* mi_maxpsz */
	0,		/* mi_hiwat */
	0		/* mi_lowat */
};

static struct qinit pf_rinit = {
	(int (*)())pfrput,	/* qi_putp */
	NULL,
	pfopen,			/* qi_qopen */
	pfclose,		/* qi_qclose */
	NULL,			/* qi_qadmin */
	&pf_minfo,		/* qi_minfo */
	NULL			/* qi_mstat */
};

static struct qinit pf_winit = {
	(int (*)())pfwput,	/* qi_putp */
	NULL,			/* qi_srvp */
	NULL,			/* qi_qopen */
	NULL,			/* qi_qclose */
	NULL,			/* qi_qadmin */
	&pf_minfo,		/* qi_minfo */
	NULL			/* qi_mstat */
};

static struct streamtab pf_info = {
	&pf_rinit,	/* st_rdinit */
	&pf_winit,	/* st_wrinit */
	NULL,		/* st_muxrinit */
	NULL		/* st_muxwinit */
};

static struct fmodsw fsw = {
	"pfmod",
	&pf_info,
	D_MTQPAIR | D_MP
};

static struct modlstrmod modlstrmod = {
	&mod_strmodops, "streams packet filter module", &fsw
};

static struct modlinkage modlinkage = {
	MODREV_1, &modlstrmod, NULL
};

int
_init(void)
{
	return (mod_install(&modlinkage));
}

int
_fini(void)
{
	return (mod_remove(&modlinkage));
}

int
_info(struct modinfo *modinfop)
{
	return (mod_info(&modlinkage, modinfop));
}

/*ARGSUSED*/
static int
pfopen(queue_t *rq, dev_t *dev, int oflag, int sflag, cred_t *crp)
{
	struct epacketfilt	*pfp;

	ASSERT(rq);

	if (sflag != MODOPEN)
		return (EINVAL);

	if (rq->q_ptr)
		return (0);

	/*
	 * Allocate and initialize per-Stream structure.
	 */
	pfp = kmem_alloc(sizeof (struct epacketfilt), KM_SLEEP);
	rq->q_ptr = WR(rq)->q_ptr = (char *)pfp;

	qprocson(rq);

	return (0);
}

static int
pfclose(queue_t	*rq)
{
	struct	epacketfilt	*pfp = (struct epacketfilt *)rq->q_ptr;

	ASSERT(pfp);

	qprocsoff(rq);

	kmem_free(pfp, sizeof (struct epacketfilt));
	rq->q_ptr = WR(rq)->q_ptr = NULL;

	return (0);
}

/*
 * Write-side put procedure.  Its main task is to detect ioctls.
 * Other message types are passed on through.
 */
static void
pfwput(queue_t *wq, mblk_t *mp)
{
	switch (mp->b_datap->db_type) {
	case M_IOCTL:
		pfioctl(wq, mp);
		break;

	default:
		putnext(wq, mp);
		break;
	}
}

/*
 * Read-side put procedure.  It's responsible for applying the
 * packet filter and passing upstream message on or discarding it
 * depending upon the results.
 *
 * Upstream messages can start with zero or more M_PROTO mblks
 * which are skipped over before executing the packet filter
 * on any remaining M_DATA mblks.
 */
static void
pfrput(queue_t *rq, mblk_t *mp)
{
	struct	epacketfilt	*pfp = (struct epacketfilt *)rq->q_ptr;
	mblk_t	*mbp, *mpp;
	struct	packdesc	pd;
	int	need;

	ASSERT(pfp);

	switch (DB_TYPE(mp)) {
	case M_PROTO:
	case M_DATA:
		/*
		 * Skip over protocol information and find the start
		 * of the message body, saving the overall message
		 * start in mpp.
		 */
		for (mpp = mp; mp && (DB_TYPE(mp) == M_PROTO); mp = mp->b_cont)
			;

		/*
		 * Null body (exclusive of M_PROTO blocks) ==> accept.
		 * Note that a null body is not the same as an empty body.
		 */
		if (mp == NULL) {
			putnext(rq, mpp);
			break;
		}

		/*
		 * Pull the packet up to the length required by
		 * the filter.  Note that doing so destroys sharing
		 * relationships, which is unfortunate, since the
		 * results of pulling up here are likely to be useful
		 * for shared messages applied to a filter on a sibling
		 * stream.
		 *
		 * Most packet sources will provide the packet in two
		 * logical pieces: an initial header in a single mblk,
		 * and a body in a sequence of mblks hooked to the
		 * header.  We're prepared to deal with variant forms,
		 * but in any case, the pullup applies only to the body
		 * part.
		 */
		mbp = mp->b_cont;
		need = pfp->pf_PByteLen;
		if (mbp && (MBLKL(mbp) < need)) {
			int len = msgdsize(mbp);

			/* XXX discard silently on pullupmsg failure */
			if (pullupmsg(mbp, MIN(need, len)) == 0) {
				freemsg(mpp);
				break;
			}
		}

		/*
		 * Misalignment (not on short boundary) ==> reject.
		 */
		if (((uintptr_t)mp->b_rptr & (sizeof (ushort_t) - 1)) ||
		    (mbp != NULL &&
		    ((uintptr_t)mbp->b_rptr & (sizeof (ushort_t) - 1)))) {
			freemsg(mpp);
			break;
		}

		/*
		 * These assignments are distasteful, but necessary,
		 * since the packet filter wants to work in terms of
		 * shorts.  Odd bytes at the end of header or data can't
		 * participate in the filtering operation.
		 */
		pd.pd_hdr = (ushort_t *)mp->b_rptr;
		pd.pd_hdrlen = (mp->b_wptr - mp->b_rptr) / sizeof (ushort_t);
		if (mbp) {
			pd.pd_body = (ushort_t *)mbp->b_rptr;
			pd.pd_bodylen = (mbp->b_wptr - mbp->b_rptr) /
							sizeof (ushort_t);
		} else {
			pd.pd_body = NULL;
			pd.pd_bodylen = 0;
		}

		/*
		 * Apply the filter.
		 */
		if (FilterPacket(&pd, pfp))
			putnext(rq, mpp);
		else
			freemsg(mpp);

		break;

	default:
		putnext(rq, mp);
		break;
	}

}

/*
 * Handle write-side M_IOCTL messages.
 */
static void
pfioctl(queue_t *wq, mblk_t *mp)
{
	struct	epacketfilt	*pfp = (struct epacketfilt *)wq->q_ptr;
	struct	Pf_ext_packetfilt	*upfp;
	struct	packetfilt	*opfp;
	ushort_t	*fwp;
	int	arg;
	int	maxoff = 0;
	int	maxoffreg = 0;
	struct iocblk	*iocp = (struct iocblk *)mp->b_rptr;
	int	error;

	switch (iocp->ioc_cmd) {
	case PFIOCSETF:
		/*
		 * Verify argument length. Since the size of packet filter
		 * got increased (ENMAXFILTERS was bumped up to 2047), to
		 * maintain backwards binary compatibility, we need to
		 * check for both possible sizes.
		 */
		switch (iocp->ioc_count) {
		case sizeof (struct Pf_ext_packetfilt):
			error = miocpullup(mp,
			    sizeof (struct Pf_ext_packetfilt));
			if (error != 0) {
				miocnak(wq, mp, 0, error);
				return;
			}
			upfp = (struct Pf_ext_packetfilt *)mp->b_cont->b_rptr;
			if (upfp->Pf_FilterLen > PF_MAXFILTERS) {
				miocnak(wq, mp, 0, EINVAL);
				return;
			}

			bcopy(upfp, pfp, sizeof (struct Pf_ext_packetfilt));
			pfp->pf_FilterEnd = &pfp->pf_Filter[pfp->pf_FilterLen];
			break;

		case sizeof (struct packetfilt):
			error = miocpullup(mp, sizeof (struct packetfilt));
			if (error != 0) {
				miocnak(wq, mp, 0, error);
				return;
			}
			opfp = (struct packetfilt *)mp->b_cont->b_rptr;
			/* this strange comparison keeps gcc from complaining */
			if (opfp->Pf_FilterLen - 1 >= ENMAXFILTERS) {
				miocnak(wq, mp, 0, EINVAL);
				return;
			}

			pfp->pf.Pf_Priority = opfp->Pf_Priority;
			pfp->pf.Pf_FilterLen = (unsigned int)opfp->Pf_FilterLen;

			bcopy(opfp->Pf_Filter, pfp->pf.Pf_Filter,
			    sizeof (opfp->Pf_Filter));
			pfp->pf_FilterEnd = &pfp->pf_Filter[pfp->pf_FilterLen];
			break;

		default:
			miocnak(wq, mp, 0, EINVAL);
			return;
		}

		/*
		 * Find and record maximum byte offset that the
		 * filter users.  We use this when executing the
		 * filter to determine how much of the packet
		 * body to pull up.  This code depends on the
		 * filter encoding.
		 */
		for (fwp = pfp->pf_Filter; fwp < pfp->pf_FilterEnd; fwp++) {
			arg = *fwp & ((1 << ENF_NBPA) - 1);
			switch (arg) {
			default:
				if ((arg -= ENF_PUSHWORD) > maxoff)
					maxoff = arg;
				break;

			case ENF_LOAD_OFFSET:
				/* Point to the offset */
				fwp++;
				if (*fwp > maxoffreg)
					maxoffreg = *fwp;
				break;

			case ENF_PUSHLIT:
			case ENF_BRTR:
			case ENF_BRFL:
				/* Skip over the literal. */
				fwp++;
				break;

			case ENF_PUSHZERO:
			case ENF_PUSHONE:
			case ENF_PUSHFFFF:
			case ENF_PUSHFF00:
			case ENF_PUSH00FF:
			case ENF_NOPUSH:
			case ENF_POP:
				break;
			}
		}

		/*
		 * Convert word offset to length in bytes.
		 */
		pfp->pf_PByteLen = (maxoff + maxoffreg + 1) * sizeof (ushort_t);
		miocack(wq, mp, 0, 0);
		break;

	default:
		putnext(wq, mp);
		break;
	}
}

/* #define	DEBUG	1 */
/* #define	INNERDEBUG	1 */

#ifdef	INNERDEBUG
#define	enprintf(a)	printf a
#else
#define	enprintf(a)
#endif

/*
 * Apply the packet filter given by pfp to the packet given by
 * pp.  Return nonzero iff the filter accepts the packet.
 *
 * The packet comes in two pieces, a header and a body, since
 * that's the most convenient form for our caller.  The header
 * is in contiguous memory, whereas the body is in a mbuf.
 * Our caller will have adjusted the mbuf chain so that its first
 * min(MLEN, length(body)) bytes are guaranteed contiguous.  For
 * the sake of efficiency (and some laziness) the filter is prepared
 * to examine only these two contiguous pieces.  Furthermore, it
 * assumes that the header length is even, so that there's no need
 * to glue the last byte of header to the first byte of data.
 */

#define	opx(i)	((i) >> ENF_NBPA)

static int
FilterPacket(struct packdesc *pp, struct epacketfilt *pfp)
{
	int		maxhdr = pp->pd_hdrlen;
	int		maxword = maxhdr + pp->pd_bodylen;
	ushort_t	*sp;
	ushort_t	*fp;
	ushort_t	*fpe;
	unsigned	op;
	unsigned	arg;
	unsigned	offreg = 0;
	ushort_t	stack[ENMAXFILTERS+1];

	fp = &pfp->pf_Filter[0];
	fpe = pfp->pf_FilterEnd;

	enprintf(("FilterPacket(%p, %p, %p, %p):\n", pp, pfp, fp, fpe));

	/*
	 * Push TRUE on stack to start.  The stack size is chosen such
	 * that overflow can't occur -- each operation can push at most
	 * one item on the stack, and the stack size equals the maximum
	 * program length.
	 */
	sp = &stack[ENMAXFILTERS];
	*sp = 1;

	while (fp < fpe) {
	op = *fp >> ENF_NBPA;
	arg = *fp & ((1 << ENF_NBPA) - 1);
	fp++;

	switch (arg) {
	default:
		arg -= ENF_PUSHWORD;
		/*
		 * Since arg is unsigned,
		 * if it were less than ENF_PUSHWORD before,
		 * it would now be huge.
		 */
		if (arg + offreg < maxhdr)
			*--sp = pp->pd_hdr[arg + offreg];
		else if (arg + offreg < maxword)
			*--sp = pp->pd_body[arg - maxhdr + offreg];
		else {
			enprintf(("=>0(len)\n"));
			return (0);
		}
		break;
	case ENF_PUSHLIT:
		*--sp = *fp++;
		break;
	case ENF_PUSHZERO:
		*--sp = 0;
		break;
	case ENF_PUSHONE:
		*--sp = 1;
		break;
	case ENF_PUSHFFFF:
		*--sp = 0xffff;
		break;
	case ENF_PUSHFF00:
		*--sp = 0xff00;
		break;
	case ENF_PUSH00FF:
		*--sp = 0x00ff;
		break;
	case ENF_LOAD_OFFSET:
		offreg = *fp++;
		break;
	case ENF_BRTR:
		if (*sp != 0)
			fp += *fp;
		else
			fp++;
		if (fp >= fpe) {
			enprintf(("BRTR: fp>=fpe\n"));
			return (0);
		}
		break;
	case ENF_BRFL:
		if (*sp == 0)
			fp += *fp;
		else
			fp++;
		if (fp >= fpe) {
			enprintf(("BRFL: fp>=fpe\n"));
			return (0);
		}
		break;
	case ENF_POP:
		++sp;
		if (sp > &stack[ENMAXFILTERS]) {
			enprintf(("stack underflow\n"));
			return (0);
		}
		break;
	case ENF_NOPUSH:
		break;
	}

	if (sp < &stack[2]) {	/* check stack overflow: small yellow zone */
		enprintf(("=>0(--sp)\n"));
		return (0);
	}

	if (op == ENF_NOP)
		continue;

	/*
	 * all non-NOP operators binary, must have at least two operands
	 * on stack to evaluate.
	 */
	if (sp > &stack[ENMAXFILTERS-2]) {
		enprintf(("=>0(sp++)\n"));
		return (0);
	}

	arg = *sp++;
	switch (op) {
	default:
		enprintf(("=>0(def)\n"));
		return (0);
	case opx(ENF_AND):
		*sp &= arg;
		break;
	case opx(ENF_OR):
		*sp |= arg;
		break;
	case opx(ENF_XOR):
		*sp ^= arg;
		break;
	case opx(ENF_EQ):
		*sp = (*sp == arg);
		break;
	case opx(ENF_NEQ):
		*sp = (*sp != arg);
		break;
	case opx(ENF_LT):
		*sp = (*sp < arg);
		break;
	case opx(ENF_LE):
		*sp = (*sp <= arg);
		break;
	case opx(ENF_GT):
		*sp = (*sp > arg);
		break;
	case opx(ENF_GE):
		*sp = (*sp >= arg);
		break;

	/* short-circuit operators */

	case opx(ENF_COR):
		if (*sp++ == arg) {
			enprintf(("=>COR %x\n", *sp));
			return (1);
		}
		break;
	case opx(ENF_CAND):
		if (*sp++ != arg) {
			enprintf(("=>CAND %x\n", *sp));
			return (0);
		}
		break;
	case opx(ENF_CNOR):
		if (*sp++ == arg) {
			enprintf(("=>COR %x\n", *sp));
			return (0);
		}
		break;
	case opx(ENF_CNAND):
		if (*sp++ != arg) {
			enprintf(("=>CNAND %x\n", *sp));
			return (1);
		}
		break;
	}
	}
	enprintf(("=>%x\n", *sp));
	return (*sp);
}