ICMPv4.cpp

00001 /*
00002  * $Id: ICMPv4.c 29 2011-06-11 14:53:08Z benoit $
00003  * $Author: benoit $
00004  * $Date: 2011-06-11 16:53:08 +0200 (sam., 11 juin 2011) $
00005  * $Rev: 29 $
00006  * 
00007  * 
00008  * 
00009  * 
00010  * 
00011  */
00012  
00013 #include "ICMPv4.h"
00014 #include "Ethernet.h"
00015 #include "Debug.h"
00016 
00017 
00018 #define    DEBUG_CURRENT_MODULE_NAME    "ICMPv4"
00019 #define    DEBUG_CURRENT_MODULE_ID        DEBUG_MODULE_ICMPV4
00020 
00021 
00022 static void Init(void);
00023 static void Handler(NetIF_t *netIF, NetPacket_t *packet);
00024 
00025 
00026 Protocol_Handler_t    icmpv4 = 
00027 { 
00028     PROTOCOL_INDEX_NOT_INITIALIZED,     /* Always PROTOCOL_INDEX_NOT_INITIALIZED at initialization */
00029     Protocol_ID_ICMPv4,                 /* Protocol ID */
00030     IPV4_PROTO_ICMPV4,                     /* Protocol number */
00031     Init,                                 /* Protocol initialisation function */
00032     Handler,                             /* Protocol handler */
00033     NULL,                                /* Protocol registration function */
00034     NULL,                                /* API registration function */
00035 };
00036 
00037 
00038 static void Init(void)
00039 {
00040     DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing ICMPv4 layer"));
00041 }
00042 
00043 
00044 static void Handler(NetIF_t *netIF, NetPacket_t *packet)
00045 {
00046     ICMPv4_Header_t      *icmpv4Packet;
00047     ICMPv4_Type_t        type;
00048     ICMPv4_Code_t        code;
00049     IPv4_Header_t        *ipv4Header;
00050     Ethernet_Header_t    *ethernetHeader;
00051     int32_t               depth, lengthToSend;
00052     
00053     //Debug_DumpBufferHex(packet, length);
00054     
00055     icmpv4Packet = (ICMPv4_Header_t *)packet->data;
00056     type = icmpv4Packet->type;
00057     code = icmpv4Packet->code;
00058 
00059     DEBUG_MODULE(DEBUG_LEVEL_VERBOSE1, ("icmpv4 frame of %d bytes icmpv4(%02x, %02x) frame of %d bytes",
00060         packet->length,
00061         type,
00062         code,
00063         packet->length
00064     ));
00065     depth = packet->depth;
00066     ipv4Header = (IPv4_Header_t *)packet->headerPtrTable[depth];
00067     ethernetHeader = (Ethernet_Header_t *)packet->headerPtrTable[depth - 1];
00068     
00069     switch(type)
00070     {
00071         case ICMPV4_TYPE_ECHO_REQUEST:
00072             DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0)
00073             {
00074                 ICMPv4_DumpHeader("Got ",  ipv4Header);
00075             }
00076             
00077             ipv4Header->dest.addr = ipv4Header->source.addr;
00078             ipv4Header->source.addr = netIF->ipv4Address.addr;
00079             ethernetHeader->destination = ethernetHeader->source;
00080             ethernetHeader->source = *((Ethernet_Addr_t *)netIF->driverParameter);
00081             icmpv4Packet->type = ICMPV4_TYPE_ECHO_REPLY;
00082             lengthToSend = packet->headerLenTable[depth - 1] + packet->headerLenTable[depth - 1] + ntohs(ipv4Header->totalLength);
00083             icmpv4Packet->crc = 0;
00084             icmpv4Packet->crc = ICMPv4_ComputeCRC(icmpv4Packet, ntohs(ipv4Header->totalLength) - packet->headerLenTable[depth]);
00085             
00086             DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0)
00087             {
00088                 ICMPv4_DumpHeader("Replying ", ipv4Header);
00089             }
00090             
00091             netIF->driver->Write((uint8_t *)ethernetHeader, lengthToSend );
00092             
00093             break;
00094             
00095         default:
00096             break;
00097     }
00098 }
00099 
00100 
00101 uint16_t ICMPv4_ComputeCRC(ICMPv4_Header_t *packet, int32_t length)
00102 {
00103     uint32_t    crc = 0, size = length;
00104     uint16_t    *data = (uint16_t *)packet, tmp;
00105     
00106     while(size > 1)
00107     {
00108         tmp = ntohs(*data);
00109         crc += tmp;
00110         data++;
00111         size -= sizeof(uint16_t);
00112     }
00113     
00114     if (size > 0)
00115     {
00116         tmp = (*((uint8_t *)data)) << 8;
00117         crc += tmp;
00118     }
00119     
00120     crc = (crc >> 16) + (crc & 0xFFFF);
00121     if (crc & 0xFFFF0000) crc = (crc >> 16) + (crc & 0xFFFF);
00122         
00123     return htons((~crc) & 0xFFFF);
00124 }
00125 
00126 
00127 Bool_t ICMPv4_CheckCRC(ICMPv4_Header_t *packet, int32_t length)
00128 {
00129     return True;
00130 }
00131 
00132 
00133 void ICMPv4_DumpHeader(const char *prefix, IPv4_Header_t *ipv4Header)
00134 {
00135     ICMPv4_Header_t    *icmpv4Header = (ICMPv4_Header_t *)(ipv4Header + 1);
00136 
00137     switch(icmpv4Header->type)
00138     {
00139         case ICMPV4_TYPE_ECHO_REQUEST:
00140             DEBUG_RAW((
00141                 "%sICMPv4 echo request from %d.%d.%d.%d",
00142                 prefix != NULL ? prefix : "",
00143                 ipv4Header->source.IP0,
00144                 ipv4Header->source.IP1,
00145                 ipv4Header->source.IP2,
00146                 ipv4Header->source.IP3
00147             ));
00148             break;
00149             
00150         case ICMPV4_TYPE_ECHO_REPLY:
00151             DEBUG_RAW((
00152                 "%sICMPv4 echo reply to %d.%d.%d.%d",
00153                 prefix != NULL ? prefix : "",
00154                 ipv4Header->dest.IP0,
00155                 ipv4Header->dest.IP1,
00156                 ipv4Header->dest.IP2,
00157                 ipv4Header->dest.IP3
00158             ));
00159             break;
00160 
00161     }
00162 }