Diff: IPv4.c

Revision:

0:19f5f51584de

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IPv4.c	Sun Jun 12 11:23:03 2011 +0000
@@ -0,0 +1,179 @@
+/*
+ * $Id: IPv4.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 "IPv4.h"
+#include "Ethernet.h"
+#include "Debug.h"
+#include <string.h>
+
+
+#define    DEBUG_CURRENT_MODULE_NAME    "IPv4"
+#define    DEBUG_CURRENT_MODULE_ID        DEBUG_MODULE_IPV4
+
+
+static void        Init(void);
+static int32_t    RegisterProtocol(Protocol_Handler_t *protocolHandler);
+static void        Handler(NetIF_t *netIF, Packet_t *packet);
+
+
+static Protocol_Handler_t    *protocolHandlerTable[IPV4_PROTOCOL_MAX_COUNT];
+static int32_t                protocolHandlerCount = 0;
+
+
+const IPv4_Addr_t         ipv4_Addr_Broadcast        = { 255, 255, 255, 255 };
+const IPv4_Addr_t         ipv4_Addr_Any            = { 0, 0, 0, 0};
+
+
+Protocol_Handler_t             ipv4 = 
+{ 
+    PROTOCOL_INDEX_NOT_INITIALIZED,        /* Always PROTOCOL_INDEX_NOT_INITIALIZED at initialization */
+    Protocol_ID_IPv4,                     /* Protocol ID */
+    htons(ETHERNET_PROTO_IPV4),         /* Protocol number */
+    Init,                                 /* Protocol initialisation function */
+    Handler,                             /* Protocol handler */
+    RegisterProtocol,                     /* Protocol registration function */
+    NULL,                                /* API registration function */
+};
+
+
+static void Init(void)
+{
+    DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing IPv4 layer"));
+    protocolHandlerCount = 0;
+    memset(protocolHandlerTable, 0, sizeof(protocolHandlerTable));
+}
+
+
+static int32_t RegisterProtocol(Protocol_Handler_t *protocolHandler)
+{
+    int32_t        result = 0;
+
+    if (protocolHandlerCount >= IPV4_PROTOCOL_MAX_COUNT)
+    {
+        DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Too many protocols"));
+        result = -1;
+        mbedNet_LastError = mbedNetResult_TooManyRegisteredProtocols;
+        goto Exit;
+    }
+    
+    protocolHandlerTable[protocolHandlerCount] = protocolHandler;
+    protocolHandler->index = protocolHandlerCount;
+    protocolHandler->Init();
+    
+    DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Registered protocol %2d ipv4/%s",
+        protocolHandler->protocolNumber,
+        protocol_IDNames[protocolHandler->protocolID]
+    ));
+    
+    protocolHandlerCount++;
+    
+Exit:
+    return result;
+}
+
+
+static void Handler(NetIF_t *netIF, Packet_t *packet)
+{
+    int32_t                protocolIndex,
+                        payloadOffset;
+    Protocol_Number_t    protocolNumber;
+    Protocol_Handler_t    *protocolHandler;
+    IPv4_Header_t        *ipv4Packet = (IPv4_Header_t *)packet->data;
+
+    protocolNumber = ipv4Packet->protocol;
+    payloadOffset = ipv4Packet->ihl << 2;
+    
+    if ((ipv4Packet->dest.addr != netIF->ipv4Address.addr) && (ipv4Packet->dest.addr != netIF->ipv4Broadcast.addr))
+    {
+        #if DEBUG_IPV4
+        IPV4_DumpIPv4Header("Not for us ", ipv4Packet);
+        #endif /* DEBUG_IPV4 */
+        goto Exit;
+    }
+    
+    #if DEBUG_IPV4
+    IPv4_DumpIPv4Header("processing ", ipv4Packet);
+    #endif /* DEBUG_IPV4 */
+    
+    //Debug_DumpBufferHex(packet, length);
+    for (protocolIndex = 0; protocolIndex < protocolHandlerCount; protocolIndex++)
+    {
+        protocolHandler = protocolHandlerTable[protocolIndex];
+        if (protocolHandler->protocolNumber == protocolNumber)
+        {
+            NetIF_ProtoPush(packet, payloadOffset, Protocol_ID_IPv4);
+            protocolHandler->HandlePacket(netIF, packet);
+            goto Exit;
+        }
+    }
+    
+    #if DEBUG_IPV4
+    IPV4_DumpIPv4Header("Not handled ", ipv4Packet);
+    #endif /* DEBUG_IPV4 */
+    
+    
+Exit:
+    return;
+}
+
+
+void IPv4_DumpIPv4Header(const char *prefix, IPv4_Header_t *ipv4Packet)
+{
+    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" ,
+        prefix != NULL ? prefix : "",
+        ipv4Packet->source.IP0,
+        ipv4Packet->source.IP1,
+        ipv4Packet->source.IP2,
+        ipv4Packet->source.IP3,
+        ipv4Packet->dest.IP0,
+        ipv4Packet->dest.IP1,
+        ipv4Packet->dest.IP2,
+        ipv4Packet->dest.IP3,
+        ipv4Packet->version,
+        ipv4Packet->ihl,
+        ipv4Packet->tos,
+        ntohs(ipv4Packet->totalLength),
+        ntohs(ipv4Packet->id),
+        (ntohs(ipv4Packet->fragmentFlags) & 0x1FFF) >> 13,
+        ntohs(ipv4Packet->fragmentFlags),
+        ipv4Packet->ttl,
+        ipv4Packet->protocol,
+        ntohs(ipv4Packet->crc)
+    ));
+}
+
+
+uint16_t    IPv4_ComputeCRC(IPv4_Header_t *ipv4Header)
+{
+    uint32_t    crc = 0, 
+                length;
+    uint16_t    *data;
+    
+    data = (uint16_t *)ipv4Header;
+    length = ipv4Header->ihl * 4;
+    
+    while(length > 1)
+    {
+        crc += *data;
+        data++;
+        length -= 2;
+    }
+    if (length)
+    {
+        crc += *(uint8_t *)(data);
+    }
+    
+    crc = (crc >> 16) + (crc & 0xFFFF);
+    crc = crc + (crc >> 16);
+    
+    return (uint16_t)(~crc);
+}