udp.c

windows mobile 5 下的底层驱动包

    pIPHeader->wCRC = 0;                // CRC is 0 until computed
    pIPHeader->dwSrcIP = pSrcAddr->dwIP;
    pIPHeader->dwDestIP = pDestAddr->dwIP;

    // Compute the CRC and we're done with the IP header

    pIPHeader->wCRC = CRC((UINT16 *)pIPHeader, sizeof(IPHeaderFormat), NULL, 0);

    // The field information comes from page 145 of 
    // TCP/IP Illustrated by W. Richard Stevens.
    // Construct the UDP pseudo header

    UDPPseudoHeader.dwSrcIP = pSrcAddr->dwIP;
    UDPPseudoHeader.dwDestIP = pDestAddr->dwIP;
    UDPPseudoHeader.bZero = 0;
    UDPPseudoHeader.bProtocol = 17;     // UDP protocol is 17
    UDPPseudoHeader.cwTotalUDPLength = htons(sizeof(UDPHeaderFormat) + 
                                           cwLength);
    // Construct the real UDP header

    pUDPHeader = (UDPHeaderFormat *)((BYTE *)pIPHeader+sizeof(IPHeaderFormat));
    pUDPHeader->wSrcPort  = pSrcAddr->wPort;
    pUDPHeader->wDestPort = pDestAddr->wPort;

    // The total length of the UDP datagram

    pUDPHeader->cwTotalUDPLength = UDPPseudoHeader.cwTotalUDPLength;
    pUDPHeader->wCRC = 0;               // CRC is 0 until computed

    // Copy user data if necessary

    if( (pUDPData - pFrameBuffer) != UDP_DATA_FRAME_OFFSET )
    {
        memcpy( (BYTE *)pUDPHeader + sizeof(UDPHeaderFormat), 
            pUDPData, 
            cwLength);
    }

    // Compute the CRC for the UDP datagram.  The pad 0 will 
    // automatically be added if there are an odd number of bytes.

    pUDPHeader->wCRC = CRC( (UINT16 *)(&UDPPseudoHeader), 
                            sizeof(UDPPseudoHeader), 
                            (UINT16 *)pUDPHeader, 
                            ntohs(pUDPHeader->cwTotalUDPLength) );

    if( pUDPHeader->wCRC == 0 )
    {
        pUDPHeader->wCRC = ~(pUDPHeader->wCRC);
    }

    // Indicate status of frame transmit

    return( OEMEthSendFrame( pFrameBuffer,
            sizeof( EthernetFrameHeader ) + 
            ntohs( pIPHeader->cwTotalLength ) ) );
}

//------------------------------------------------------------------------------
//
//  Function Name:  SetPromiscuousIP( void )
//  Description..:  This functions sets the global PromiscuousIP flag. The flag
//                  is used to indicate when the CheckUDP routine should allow 
//                  packets through the filtering system (set).
//  Inputs.......:  none
//  Outputs......:  none
//
//------------------------------------------------------------------------------

void SetPromiscuousIP( void ) 
{
    fPromiscuousIP = 1;
}

//------------------------------------------------------------------------------
//
//  Function Name:  ClearPromiscuousIP( void )
//  Description..:  This functions clears the global PromiscuousIP flag. The flag
//                  is used to indicate when the CheckUDP routine should only 
//                  allow packets that match our address through.
//  Inputs.......:  none
//  Outputs......:  none
//
//------------------------------------------------------------------------------

void ClearPromiscuousIP( void ) 
{
    fPromiscuousIP = 0;
}

