udp.c

代码在ti的c67系列单片机上实现了完整的TCPIP协议栈

//--------------------------------------------------------------------------
// Ip Stack
//--------------------------------------------------------------------------
// UDP.C
//
// UDP General Functions
//
// Author: Michael A. Denio
// Copyright 1999 by Texas Instruments Inc.
//-------------------------------------------------------------------------
#include <stkmain.h>

// Private and Public Globals
UDPSTATS udps;                      // Stats

PSEUDO   upseudo;                   // Pseudo header for checksum

#define UDP_MSS_DEFAULT 1500        // Default Max seg size

//--------------------------------------------------------------------
// void UdpChecksum( UDPHDR *pbHdr )
//
// Checksums the UDP header
//--------------------------------------------------------------------
void UdpChecksum( UDPHDR *pUdpHdr )
{
    int     tmp1;
    UINT16  *pw;
    UINT32  dwTSum;

    // Get header size in bytes
    tmp1 = (int)HNC16(upseudo.Length);

    // Checksum field is NULL in checksum calculations
    pUdpHdr->UDPChecksum = 0;

    // Checksum the header
    pw = (UINT16 *)pUdpHdr;
    dwTSum = 0;
    for( ; tmp1 > 1; tmp1 -= 2 )
        dwTSum += (UINT32)*pw++;
    if( tmp1 )
        dwTSum += (UINT32)(*pw & 0xFF);

    // Checksum the pseudo header
    pw = (UINT16 *)&upseudo;
    for( tmp1=0; tmp1 < 6; tmp1++ )
        dwTSum += (UINT32)*pw++;

    dwTSum = (dwTSum&0xFFFF) + (dwTSum>>16);
    dwTSum = (dwTSum&0xFFFF) + (dwTSum>>16);
    dwTSum = ~dwTSum;

    // Note checksum is Net/Host byte order independent
    pUdpHdr->UDPChecksum = (UINT16)dwTSum;
}

//--------------------------------------------------------------------
// UdpOutput()
//
// Called when UDP packet should be sent
//--------------------------------------------------------------------
int UdpOutput( HANDLE hSock, UINT8 *buf, INT32 size, INT32 *pRetSize )
{
    HANDLE      hPkt,hFrag,hRoute,hIFTx;
    UINT8       *pb;
    uint        Offset,IPHdrLen;
    INT32       mss;
    UDPHDR      *pUdpHdr;
    uint        length;
    int         error;

    //
    // Bound the size to something we can handle
    //
    // We'll try and get the MTU from the route, then the egress IF,
    // or finally we just assume a default.
    //
    if( (hRoute = SockGetRoute( hSock )) )
    {
        mss = (INT32)RtGetMTU( hRoute );
        hIFTx = 0;
    }
    else if( (hIFTx = SockGetIFTx( hSock )) )
        mss = (INT32)IFGetMTU( hIFTx );
    else
        mss = UDP_MSS_DEFAULT;

    mss -= SockGetIpHdrSize(hSock) + UDPHDR_SIZE;   // Sub off IpHdr & UdpHdr

    if( size > mss )
        return( EMSGSIZE );

    //
    // Create the packet
    // Payload = size
    // Reserve = UDPHDR_SIZE
    //
    length = UDPHDR_SIZE + (uint)size;
    if( !(hPkt = SockCreatePacket( hSock, (uint)size, UDPHDR_SIZE)) )
    {
        udps.dwSndNoPacket++;
        return( ENOBUFS );
    }

    // Get the frag
    hFrag = PktGetFrag( hPkt );

    // Get the buffer parameters
    pb = FragGetBufParams( hFrag, 0, 0, &Offset );

    // Get the IP header len
    IPHdrLen = PktGetSizeNet( hPkt );

    // Assign a UDP header pointer
    pUdpHdr = (UDPHDR *)(pb + Offset + IPHdrLen);

    //
    // Fill in UDP Header
    //

    // Set some UDP header stuff
    pUdpHdr->SrcPort = SockGetLPort(hSock);
    pUdpHdr->DstPort = SockGetFPort(hSock);
    pUdpHdr->Length  = HNC16( length );

    // Copy the data
    mmCopy( (pb + Offset + IPHdrLen + UDPHDR_SIZE), buf, (uint)size );

    // Checksum Header
    upseudo.dwIPSrc  = SockGetLIP(hSock);
    upseudo.dwIPDst  = SockGetFIP(hSock);
    upseudo.Null     = 0;
    upseudo.Protocol = 17;
    upseudo.Length   = pUdpHdr->Length;
    UdpChecksum( pUdpHdr );

    // Indicate the preferred route and IF
    PktSetRoute( hPkt, hRoute );
    PktSetIFTx( hPkt, hIFTx );

    // Count it
    udps.dwSndTotal++;

    // Send the packet
    error=(int)IPTxPacket(hPkt,SockGetOptionFlags(hSock)&FLG_IPTX_SOSUPPORTED);

    if( !error )
        *pRetSize = size;

    return( error );
}

