bpfdrv.c

VXWORKS源代码

    if (pBpfProg->bf_insns == NULL)
        {
	/* If no filter instructions then filter instuction length must be 0 */

        if (pBpfProg->bf_len != 0)
            {
            return (EINVAL);
            }

        /* Set filter to NULL and reset device buffers */

        pBpfDev->pBpfInsnFilter = NULL;
        pBpfDev->filterLen = 0;
        bpfDevReset (pBpfDev);

        if (pBpfInsnOld != NULL)
            {
            free (pBpfInsnOld);
            return (OK);
            }
        }

    filterLen = pBpfProg->bf_len;
    if (filterLen > BPF_MAXINSNS)
        return (EINVAL);

    if (filterLen != pBpfDev->filterLen)
        newFlag = TRUE;

    pBpfDev->filterLen = filterLen;
    filterSize = filterLen * sizeof (*pBpfProg->bf_insns);

    if (newFlag)
        {
        pBpfInsnNew = (BPF_INSN *)malloc (filterSize);
        if (pBpfInsnNew == NULL)
            {
            return (ENOMEM);
            }
        }
    else
        pBpfInsnNew = pBpfInsnOld;

    /* Copy instructions from given program to internal storage */

    bcopy ((char *) pBpfProg->bf_insns, (char *) pBpfInsnNew, filterSize);

    /* 
     * Validate the filter.  Since the filter is executed as part of the
     * critical receive path, validation prevents crashing the network stack.
     */

    if (bpf_validate (pBpfInsnNew, filterLen))
        {
        pBpfDev->pBpfInsnFilter = pBpfInsnNew;

        bpfDevReset (pBpfDev);
        if (newFlag && pBpfInsnOld != NULL)
            {
            free (pBpfInsnOld);
            }
        return (OK);
        }

    /* New filter is invalid.  Free filter if necessary and return EINVAL */

    if (newFlag)
        free (pBpfInsnNew);

    return (EINVAL);
    }

/*******************************************************************************
*
* bpfIfSet - attaches BPF device to a network interface
*
* When <restartFlag> is TRUE, this routine reattaches a BPF device to a
* network interface (for the BIOCSTART ioctl). Otherwise, it handles
* the BIOCSETIF ioctl by detaching a BPF device from its current network
* interface, if any, and then attaching to a new network interface.
*
* RETURNS:
* OK, EINVAL, or ENOBUFS
*
* CAVEATS:
* Must be called by a task which holds the unit-specific _bpfUnitLock.
*
* INTERNAL
* This routine returns errno values if a failure occurs so that the ioctl
* mechanism can set the error number appropriately.
* 
* NOMANUAL
*/

LOCAL STATUS bpfIfSet
    (
    BPF_DEV_CTRL * pBpfDev,   /* BPF device control structure */
    struct ifreq * pIfReq,    /* Name of the network interface to attach to */
    BOOL 	restartFlag 	/* Create a new attachment or renew one? */
    )
    {
    char * pDevName;
    char devName [END_NAME_MAX + 1];
    int unit;
    int lockKey;     /* Blocks any other BPF operations. */
    STATUS result;

    BOOL attachFlag = FALSE; 	/* New attachment to a network interface? */

    END_OBJ * pEnd = NULL; 	/* Handle for END device. */
    struct ifnet * pIf = NULL; 	/* Handle for BSD device. */

    /* Validate the name of the network interface. */

    if (pIfReq == NULL)
        return (EINVAL);

    pDevName = pIfReq->ifr_name;

    if (*pDevName == EOS)
        return (EINVAL);    /* No device name given. */

    while (*pDevName != EOS && !isdigit ( (int) (*pDevName)))
        pDevName++;

    if (*pDevName != EOS)
        unit = atoi (pDevName);
    else
        return (EINVAL);    /* No unit number included in device name. */

    if (unit < 0)
        return (EINVAL);    /* Invalid unit number. */

    bzero (devName, END_NAME_MAX + 1);
    strncpy (devName, pIfReq->ifr_name,
             min (END_NAME_MAX, pDevName - pIfReq->ifr_name));

    pEnd = endFindByName (devName, unit);
    if (pEnd == NULL)
        {
        /* Check for BSD device. */

        pIf = ifunit (pIfReq->ifr_name);
        if (pIf == NULL)
            return (EINVAL);    /* No such device. */
        }

    /* Allocate buffers for BPF unit if it hasn't been done yet. */

    if (!restartFlag && pBpfDev->recvStoreBuf == NULL)
        {
        if (bpfBufsAlloc (pBpfDev) != OK)
            {
            return (ENOMEM);
            }
        }

    /*
     * Bind BPF unit to network interface if not already attached to it.
     * Prevent any other BPF units in the device from changing the list
     * of network interface attachments.
     */

    _bpfDevLock (pBpfDev->bpfDevId);    /* Preserve attachments list. */

    if (pBpfDev->pNetDev == NULL)
        attachFlag = TRUE;    /* New attachment for BPF device. */
    else if (pEnd)
        {
        /* Check if changing attachments to new END device. */

        if ( (pBpfDev->pNetDev->bsdFlag == TRUE) ||
            (PCOOKIE_TO_ENDOBJ(pBpfDev->pNetDev->pCookie) != pEnd))
            attachFlag = TRUE;
        }
    else
        {
        /* Check if changing attachments to new BSD device. */

        if ( (pBpfDev->pNetDev->bsdFlag == FALSE) ||
            (pBpfDev->pNetDev->pCookie != pIf))
            attachFlag = TRUE;
        }

    if (attachFlag)
        {
        /* 
         * Detach BPF unit if attached to another network interface.
         * Only possible for BIOCSETIF (when restartFlag is FALSE). 
         */

        if (pBpfDev->pNetDev != NULL)
            {
            _bpfDevDetach (pBpfDev);
            }

        /* Attach to the new network interface. */

        lockKey = _bpfLock ();
        result = _bpfProtoAttach (pBpfDev, devName, unit, pIf);
        _bpfUnlock (lockKey);

        if (result != OK)
            {
            _bpfDevUnlock (pBpfDev->bpfDevId);
            return (result);
            }

        /* Add the BPF unit to the network device's list. */

        lstAdd ( & (pBpfDev->pNetDev->bpfUnitList), (NODE *) pBpfDev);
        }

    _bpfDevUnlock (pBpfDev->bpfDevId);

    /*
     * For BIOCSETIF operation, reset buffers even if re-attaching to
     * the same network interface.
     */

    if (!restartFlag)
        bpfDevReset (pBpfDev);

    return (OK);
    }

