Modules/IPStack - Fix UDP checksums, logging cleanup

diff --git a/KernelLand/Modules/IPStack/ipv4.c b/KernelLand/Modules/IPStack/ipv4.c
index a765400..71a0533 100644 (file)
--- a/KernelLand/Modules/IPStack/ipv4.c
+++ b/KernelLand/Modules/IPStack/ipv4.c
@@ -10,7 +10,7 @@
 
 // === CONSTANTS ===
 #define DEFAULT_TTL    32
 
 // === CONSTANTS ===
 #define DEFAULT_TTL    32

-#define IPV4_TRACE     0       // set to 1 to enable packet tracing
+#define IPV4_TRACE     1       // set to 1 to enable packet tracing

 
 // === IMPORTS ===
 extern tInterface      *gIP_Interfaces;
 
 // === IMPORTS ===
 extern tInterface      *gIP_Interfaces;


@@ -191,14 +191,14 @@ void IPv4_int_GetPacket(tAdapter *Adapter, tMacAddr From, int Length, void *Buff
                hdr->Destination.B[0], hdr->Destination.B[1], hdr->Destination.B[2], hdr->Destination.B[3]
                );
        #endif
                hdr->Destination.B[0], hdr->Destination.B[1], hdr->Destination.B[2], hdr->Destination.B[3]
                );
        #endif

-
-       // TODO: Should ARP sniffing be used?
-       // - If we get a packet, cache the source MAC
-       ARP_UpdateCache4(hdr->Source, From);

        
        // Get Data and Data Length
        dataLength = ntohs(hdr->TotalLength) - sizeof(tIPv4Header);
        data = &hdr->Options[0];
        
        // Get Data and Data Length
        dataLength = ntohs(hdr->TotalLength) - sizeof(tIPv4Header);
        data = &hdr->Options[0];

+
+       // Populate ARP cache from sniffing.
+       // - Downside: Poisoning, polluting from routed packets
+       //ARP_UpdateCache4(hdr->Source, From);

        
        // Get Interface (allowing broadcasts)
        iface = IPv4_GetInterface(Adapter, hdr->Destination, 1);
        
        // Get Interface (allowing broadcasts)
        iface = IPv4_GetInterface(Adapter, hdr->Destination, 1);


@@ -274,6 +274,10 @@ void IPv4_int_GetPacket(tAdapter *Adapter, tMacAddr From, int Length, void *Buff
                
                return ;
        }
                
                return ;
        }

+
+       // Populate ARP cache from recieved packets
+       // - Should be safe
+       ARP_UpdateCache4(hdr->Source, From);

        
        // Send it on
        if( !gaIPv4_Callbacks[hdr->Protocol] ) {
        
        // Send it on
        if( !gaIPv4_Callbacks[hdr->Protocol] ) {

diff --git a/KernelLand/Modules/IPStack/link.c b/KernelLand/Modules/IPStack/link.c
index 06f774a..ac31ce2 100644 (file)
--- a/KernelLand/Modules/IPStack/link.c
+++ b/KernelLand/Modules/IPStack/link.c
@@ -12,7 +12,7 @@
 #include "include/adapters_int.h"
 
 // === CONSTANTS ===
 #include "include/adapters_int.h"
 
 // === CONSTANTS ===

-#define LINK_LOGPACKETS        0
+#define LINK_LOGPACKETS        1

 #define VALIDATE_CHECKSUM      0
 #define        MAX_PACKET_SIZE 2048
 
 #define VALIDATE_CHECKSUM      0
 #define        MAX_PACKET_SIZE 2048
 


@@ -143,8 +143,9 @@ int Link_HandlePacket(tAdapter *Adapter, tIPStackBuffer *Buffer)
        
        #if LINK_LOGPACKETS
        Log_Log("Net Link",
        
        #if LINK_LOGPACKETS
        Log_Log("Net Link",

-               "Packet from %02x:%02x:%02x:%02x:%02x:%02x"
+               "eth%i Packet from %02x:%02x:%02x:%02x:%02x:%02x"

                " to %02x:%02x:%02x:%02x:%02x:%02x (Type=%04x)",
                " to %02x:%02x:%02x:%02x:%02x:%02x (Type=%04x)",

+               Adapter->Index,

                hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
                hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5],
                hdr->Dest.B[0], hdr->Dest.B[1], hdr->Dest.B[2],
                hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
                hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5],
                hdr->Dest.B[0], hdr->Dest.B[1], hdr->Dest.B[2],


@@ -178,7 +179,7 @@ int Link_HandlePacket(tAdapter *Adapter, tIPStackBuffer *Buffer)
                }
        }
        // No? Ignore it
                }
        }
        // No? Ignore it

-       Log_Log("Net Link", "Unregistered type 0x%04x", type);
+       Log_Log("Net Link", "eth%i Unregistered type 0x%04x", Adapter->Index, type);

        
        free(data);     
        return 1;
        
        free(data);     
        return 1;