//--------------------------------------------------------------------
// UdpInput()
//
// Rx UDP Packet
//--------------------------------------------------------------------
void UdpInput( HANDLE hPkt )
{
    HANDLE     hFrag,hFragCopy,hSock,hSBRx,hIFTx;
    uint       w,Offset,Valid,IPHdrLen,UDPLen;
    IPN        dwIPSrc,dwIPDst;
    UINT8      *pb;
    IPHDR      *pIpHdr;
    UDPHDR     *pUdpHdr;

    if( !(hFrag = PktGetFrag( hPkt )) )
    {
        DbgPrintf(DBG_ERROR,"UdpInput: No frag data");
        PktFree( hPkt );
        return;
    }

    // Get the size of the IP Header and pointer to our data
    w = PktGetFlags( hPkt );
    if( !(w & FLG_PKT_IP_VALID) || (w & FLG_PKT_LLC_VALID) )
    {
        DbgPrintf(DBG_ERROR,"UdpInput: Bad packet");
        PktFree( hPkt );
        return;
    }

    // Get the buffer parameters
    pb = FragGetBufParams( hFrag, 0, &Valid, &Offset );

    // Get the IP header len
    IPHdrLen = PktGetSizeNet( hPkt );

    // Assign an IP header pointer
    pIpHdr = (IPHDR *)(pb + Offset);

    // Assign a UDP header pointer
    pUdpHdr = (UDPHDR *)(pb + Offset + IPHdrLen);

    // Count the total number of packets in
    udps.dwRcvTotal++;

    // Get the total length of the TCP message
    UDPLen = (uint)(HNC16( pIpHdr->TotalLen )) - IPHdrLen;

    // Check for packet too small
    if( UDPLen < UDPHDR_SIZE )
    {
        udps.dwRcvShort++;
        PktFree( hPkt );
        return;
    }

    // Check for bad header length
    if( pUdpHdr->Length != HNC16(UDPLen) )
    {
        udps.dwRcvBadLen++;
        PktFree( hPkt );
        return;
    }

    // We need some stuff for the pseudo header
    //
    // Get source and destination
    dwIPSrc = RdNet32(&pIpHdr->dwIPSrc);
    dwIPDst = RdNet32(&pIpHdr->dwIPDst);

    // Record the Transport Header Size
    PktSetSizeTP( hPkt, UDPLen );
    PktSetFlagsSet( hPkt, FLG_PKT_TP_VALID );

    if( pUdpHdr->UDPChecksum )
    {
        // Init pseudo header for checksum
        upseudo.dwIPSrc  = dwIPSrc;
        upseudo.dwIPDst  = dwIPDst;
        upseudo.Null     = 0;
        upseudo.Protocol = 17;
        upseudo.Length   = pUdpHdr->Length;

        // Verify checksum
        w = (uint)pUdpHdr->UDPChecksum;
        UdpChecksum( pUdpHdr );
        if( w != (uint)pUdpHdr->UDPChecksum )
        {
            udps.dwRcvBadSum++;
            PktFree( hPkt );
            return;
        }
    }

    // Get the actual number of bytes to "receive"
    UDPLen -= UDPHDR_SIZE;

    // Set the Frag Parms to hand off to a SB
    FragSetAux1( hFrag, 0x10000 | (UINT32)(pUdpHdr->SrcPort) );
    FragSetAux2( hFrag, dwIPSrc );
    FragSetBufParams( hFrag, UDPLen, Offset+IPHdrLen+UDPHDR_SIZE );

    // Handle BROADCAST/MULTICAST
    if( dwIPDst == INADDR_BROADCAST || IN_MULTICAST( dwIPDst ) ||
            (PktGetFlags(hPkt) & (FLG_PKT_MACMCAST|FLG_PKT_MACBCAST)) )
    {
        // Multicast Packet
        // We have to copy the frag for each socket with a matchng PCB
        hSock = 0;
        w = 0;          // Recv Flag
        while( (hSock = SockPcbResolveChain( hSock, SOCKPROT_UDP, 0, dwIPDst,
                                             (uint)pUdpHdr->DstPort, dwIPSrc,
                                             (uint)pUdpHdr->SrcPort)) )
        {
            // Get the broadcast IF of this socket
            hIFTx = SockGetIFTx( hSock );

            // If there is a specified BCAST device, and the packet's
            // Rx device doesn't match it, then we ignore the packet
            if( hIFTx && ( hIFTx != PktGetIFRx(hPkt) ) )
                continue;

            // Flag that we have matched a socket
            w = 1;

            // Get the Linear Receive Buffer
            hSBRx = SockGetRx( hSock );

            if( SBGetSpace( hSBRx ) < (INT32)UDPLen )
                udps.dwRcvFull++;
            else if( hFragCopy = FragCopy( hFrag ) )
            {
                // Give the frag to the SB
                SBWrite( hSBRx, (INT32)UDPLen, 0, hFragCopy );

                // Notify the Socket
                SockNotify( hSock, SOCK_NOTIFY_RCVDATA );
            }
        }

        // If we didn't match anyone, count it
        if( !w )
            udps.dwRcvNoPortB++;

        PktFree( hPkt );
        return;
    }

    // Find a PCB for this packet
    hSock = SockPcbResolve( SOCKPROT_UDP, dwIPDst, (uint)pUdpHdr->DstPort,
                     dwIPSrc, (uint)pUdpHdr->SrcPort, SOCK_RESOLVE_BEST );

    // If there's no PCB, send an ICMP error
    if( !hSock )
    {
        if( !(PktGetFlags( hPkt ) & FLG_PKT_MACBCAST) )
            ICMPGenPacket( pIpHdr, PktGetIFRx( hPkt ), ICMP_UNREACH,
                           ICMP_UNREACH_PORT, 0 );
        udps.dwRcvNoPort++;
        PktFree( hPkt );
        return;
    }

    // Get the Receive Socket Buffer
    hSBRx = SockGetRx( hSock );

    // If there's no space on the receiver queue, then discard the packet
    // with an ICMP error
    if( SBGetSpace( hSBRx ) < (INT32)UDPLen )
    {
        if( !(PktGetFlags( hPkt ) & FLG_PKT_MACBCAST) )
            ICMPGenPacket( pIpHdr, PktGetIFRx( hPkt ),
                           ICMP_SOURCEQUENCH, 0, 0 );
        udps.dwRcvFull++;
        PktFree( hPkt );
        return;
    }

    // Disconnect the frag from the packet and free the packet
    PktSetFrag( hPkt, 0 );
    PktFree( hPkt );

    // Give the frag to the SB
    SBWrite( hSBRx, (INT32)UDPLen, 0, hFrag );

    // Notify the Socket
    SockNotify( hSock, SOCK_NOTIFY_RCVDATA );
}