File content as of revision 7:8e12f7357b9f:

/*
 * $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, NetPacket_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, NetPacket_t *packet)
{
    int32_t               protocolIndex,
                        payloadOffset;
    Protocol_Number_t    protocolNumber;
    Protocol_Handler_t    *protocolHandler;
    IPv4_Header_t        *ipv4Packet;
    
    
    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)	||
			(ipv4Packet->dest.addr == ipv4_Addr_Broadcast.addr)			)
    {
        for (protocolIndex = 0; protocolIndex < protocolHandlerCount; protocolIndex++)
        {
            protocolHandler = protocolHandlerTable[protocolIndex];
            if (protocolHandler->protocolNumber == protocolNumber)
            {
                NetIF_ProtoPush(packet, payloadOffset, Protocol_ID_IPv4);
                protocolHandler->HandlePacket(netIF, packet);
                break;
            }
        }
    }
        
    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);
}