//------------------------------------------------------------------------------
//
//  Function Name:  EbootCheckUD(...)
//  Description..:  This routine will check a UDP frame that has been received.  
//                  It will make sure that it was for our IP address and that 
//                  the checksums are right and that it's a UDP packet.  If 
//                  something is wrong, the packet will be discarded and the 
//                  routine will return non-zero.  If everything is right, 
//                  then all the port and IP information will be filled out and 
//                  the routine will return 0. Note that if we are doing the 
//                  DHCP process, the DHCP server will send the OFFER packet to 
//                  the IP address that it would like to give us.  We have to 
//                  be able to accept a packet for any IP address in that 
//                  case.  This condition is signaled to the routine by 
//                  fPromiscuousIP == 1, which is set using SetPromiscuousIP()
//
//  Inputs.......:  EDBG_ADDR *         pntr to address
//                  BYTE *              pntr to frame
//                  UINT16 *            pntr to dst addr
//                  UINT16 *            pntr to src addr
//                  UINT16 **           pntr to pntr to data
//                  UINT16 *            pntr to data length
//  Outputs......:  0 on success, non zero on failure
//
//------------------------------------------------------------------------------

UINT16 EbootCheckUDP( EDBG_ADDR *pMyAddr, 
                      BYTE      *pFrameBuffer, 
                      UINT16    *wDestPort, 
                      UINT16    *wSrcPort, 
                      UINT16   **pwData, 
                      UINT16    *cwLength ) 
{
    IPHeaderFormat          *pIPHeader;
    UDPPseudoHeaderFormat    UDPPseudoHeader;
    UDPHeaderFormat         *pUDPHeader;

    // Note that I don't do any checking that depends on a length field 
    // in the packet until after the CRC is verified for that data.  
    // This prevents the code from running past the end of buffers, etc. 
    // when a bad packet is received.

    pIPHeader = (IPHeaderFormat *)(pFrameBuffer + sizeof(EthernetFrameHeader));

    // Make sure that it was for our IP address, unless we're 
    // doing DHCP (indicated by fPromiscuousIP == 1)

    if( !fPromiscuousIP && pIPHeader->dwDestIP != pMyAddr->dwIP )
    {
//      EdbgOutputDebugString( 
//      "!CheckUDP: Not our IP (0x%X)\n",pIPHeader->dwDestIP);

        return( 1 );
    }

    // Make sure that it is a UDP packet

    if( pIPHeader->bProtocol != 17 )
    {
        EdbgOutputDebugString( "!CheckUDP: Not UDP (proto = 0x%X)\n",
            pIPHeader->bProtocol );

        return( 2 );
    }

    // Check the IP header checksum

    if( CRC( (UINT16 *)pIPHeader, sizeof(IPHeaderFormat), NULL, 0 ) != 0 )
    {
        EdbgOutputDebugString( "!CheckUDP: IP header checksum failure\n" );
        return( 3 );
    }

    // Build the UDP Pseudo Header

    UDPPseudoHeader.dwSrcIP   = pIPHeader->dwSrcIP;
    UDPPseudoHeader.dwDestIP  = pIPHeader->dwDestIP;
    UDPPseudoHeader.bZero     = 0;
    UDPPseudoHeader.bProtocol = 17;         // UDP Proto is 17
    UDPPseudoHeader.cwTotalUDPLength = 
        htons( htons(pIPHeader->cwTotalLength) - sizeof( IPHeaderFormat ));

    // Check the UDP checksum, I'm using the cwTotalUDPLength 
    // calculated from the IP header info because we know that 
    // it's not corrupted and won't give an outrageous length for the packet

    pUDPHeader = (UDPHeaderFormat *)((BYTE *)pIPHeader+sizeof(IPHeaderFormat));

    if( pUDPHeader->wCRC != 0 && 
        CRC( (UINT16 *)&UDPPseudoHeader, 
        sizeof(UDPPseudoHeader), 
        (UINT16 *)pUDPHeader, 
        ntohs(UDPPseudoHeader.cwTotalUDPLength) ) != 0 )
    {
        EdbgOutputDebugString( "!CheckUDP: UDP header checksum failure\n" );
        return( 4 );
    }

    // Now we know we have a good packet, fill out the fields

    *wDestPort = pUDPHeader->wDestPort;
    *wSrcPort = pUDPHeader->wSrcPort;
    *pwData = (UINT16 *)((BYTE *)pUDPHeader + sizeof(UDPHeaderFormat));
    *cwLength = htons(pUDPHeader->cwTotalUDPLength) - sizeof(UDPHeaderFormat);

    // Indicate success

    return( 0 );
}