/*******************************************************************************
*
* bpfPacketCatch - copy a packet to the BPF device store buffer
*
* This routine copies a packet to the BPF device store buffer. If required, it
* will also wakeup any pended read tasks.
*
* RETURNS:
* Not Applicable
*
* CAVEATS:
* Must be called by a task which holds the unit-specific _bpfUnitLock.
*
* NOMANUAL
*/

LOCAL void bpfPacketCatch
    (
    FAST BPF_DEV_CTRL * pBpfDev,    /* BPF device */
    FAST M_BLK_ID     pMBlk,        /* packet to copy to BPF device */
    FAST int          snapLen,      /* bytes of packet to copy */
    FAST int          bpfHdrLen,    /* len of bpf hdr + padding */
    struct timeval    * pTimeVal,   /* timestamp when packet was received */   
    FAST int          linkHdrLen    /* size of packet's link level header */
    )
    {
    FAST struct bpf_hdr * pBpfHdr;  /* BPF packet header */
    FAST int totLen;                /* total len of header + packet */
    FAST int curLen = 0;            /* current len of buffer */

    /* snapLen of -1 means copy entire packet */

    if (snapLen == -1) 
	snapLen = pMBlk->mBlkPktHdr.len;

    /*
     * Figure out how many bytes to move.  If the packet is
     * greater or equal to the snapshot length, transfer that
     * much.  Otherwise, transfer the whole packet (unless
     * we hit the buffer size limit).
     */

    totLen = bpfHdrLen + min (snapLen, pMBlk->mBlkPktHdr.len);
    if (totLen > pBpfDev->bufSize)
	{
	totLen = pBpfDev->bufSize;
	}

    /* Round up the end of the previous packet to the next longword. */
    
    curLen = BPF_WORDALIGN (pBpfDev->recvStoreLen);
    if (curLen + totLen > pBpfDev->bufSize)
	{

	/*
	 * This packet will overflow the storage buffer.
	 * Rotate the buffers if we can, then wakeup any
	 * pending reads.
	 */
	
	if (pBpfDev->freeBuf == NULL)
	    {

	    /*
	     * We haven't completed the previous read yet, 
	     * so drop the packet. 
	     */
	    
	    ++pBpfDev->pktDropCount;
	    return;
	    }

	ROTATE_BUFFERS (pBpfDev);
	bpfWakeup (pBpfDev);
	curLen = 0;
	}
    else if (pBpfDev->immediateMode)
	{

	/*
	 * Immediate mode is set.  A packet arrived so any
	 * pending reads should be triggered.
	 */

	bpfWakeup (pBpfDev);
	}
    
    /* Fill bpf header */

    pBpfHdr = (struct bpf_hdr *) (pBpfDev->recvStoreBuf + curLen);

    pBpfHdr->bh_tstamp = * pTimeVal;
    pBpfHdr->bh_caplen = totLen - bpfHdrLen;
    pBpfHdr->bh_datalen = pMBlk->mBlkPktHdr.len;
    pBpfHdr->bh_hdrlen = bpfHdrLen;
    pBpfHdr->bh_linklen = linkHdrLen;

    /* Copy the packet data into the store buffer and update its length. */
    
    netMblkOffsetToBufCopy (pMBlk, 0, (char *) pBpfHdr + bpfHdrLen, 
			    totLen - bpfHdrLen, NULL);
    pBpfDev->recvStoreLen = curLen + totLen;
    return;
    }

