More work on UDI, Added more Login VTerms

[tpg/acess2.git] / Modules / IPStack / ipv4.c

diff --git a/Modules/IPStack/ipv4.c b/Modules/IPStack/ipv4.c
index 9723d8d..7e7c585 100644 (file)
--- a/Modules/IPStack/ipv4.c
+++ b/Modules/IPStack/ipv4.c
@@ -8,9 +8,12 @@
 
 // === IMPORTS ===
 extern tInterface      *gIP_Interfaces;
+extern void    ICMP_Initialise();
+extern void    UDP_Initialise();
 
 // === PROTOTYPES ===
  int   IPv4_Initialise();
+ int   IPv4_RegisterCallback(int ID, tIPCallback Callback);
 void   IPv4_int_GetPacket(tAdapter *Interface, tMacAddr From, int Length, void *Buffer);
 tInterface     *IPv4_GetInterface(tAdapter *Adapter, tIPv4 Address, int Broadcast);
 Uint32 IPv4_Netmask(int FixedBits);
@@ -24,10 +27,24 @@ tIPCallback gaIPv4_Callbacks[256];
  */
 int IPv4_Initialise()
 {
+       ICMP_Initialise();
+       UDP_Initialise();
        Link_RegisterType(IPV4_ETHERNET_ID, IPv4_int_GetPacket);
        return 1;
 }
 
+/**
+ * \fn int IPv4_RegisterCallback( int ID, tIPCallback Callback )
+ * \brief Registers a callback
+ */
+int IPv4_RegisterCallback(int ID, tIPCallback Callback)
+{
+       if( ID < 0 || ID > 255 )        return 0;
+       if( gaIPv4_Callbacks[ID] )      return 0;
+       gaIPv4_Callbacks[ID] = Callback;
+       return 1;
+}
+
 /**
  * \fn void IPv4_int_GetPacket(tInterface *Adapter, tMacAddr From, int Length, void *Buffer)
  * \brief Process an IPv4 Packet
@@ -40,18 +57,41 @@ void IPv4_int_GetPacket(tAdapter *Adapter, tMacAddr From, int Length, void *Buff
         int    dataLength;
        if(Length < sizeof(tIPv4Header))        return;
        
+       //Log("[IPv4 ] Version = %i", hdr->Version);
+       Log("[IPv4 ] HeaderLength = %i", hdr->HeaderLength);
+       Log("[IPv4 ] DiffServices = %i", hdr->DiffServices);
+       //Log("[IPv4 ] TotalLength = %i", ntohs(hdr->TotalLength) );
+       //Log("[IPv4 ] Identifcation = %i", ntohs(hdr->Identifcation) );
+       //Log("[IPv4 ] TTL = %i", hdr->TTL );
+       Log("[IPv4 ] Protocol = %i", hdr->Protocol );
+       //Log("[IPv4 ] HeaderChecksum = 0x%x", ntohs(hdr->HeaderChecksum) );
+       Log("[IPv4 ] Source = %i.%i.%i.%i",
+               hdr->Source.B[0], hdr->Source.B[1], hdr->Source.B[2], hdr->Source.B[3] );
+       Log("[IPv4 ] Destination = %i.%i.%i.%i",
+               hdr->Destination.B[0], hdr->Destination.B[1],
+               hdr->Destination.B[2], hdr->Destination.B[3] );
+       
        // Check that the version IS IPv4
-       if(hdr->Version != 4)   return;
+       if(hdr->Version != 4) {
+               Log("[IPv4 ] hdr->Version(%i) != 4", hdr->Version);
+               return;
+       }
        
        // Check Header checksum
        //TODO
        
        // Check Packet length
-       if(hdr->TotalLength > Length)   return;
+       if( ntohs(hdr->TotalLength) > Length) {
+               Log("[IPv4 ] hdr->TotalLength(%i) > Length(%i)", hdr->TotalLength, Length);
+               return;
+       }
        
        // Get Interface (allowing broadcasts)
-       iface = IPv4_GetInterface(Adapter, hdr->Source, 1);
-       if(!iface)      return; // Not for us? Well, let's ignore it
+       iface = IPv4_GetInterface(Adapter, hdr->Destination, 1);
+       if(!iface) {
+               Log("[IPv4 ] Ignoring Packet (Not for us)");
+               return; // Not for us? Well, let's ignore it
+       }
        
        // Defragment
        //TODO
@@ -71,6 +111,9 @@ tInterface *IPv4_GetInterface(tAdapter *Adapter, tIPv4 Address, int Broadcast)
 {
        tInterface      *iface = NULL;
        Uint32  netmask;
+       Uint32  addr, this;
+       
+       addr = ntohl( Address.L );
        
        for( iface = gIP_Interfaces; iface; iface = iface->Next)
        {
@@ -81,10 +124,20 @@ tInterface *IPv4_GetInterface(tAdapter *Adapter, tIPv4 Address, int Broadcast)
                
                if( !Broadcast )        continue;
                
+               this = ntohl( iface->IP4.Address.L );
+               
                // Check for broadcast
                netmask = IPv4_Netmask(iface->IP4.SubnetBits);
-               if( (Address.L & netmask) == (iface->IP4.Address.L & netmask)
-                && (Address.L & ~netmask) == (0xFFFFFFFF & ~netmask) )
+               
+               //Log("netmask = 0x%08x", netmask);
+               //Log("addr = 0x%08x", addr);
+               //Log("this = 0x%08x", this);
+               //Log("%08x == %08x && %08x == %08x",
+               //      (addr & netmask), (this & netmask),
+               //      (addr & ~netmask), (0xFFFFFFFF & ~netmask)
+               //      );
+               if( (addr & netmask) == (this & netmask)
+                && (addr & ~netmask) == (0xFFFFFFFF & ~netmask) )
                        return iface;
        }
        return NULL;
@@ -100,5 +153,6 @@ Uint32 IPv4_Netmask(int FixedBits)
        Uint32  ret = 0xFFFFFFFF;
        ret >>= (32-FixedBits);
        ret <<= (32-FixedBits);
+       // Returs a little endian netmask
        return ret;
 }