icmp.c

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

//--------------------------------------------------------------------------
// Ip Stack
//--------------------------------------------------------------------------
// ICMP.C
//
// Routines related to ICMP
//
//
// Author: Michael A. Denio
// Copyright 1999 by Texas Instruments Inc.
//-------------------------------------------------------------------------
#include <stkmain.h>

UINT32   _dwICMPInErrors  = 0;
UINT32   _dwICMPOutErrors = 0;
UINT32   _dwICMPIn[ ICMP_MAXTYPE+1 ];
UINT32   _dwICMPOut[ ICMP_MAXTYPE+1 ];

//--------------------------------------------------------------------
// void ICMPChecksum( ICMPHDR *picmp )
//
// Checksums an ICMP header
//--------------------------------------------------------------------
void ICMPChecksum( ICMPHDR *pIcHdr, uint Size )
{
    int     tmp1;
    UINT16  *pw;
    UINT32  dwTSum;

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

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

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

//--------------------------------------------------------------------
// ICMPGenPacket( pbHdr, hIFRx, Type, Code, dwAux )
//
// Generates 8 byte ICMP Message Packet
//--------------------------------------------------------------------
void ICMPGenPacket
(
    IPHDR  *pIpHdr,
    HANDLE hIFRx,
    uint   Type,
    uint   Code,
    UINT32 dwAux
)
{
    HANDLE   hPkt,hFrag;
    uint     Offset,IPHdrLen,ICMPLen;
    UINT8    *pb;
    ICMPHDR  *pIcHdr;
    IPN      dwIPSrc,dwIPTmp;

    // Don't let idiots crash us
    if( Type > ICMP_MAXTYPE )
    {
        _dwICMPOutErrors++;
        return;
    }

    // Get the source of this disaster
    dwIPSrc = RdNet32( &pIpHdr->dwIPSrc );

    // Get the IP header len
    IPHdrLen = (pIpHdr->VerLen & 0xF) * 4;

    // Don't send ICMP for anything but first fragment
    if( pIpHdr->FlagOff & ~(HNC16(IP_DF|IP_MF)) )
        return;

    // Don't ICMP to bad address
    if( dwIPSrc == INADDR_ANY || dwIPSrc == INADDR_BROADCAST ||
            IN_EXPERIMENTAL(dwIPSrc) || IN_MULTICAST(dwIPSrc) ||
            IN_LOOPBACK(dwIPSrc) )
        return;

    // Don't ICMP because of a packet sent to a bad address
    dwIPTmp = RdNet32( &pIpHdr->dwIPDst );
    if( dwIPTmp == INADDR_ANY || dwIPTmp == INADDR_BROADCAST ||
            IN_EXPERIMENTAL(dwIPTmp) || IN_MULTICAST(dwIPTmp) ||
            IN_LOOPBACK(dwIPTmp) )
        return;

    // Bump the stats
    _dwICMPOut[ Type ]++;

    //
    // Create the packet
    // Payload = ICMPHDR + 4 bytes, PLUS the org IP header + 8 bytes
    // Reserve = Reserve size for a STANDARD IP header.
    //
    ICMPLen = ICMPHDR_SIZE + 4 + IPHdrLen + 8;
    if( !(hPkt = IFCreatePacket( ICMPLen, IPHDR_SIZE, 0 )) )
    {
        _dwICMPOutErrors++;
        return;
    }

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

    // Get a pointer to the new ICMP header
    pb        = FragGetBufParams( hFrag, 0, 0, 0 );
    Offset    = PktGetSizeLLC( hPkt ) + IPHDR_SIZE;
    pIcHdr    = (ICMPHDR *)(pb + Offset);

    // Set the type and code
    pIcHdr->Type = (UINT8)Type;
    pIcHdr->Code = (UINT8)Code;

    // The next four bytes are supplied to us in dwAux
    WrNet32( pIcHdr->Data, dwAux );

    // Copy the original IP header, plus 8 bytes of original payload
    mmCopy( pIcHdr->Data+4, pIpHdr, IPHdrLen+8 );

    // Checksum the ICMP header
    ICMPChecksum( pIcHdr, ICMPLen );

    // Now allocate a NEW standard sized IP header
    // Recall that we reserved room for it when we created the packet
    Offset -= IPHDR_SIZE;
    pIpHdr  = (IPHDR *)(pb + Offset);

    // Set some IP header stuff
    pIpHdr->VerLen   = 0x45;      // Required when creating own header
    pIpHdr->Ttl      = ICMP_TTL;  // Use default ICMP Ttl
    pIpHdr->Tos      = 0;         // Rcmd'd for ICMP errors
    pIpHdr->Protocol = 1;         // ICMP

    WrNet32( &pIpHdr->dwIPDst, dwIPSrc );

    // Find a good IP source address
    if( hIFRx || !(dwIPSrc = BindIFNet2IPHost( 0, dwIPSrc )) )
        dwIPSrc = BindIF2IPHost( hIFRx );
    WrNet32( &pIpHdr->dwIPSrc, dwIPSrc );

    // Set the fragment valid data size and offset
    FragSetBufParams( hFrag, ICMPLen+IPHDR_SIZE, Offset );
    PktSetFlagsSet( hPkt, FLG_PKT_IP_VALID );

    // Send the packet
    IPTxPacket( hPkt, 0 );
}

//--------------------------------------------------------------------
// ICMPSendRtAdv( HANDLE hIFTx, uint Life, IPN dwIPAddr, UINT32 dwPref )
//
// Generates 8 byte ICMP Message Packet
//--------------------------------------------------------------------
void ICMPSendRtAdv( HANDLE hIFTx, uint Life, IPN dwIPAddr, long Pref )
{
    HANDLE     hPkt,hFrag;
    uint       Offset,ICMPLen;
    UINT8      *pb;
    ICMPRTAHDR *pRtaHdr;
    ICMPHDR    *pIcHdr;
    IPHDR      *pIpHdr;

    //
    // Create the packet
    // Payload = ICMPHDR + ICMPRTAHDR
    // Reserve = Reserve size for a STANDARD IP header.
    //
    ICMPLen = ICMPHDR_SIZE + ICMPRTAHDR_SIZE;
    if( !(hPkt = IFCreatePacket( ICMPLen, IPHDR_SIZE, 0 )) )
    {
        _dwICMPOutErrors++;
        return;
    }

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

    // Get a pointer to the new ICMP header
    pb        = FragGetBufParams( hFrag, 0, 0, 0 );
    Offset    = PktGetSizeLLC( hPkt ) + IPHDR_SIZE;
    pIcHdr    = (ICMPHDR *)(pb + Offset);

    // Bump the stats
    _dwICMPOut[ ICMP_ROUTERADVERT ]++;

    // Set the type and code
    pIcHdr->Type = ICMP_ROUTERADVERT;
    pIcHdr->Code = 0;

    // Get a pointer to the RTA header
    pRtaHdr= (ICMPRTAHDR *)pIcHdr->Data;

    // Validate Header
    pRtaHdr->NumAddr   =  1;
    pRtaHdr->Size      =  2;
    pRtaHdr->Lifetime     =  HNC16(Life);
    WrNet32( &pRtaHdr->rta[0].dwIPAddr, dwIPAddr );
    WrNet32( &pRtaHdr->rta[0].Pref, HNC32(Pref) );

    // Checksum the ICMP header
    ICMPChecksum( pIcHdr, ICMPLen );

    // Now allocate a NEW standard sized IP header
    // Recall that we reserved room for it when we created the packet
    Offset -= IPHDR_SIZE;
    pIpHdr  = (IPHDR *)(pb + Offset);

    // Set some IP header stuff
    pIpHdr->VerLen   = 0x45;      // Required when creating own header
    pIpHdr->Ttl      = ICMP_TTL;  // Use default ICMP Ttl
    pIpHdr->Tos      = 0;         // Rcmd'd for ICMP errors
    pIpHdr->Protocol = 1;         // ICMP

    WrNet32( &pIpHdr->dwIPSrc, dwIPAddr );
    WrNet32( &pIpHdr->dwIPDst, INADDR_BROADCAST );

    // Set the fragment valid data size and offset
    FragSetBufParams( hFrag, ICMPLen+IPHDR_SIZE, Offset );
    PktSetFlagsSet( hPkt, FLG_PKT_IP_VALID );

    // Since we know the egress IF, may as well tell IP
    PktSetIFTx( hPkt, hIFTx );

    // Send the packet
    IPTxPacket( hPkt, FLG_IPTX_BROADCAST );
}