/*******************************************************************************
*
* bpfBufsAlloc - allocate space for BPF device buffers
*
* This routine allocates space for BPF device buffers.  Two buffers of size 
* <pBpfDev>->bufSize will be allocated from system memory. 
* 
* RETURNS:
* OK or ERROR
*
* ERRNO:
* None
*
* CAVEATS:
* Must be called by a task which holds the unit-specific _bpfUnitLock.
*
* INTERNAL
* This routine only executes within the ioctl mechanism, which sets
* the error number to ENOMEM if it fails.
*
* NOMANUAL
*/

LOCAL STATUS bpfBufsAlloc 
    (
    BPF_DEV_CTRL * pBpfDev   /* BPF device */
    )
    {
    pBpfDev->freeBuf = malloc (pBpfDev->bufSize);
    if (pBpfDev->freeBuf == NULL)
        return (ERROR);

    pBpfDev->recvStoreBuf = malloc (pBpfDev->bufSize);
    if (pBpfDev->recvStoreBuf == NULL)
        {
        free (pBpfDev->freeBuf);
        return (ERROR);
        }
    pBpfDev->recvStoreLen = 0;
    pBpfDev->readHoldLen = 0;
    return (OK);
    }

/*******************************************************************************
*
* bpfDevFree - free buffers in use by a BPF device
*
* This routine frees the buffers in use by a BPF device.  It then marks the
* BPF device as being no longer in use.
*
* RETURNS:
* Not Applicable
*
* CAVEATS:
* Must be called by a task which holds the unit-specific _bpfUnitLock.
*
* NOMANUAL
*/

LOCAL void bpfDevFree
    (
    FAST BPF_DEV_CTRL * pBpfDev
    )
    {
    if (pBpfDev->recvStoreBuf != NULL)
        {
        free (pBpfDev->recvStoreBuf);
        }

    if (pBpfDev->readHoldBuf != NULL)
        {
        free (pBpfDev->readHoldBuf);
        }

    if (pBpfDev->freeBuf != NULL)
        {
        free (pBpfDev->freeBuf);
        }

    if (pBpfDev->pBpfInsnFilter != NULL)
        {
        free (pBpfDev->pBpfInsnFilter);
        }

    semDelete (pBpfDev->readWkupSem);

    /* Protect status change against conflicts with bpfOpen routine. */

    _bpfDevLock (pBpfDev->bpfDevId);

    pBpfDev->inUse = FALSE;

    _bpfDevUnlock (pBpfDev->bpfDevId);

    return;
    }

/*******************************************************************************
*
* bpfWakeup - wakes up all tasks pended on a BPF device
*
* This routine wakes up all tasks waiting for a BPF device to receive
* data. Tasks pended within an active read are triggered by the internal
* semaphore. Any tasks waiting with the select operation are started
* with the appropriate method.
*
* RETURNS:
* Not Applicable
*
* CAVEATS:
* Must be called by a task which holds the unit-specific _bpfUnitLock.
*
* NOMANUAL
*/

LOCAL void bpfWakeup 
    (
    BPF_DEV_CTRL * pBpfDev   /* BPF device control structure */
    )
    {
    /* Wake all tasks pending in a read operation. */

    semFlush (pBpfDev->readWkupSem);

    /* Wake any tasks pending within a select operation. */

    if (pBpfDev->selectFlag)
        selWakeupAll (& (pBpfDev->selWkupList), SELREAD);

    return;
    }

/*******************************************************************************
*
* bpfSleep - block on a BPF device waiting for packets to arrive
*
* This routine will block on a BPF device to wait for packets to arrive.  When
* a packet arrives the pended task will be signalled.  If <numTicks> is 
* WAIT_FOREVER the task will block until packet arrival.  Otherwise the task
* will unblock when <numTicks> expires.
*
* RETURNS:
* OK
* EWOULDBLOCK - if <numTicks> expires before packets arrive
* EPIPE       - if the attached network interface is shut down or the
*               BPF unit is closed.
*
* CAVEATS:
* Must be called by a task which holds the unit-specific _bpfUnitLock.
*
* NOMANUAL
*/

LOCAL int bpfSleep
    (
    BPF_DEV_CTRL * pBpfDev,
    int numTicks
    )
    {
    int error = OK;

    if (semTake (pBpfDev->readWkupSem, numTicks) != OK)
        {
        if (errnoOfTaskGet (taskIdSelf ()) == S_objLib_OBJ_TIMEOUT)
            error = EWOULDBLOCK;
        else
            error = EPIPE;
        }

    return (error);
    }

/*******************************************************************************
*
* bpfTimeStamp - returns a timestamp value
*
* This routine returns a timestamp value in <pTimeval>.
*
* RETURNS:
* Not Applicable
*
* NOMANUAL
*/

LOCAL void bpfTimeStamp 
    (
    struct timeval * pTimeval   /* holds returned timestamp value */
    )
    {
    static int timeStamp = 0;

    pTimeval->tv_sec = ++timeStamp;
    pTimeval->tv_usec = 0;
    return;
    }