//------------------------------------------------------------------------------
//
//  Function Name:  DuplicateIPWarning( DWORD dwIP, BYTE *pchMAC )
//  Description..:  This function prints a warning when ProcessARP has 
//                  detected an IP address collision.
//  Inputs.......:  DWORD       IP          ip address
//                  BYTE *      MAC         pntr to MAC address
//  Outputs......:  none
//
//------------------------------------------------------------------------------

static void DuplicateIPWarning( DWORD dwIP, BYTE *pchMAC )
{
    EdbgOutputDebugString(
        "Duplicate IP Address Detected:\n"
        "-IP address %s in use by device with MAC address %B:%B:%B:%B:%B:%B.\n"
        "-Requesting new IP address via DHCP...\n", 
        inet_ntoa( dwIP ), 
        pchMAC[0], pchMAC[1], pchMAC[2], pchMAC[3], pchMAC[4], pchMAC[5] );
}

//------------------------------------------------------------------------------
//
//  Function Name:  CRC( ... )
//  Description..:  This routine will calculate the complemented 16-bit 
//                  1's complement sum used to compute network CRCs.  
//                  The CRC can be calculated over two different memory 
//                  regions.  If only one region is desired, then the other's 
//                  length can be set to 0.  Also, if an odd number of bytes 
//                  are specified, the routine will convert the last byte to 
//                  a word (w/o sign extension) and include that in the 
//                  calculation.
//  Inputs.......:  UINT16 *        pntr to region 1
//                  UINT16          length of region 1
//                  UNIT16 *        pntr to region 2
//                  UINT16          length of region 2
//  Outputs......:  the calculated CRC
//
//------------------------------------------------------------------------------

static UINT16 CRC( UINT16 *pwRegion1, 
                   UINT16  wLength1, 
                   UINT16 *pwRegion2, 
                   UINT16  wLength2 ) 
{
    DWORD dwSum;
    DWORD dwCarryOut;
    UINT16 wCRC;

    // There is no need to swap for network ordering during calculation 
    // because of the end around carry used in 1's complement addition

    dwSum = 0;

    // Region 1

    while( wLength1 > sizeof(BYTE) )
    {
        dwSum += *pwRegion1++;
        wLength1 -= sizeof(UINT16);
    }

    if( wLength1 )
    {
        dwSum += ((UINT16)*(BYTE *)pwRegion1 & 0x00FF);
    }

    // Region 2

    while( wLength2 > sizeof(BYTE) )
    {
        dwSum += *pwRegion2++;
        wLength2 -= sizeof(UINT16);
    }

    if( wLength2 )
    {
        dwSum += ((UINT16)*(BYTE *)pwRegion2 & 0x00FF);
    }

    // Add back in all the carry out's from the lower 
    // 16 bits because this is 1's complement.

    while( dwSum & 0xFFFF0000UL )
    {
        dwCarryOut = dwSum >> 16;
        dwSum &= 0x0000FFFFUL;
        dwSum += dwCarryOut;
    }

    wCRC = (UINT16)dwSum;

    // There is no need to flip for network byte order 
    // because we did all the sums backwards already.

    wCRC = ~wCRC;

    return( wCRC );
}

//------------------------------------------------------------------------------
//
//  Function Name:  UpperDWFromMAC( BYTE *pAddr )
//  Description..:  This function extracts upper DWORD from MAC address.
//  Inputs.......:  BYTE *      pntr to address
//  Outputs......:  DWORD
//
//------------------------------------------------------------------------------

static DWORD UpperDWFromMAC( BYTE *pAddr ) 
{
    DWORD ret;

    // Given a hex ethernet address of 12 34 56 78 9a bc
    // the 4 byte return value should look like 0x00123456

    ret  = (DWORD)pAddr[0];
    ret  <<= 8;
    ret |= (DWORD)pAddr[1];
    ret  <<= 8;
    ret |= (DWORD)pAddr[2];
    return ret;
}