IPv4.cpp

00001 /*
00002  * $Id: IPv4.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 "IPv4.h"
00014 #include "Ethernet.h"
00015 #include "Debug.h"
00016 #include <string.h>
00017 
00018 
00019 #define    DEBUG_CURRENT_MODULE_NAME    "IPv4"
00020 #define    DEBUG_CURRENT_MODULE_ID        DEBUG_MODULE_IPV4
00021 
00022 
00023 static void     Init(void);
00024 static int32_t  RegisterProtocol(Protocol_Handler_t *protocolHandler);
00025 static void     Handler(NetIF_t *netIF, NetPacket_t *packet);
00026 
00027 
00028 static Protocol_Handler_t    *protocolHandlerTable[IPV4_PROTOCOL_MAX_COUNT];
00029 static int32_t                protocolHandlerCount = 0;
00030 
00031 
00032 const IPv4_Addr_t         ipv4_Addr_Broadcast        = { 255, 255, 255, 255 };
00033 const IPv4_Addr_t         ipv4_Addr_Any            = { 0, 0, 0, 0};
00034 
00035 
00036 Protocol_Handler_t             ipv4 = 
00037 { 
00038     PROTOCOL_INDEX_NOT_INITIALIZED,        /* Always PROTOCOL_INDEX_NOT_INITIALIZED at initialization */
00039     Protocol_ID_IPv4,                     /* Protocol ID */
00040     htons(ETHERNET_PROTO_IPV4),         /* Protocol number */
00041     Init,                                 /* Protocol initialisation function */
00042     Handler,                             /* Protocol handler */
00043     RegisterProtocol,                     /* Protocol registration function */
00044     NULL,                                /* API registration function */
00045 };
00046 
00047 
00048 static void Init(void)
00049 {
00050     DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing IPv4 layer"));
00051     protocolHandlerCount = 0;
00052     memset(protocolHandlerTable, 0, sizeof(protocolHandlerTable));
00053 }
00054 
00055 
00056 static int32_t RegisterProtocol(Protocol_Handler_t *protocolHandler)
00057 {
00058     int32_t        result = 0;
00059 
00060     if (protocolHandlerCount >= IPV4_PROTOCOL_MAX_COUNT)
00061     {
00062         DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Too many protocols"));
00063         result = -1;
00064         mbedNet_LastError = mbedNetResult_TooManyRegisteredProtocols;
00065         goto Exit;
00066     }
00067     
00068     protocolHandlerTable[protocolHandlerCount] = protocolHandler;
00069     protocolHandler->index = protocolHandlerCount;
00070     protocolHandler->Init();
00071     
00072     DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Registered protocol %2d ipv4/%s",
00073         protocolHandler->protocolNumber,
00074         protocol_IDNames[protocolHandler->protocolID]
00075     ));
00076     
00077     protocolHandlerCount++;
00078     
00079 Exit:
00080     return result;
00081 }
00082 
00083 
00084 static void Handler(NetIF_t *netIF, NetPacket_t *packet)
00085 {
00086     int32_t               protocolIndex,
00087                         payloadOffset;
00088     Protocol_Number_t    protocolNumber;
00089     Protocol_Handler_t    *protocolHandler;
00090     IPv4_Header_t        *ipv4Packet;
00091     
00092     
00093     ipv4Packet = (IPv4_Header_t *)packet->data;
00094     protocolNumber = ipv4Packet->protocol;
00095     payloadOffset = ipv4Packet->ihl << 2;
00096     
00097     
00098     if (    (ipv4Packet->dest.addr == netIF->ipv4Address.addr)      || 
00099             (ipv4Packet->dest.addr == netIF->ipv4Broadcast.addr)    ||
00100             (ipv4Packet->dest.addr == ipv4_Addr_Broadcast.addr)         )
00101     {
00102         for (protocolIndex = 0; protocolIndex < protocolHandlerCount; protocolIndex++)
00103         {
00104             protocolHandler = protocolHandlerTable[protocolIndex];
00105             if (protocolHandler->protocolNumber == protocolNumber)
00106             {
00107                 NetIF_ProtoPush(packet, payloadOffset, Protocol_ID_IPv4);
00108                 protocolHandler->HandlePacket(netIF, packet);
00109                 break;
00110             }
00111         }
00112     }
00113         
00114     return;
00115 }
00116 
00117 
00118 void IPv4_DumpIPv4Header(const char *prefix, IPv4_Header_t *ipv4Packet)
00119 {
00120     DEBUG_RAW(("%sIPv4 %d.%d.%d.%d --> %d.%d.%d.%d  ver:%01X ihl:%01X tos:%03d totlen:%04d id:%04X flags:%1d frag:%05d ttl:%3d  proto:%03d crc:%04X" ,
00121         prefix != NULL ? prefix : "",
00122         ipv4Packet->source.IP0,
00123         ipv4Packet->source.IP1,
00124         ipv4Packet->source.IP2,
00125         ipv4Packet->source.IP3,
00126         ipv4Packet->dest.IP0,
00127         ipv4Packet->dest.IP1,
00128         ipv4Packet->dest.IP2,
00129         ipv4Packet->dest.IP3,
00130         ipv4Packet->version,
00131         ipv4Packet->ihl,
00132         ipv4Packet->tos,
00133         ntohs(ipv4Packet->totalLength),
00134         ntohs(ipv4Packet->id),
00135         (ntohs(ipv4Packet->fragmentFlags) & 0x1FFF) >> 13,
00136         ntohs(ipv4Packet->fragmentFlags),
00137         ipv4Packet->ttl,
00138         ipv4Packet->protocol,
00139         ntohs(ipv4Packet->crc)
00140     ));
00141 }
00142 
00143 
00144 uint16_t IPv4_ComputeCRC(IPv4_Header_t *ipv4Header)
00145 {
00146     uint32_t    crc = 0, 
00147                 length;
00148     uint16_t    *data;
00149     
00150     data = (uint16_t *)ipv4Header;
00151     length = ipv4Header->ihl * 4;
00152     
00153     while(length > 1)
00154     {
00155         crc += *data;
00156         data++;
00157         length -= 2;
00158     }
00159     if (length)
00160     {
00161         crc += *(uint8_t *)(data);
00162     }
00163     
00164     crc = (crc >> 16) + (crc & 0xFFFF);
00165     crc = crc + (crc >> 16);
00166     
00167     return (uint16_t)(~crc);
00168 }