diff --git a/KernelLand/Modules/IPStack/udp.c b/KernelLand/Modules/IPStack/udp.c
index ddc79dd..8c5c6ca 100644 (file)
--- a/KernelLand/Modules/IPStack/udp.c
+++ b/KernelLand/Modules/IPStack/udp.c
@@ -27,6 +27,9 @@ void  UDP_Channel_Close(tVFS_Node *Node);
 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel);
  int   UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port);
 void   UDP_int_FreePort(Uint16 Port);
 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel);
  int   UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port);
 void   UDP_int_FreePort(Uint16 Port);

+Uint16 UDP_int_MakeChecksum(tInterface *Iface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data); 
+Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data);
+Uint16 UDP_int_FinaliseChecksum(Uint16 Value);

 
 // === GLOBALS ===
 tVFS_NodeType  gUDP_NodeType = {
 
 // === GLOBALS ===
 tVFS_NodeType  gUDP_NodeType = {


@@ -119,8 +122,16 @@ void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 {
        tUDPHeader      *hdr = Buffer;
        
 {
        tUDPHeader      *hdr = Buffer;
        

-       Log_Debug("UDP", "%i bytes :%i->:%i (Cksum 0x%04x)",
-               ntohs(hdr->Length), ntohs(hdr->SourcePort), ntohs(hdr->Length), ntohs(hdr->Checksum));
+       #if 1
+       size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
+       char    tmp[len+1];
+       strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
+       Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
+               ntohs(hdr->Length),
+               tmp, ntohs(hdr->SourcePort),
+               IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
+               ntohs(hdr->Checksum));
+       #endif

        
        // Check registered connections
        Mutex_Acquire(&glUDP_Channels);
        
        // Check registered connections
        Mutex_Acquire(&glUDP_Channels);


@@ -148,21 +159,27 @@ void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, U
 
        if(Channel->Interface && Channel->Interface->Type != AddrType)  return ;
        
 
        if(Channel->Interface && Channel->Interface->Type != AddrType)  return ;
        

+       // Create the packet
+       hdr.SourcePort = htons( Channel->LocalPort );
+       hdr.DestPort = htons( Port );
+       hdr.Length = htons( sizeof(tUDPHeader) + Length );
+       hdr.Checksum = 0;
+       hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
+       
+       tIPStackBuffer  *buffer;

        switch(AddrType)
        {
        case 4:
        switch(AddrType)
        {
        case 4:

-               // Create the packet
-               hdr.SourcePort = htons( Channel->LocalPort );
-               hdr.DestPort = htons( Port );
-               hdr.Length = htons( sizeof(tUDPHeader) + Length );
-               hdr.Checksum = 0;       // Checksum can be zero on IPv4

                // Pass on the the IPv4 Layer
                // Pass on the the IPv4 Layer

-               tIPStackBuffer  *buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
+               buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);

                IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
                IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
                // TODO: What if Channel->Interface is NULL here?
                IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
                break;
                IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
                IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
                // TODO: What if Channel->Interface is NULL here?
                IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
                break;

+       default:
+               Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
+               break;

        }
 }
 
        }
 }
 


@@ -463,3 +480,59 @@ void UDP_int_FreePort(Uint16 Port)
        gUDP_Ports[Port/32] &= ~(1 << (Port%32));
        Mutex_Release(&glUDP_Ports);
 }
        gUDP_Ports[Port/32] &= ~(1 << (Port%32));
        Mutex_Release(&glUDP_Ports);
 }

+
+/**
+ *
+ */
+Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
+{
+       size_t  addrsize = IPStack_GetAddressSize(Interface->Type);
+       struct {
+               Uint8   Zeroes;
+               Uint8   Protocol;
+               Uint16  UDPLength;
+       } pheader;
+       
+       pheader.Zeroes = 0;
+       switch(Interface->Type)
+       {
+       case 4: pheader.Protocol = IP4PROT_UDP; break;
+       //case 6:       pheader.Protocol = IP6PROT_UDP; break;
+       default:
+               Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
+               return 0;
+       }
+       pheader.UDPLength = Hdr->Length;
+       
+       Uint16  csum = 0;
+       csum = UDP_int_PartialChecksum(csum, addrsize, Interface->Address);
+       csum = UDP_int_PartialChecksum(csum, addrsize, Dest);
+       csum = UDP_int_PartialChecksum(csum, sizeof(pheader), &pheader);
+       csum = UDP_int_PartialChecksum(csum, sizeof(tUDPHeader), Hdr);
+       csum = UDP_int_PartialChecksum(csum, Len, Data);
+       
+       return UDP_int_FinaliseChecksum(csum);
+}
+
+static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
+{
+       // One's complement arithmatic, overflows increment bottom bit
+       return a + b + (b > 0xFFFF - a ? 1 : 0);
+}
+
+Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
+{
+       Uint16  ret = Prev;
+       const Uint16    *data = Data;
+       for( int i = 0; i < Len/2; i ++ )
+               ret = _add_ones_complement16(ret, htons(*data++));
+       if( Len % 2 == 1 )
+               ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
+       return ret;
+}
+
+Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
+{
+       Value = ~Value; // One's complement it
+       return (Value == 0 ? 0xFFFF : Value);
+}