File content as of revision 7:8e12f7357b9f:

/*
 * $Id: ICMPv4.c 29 2011-06-11 14:53:08Z benoit $
 * $Author: benoit $
 * $Date: 2011-06-11 16:53:08 +0200 (sam., 11 juin 2011) $
 * $Rev: 29 $
 * 
 * 
 * 
 * 
 * 
 */
 
#include "ICMPv4.h"
#include "Ethernet.h"
#include "Debug.h"


#define    DEBUG_CURRENT_MODULE_NAME    "ICMPv4"
#define    DEBUG_CURRENT_MODULE_ID        DEBUG_MODULE_ICMPV4


static void Init(void);
static void Handler(NetIF_t *netIF, NetPacket_t *packet);


Protocol_Handler_t    icmpv4 = 
{ 
    PROTOCOL_INDEX_NOT_INITIALIZED,     /* Always PROTOCOL_INDEX_NOT_INITIALIZED at initialization */
    Protocol_ID_ICMPv4,                 /* Protocol ID */
    IPV4_PROTO_ICMPV4,                     /* Protocol number */
    Init,                                 /* Protocol initialisation function */
    Handler,                             /* Protocol handler */
    NULL,                                /* Protocol registration function */
    NULL,                                /* API registration function */
};


static void Init(void)
{
    DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing ICMPv4 layer"));
}


static void Handler(NetIF_t *netIF, NetPacket_t *packet)
{
    ICMPv4_Header_t      *icmpv4Packet;
	ICMPv4_Type_t        type;
    ICMPv4_Code_t        code;
    IPv4_Header_t        *ipv4Header;
    Ethernet_Header_t    *ethernetHeader;
    int32_t               depth, lengthToSend;
    
    //Debug_DumpBufferHex(packet, length);
    
	icmpv4Packet = (ICMPv4_Header_t *)packet->data;
    type = icmpv4Packet->type;
    code = icmpv4Packet->code;

    DEBUG_MODULE(DEBUG_LEVEL_VERBOSE1, ("icmpv4 frame of %d bytes icmpv4(%02x, %02x) frame of %d bytes",
        packet->length,
        type,
        code,
        packet->length
    ));
    depth = packet->depth;
    ipv4Header = (IPv4_Header_t *)packet->headerPtrTable[depth];
    ethernetHeader = (Ethernet_Header_t *)packet->headerPtrTable[depth - 1];
    
    switch(type)
    {
        case ICMPV4_TYPE_ECHO_REQUEST:
            DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0)
            {
                ICMPv4_DumpHeader("Got ",  ipv4Header);
            }
            
            ipv4Header->dest.addr = ipv4Header->source.addr;
            ipv4Header->source.addr = netIF->ipv4Address.addr;
            ethernetHeader->destination = ethernetHeader->source;
            ethernetHeader->source = *((Ethernet_Addr_t *)netIF->driverParameter);
            icmpv4Packet->type = ICMPV4_TYPE_ECHO_REPLY;
            lengthToSend = packet->headerLenTable[depth - 1] + packet->headerLenTable[depth - 1] + ntohs(ipv4Header->totalLength);
            icmpv4Packet->crc = 0;
            icmpv4Packet->crc = ICMPv4_ComputeCRC(icmpv4Packet, ntohs(ipv4Header->totalLength) - packet->headerLenTable[depth]);
            
            DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0)
            {
                ICMPv4_DumpHeader("Replying ", ipv4Header);
            }
            
            netIF->driver->Write((uint8_t *)ethernetHeader, lengthToSend );
            
            break;
            
        default:
            break;
    }
}


uint16_t ICMPv4_ComputeCRC(ICMPv4_Header_t *packet, int32_t length)
{
    uint32_t    crc = 0, size = length;
    uint16_t    *data = (uint16_t *)packet, tmp;
    
    while(size > 1)
    {
        tmp = ntohs(*data);
        crc += tmp;
        data++;
        size -= sizeof(uint16_t);
    }
    
    if (size > 0)
    {
        tmp = (*((uint8_t *)data)) << 8;
        crc += tmp;
    }
    
    crc = (crc >> 16) + (crc & 0xFFFF);
    if (crc & 0xFFFF0000) crc = (crc >> 16) + (crc & 0xFFFF);
        
    return htons((~crc) & 0xFFFF);
}


Bool_t ICMPv4_CheckCRC(ICMPv4_Header_t *packet, int32_t length)
{
    return True;
}


void ICMPv4_DumpHeader(const char *prefix, IPv4_Header_t *ipv4Header)
{
    ICMPv4_Header_t    *icmpv4Header = (ICMPv4_Header_t *)(ipv4Header + 1);

    switch(icmpv4Header->type)
    {
        case ICMPV4_TYPE_ECHO_REQUEST:
            DEBUG_RAW((
                "%sICMPv4 echo request from %d.%d.%d.%d",
                prefix != NULL ? prefix : "",
                ipv4Header->source.IP0,
                ipv4Header->source.IP1,
                ipv4Header->source.IP2,
                ipv4Header->source.IP3
            ));
            break;
            
        case ICMPV4_TYPE_ECHO_REPLY:
            DEBUG_RAW((
                "%sICMPv4 echo reply to %d.%d.%d.%d",
                prefix != NULL ? prefix : "",
                ipv4Header->dest.IP0,
                ipv4Header->dest.IP1,
                ipv4Header->dest.IP2,
                ipv4Header->dest.IP3
            ));
            break;

    }
}