More work on IPStack (Also fixed Proc_SpawnWorker)

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

diff --git a/Modules/IPStack/link.c b/Modules/IPStack/link.c
index 9dee6e4..cb59f0f 100644 (file)
--- a/Modules/IPStack/link.c
+++ b/Modules/IPStack/link.c
@@ -8,12 +8,21 @@
 // === CONSTANTS ===
 #define        MAX_PACKET_SIZE 2048
 
+// === PROTOTYPES ===
+void   Link_RegisterType(Uint16 Type, tPacketCallback Callback);
+void   Link_InitCRC();
+Uint32 Link_CalculateCRC(void *Data, int Length);
+void   Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, int Length, void *Buffer);
+void   Link_WatchDevice(tAdapter *Adapter);
+
 // === GLOBALS ===
  int   giRegisteredTypes = 0;
 struct {
        Uint16  Type;
        tPacketCallback Callback;
 }      *gaRegisteredTypes;
+ int   gbLink_CRCTableGenerated = 0;
+Uint32 gaiLink_CRCTable[256];
 
 // === CODE ===
 /**
@@ -26,6 +35,9 @@ void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
 {
         int    i;
        void    *tmp;
+       
+       Type = htons(Type);     // Set to network order
+       
        for( i = giRegisteredTypes; i -- ; )
        {
                if(gaRegisteredTypes[i].Type == Type) {
@@ -39,7 +51,10 @@ void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
        if(i == -1)
        {
                tmp = realloc(gaRegisteredTypes, (giRegisteredTypes+1)*sizeof(*gaRegisteredTypes));
-               if(!tmp)        Panic("[NET  ] Out of heap space!");
+               if(!tmp) {
+                       Warning("[NET  ] Out of heap space!");
+                       return ;
+               }
                i = giRegisteredTypes;
                giRegisteredTypes ++;
                gaRegisteredTypes = tmp;
@@ -55,16 +70,23 @@ void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
  */
 void Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, int Length, void *Buffer)
 {
-        int    bufSize = sizeof(tEthernetHeader) + Length;
-       Uint8   buf[bufSize];
+        int    bufSize = sizeof(tEthernetHeader) + Length + 4;
+       Uint8   buf[bufSize];   // dynamic stack arrays ftw!
        tEthernetHeader *hdr = (void*)buf;
        
+       Log("[NET  ] Sending %i bytes to %02x:%02x:%02x:%02x:%02x:%02x (Type 0x%x)",
+               Length, To.B[0], To.B[1], To.B[2], To.B[3], To.B[4], To.B[5], Type);
+       
        hdr->Dest = To;
        hdr->Src = Adapter->MacAddr;
        hdr->Type = htons(Type);
        
        memcpy(hdr->Data, Buffer, Length);
        
+       *(Uint32*) &hdr->Data[Length] = 0;
+       *(Uint32*) &hdr->Data[Length] = htonl( Link_CalculateCRC(buf, bufSize) );
+       
+       
        VFS_Write(Adapter->DeviceFD, bufSize, buf);
 }
 
@@ -77,7 +99,8 @@ void Link_WatchDevice(tAdapter *Adapter)
         int    tid = Proc_SpawnWorker();       // Create a new worker thread
        
        if(tid < 0) {
-               Panic("[NET  ] Unable to create watcher thread for '%s'", Adapter->Device);
+               Warning("[NET  ] Unable to create watcher thread for '%s'", Adapter->Device);
+               return ;
        }
        
        if(tid > 0) {
@@ -85,12 +108,16 @@ void Link_WatchDevice(tAdapter *Adapter)
                return ;
        }
        
+       if( !gbLink_CRCTableGenerated )
+               Link_InitCRC();
+       
        // Child Thread
        while(Adapter->DeviceFD != -1)
        {
                Uint8   buf[MAX_PACKET_SIZE];
                tEthernetHeader *hdr = (void*)buf;
                 int    ret, i;
+               Uint32  checksum;
                
                // Wait for a packet (Read on a network device is blocking)
                ret = VFS_Read(Adapter->DeviceFD, MAX_PACKET_SIZE, buf);
@@ -101,13 +128,27 @@ void Link_WatchDevice(tAdapter *Adapter)
                        continue;
                }
                
+               Log("[NET  ] Packet from %02x:%02x:%02x:%02x:%02x:%02x",
+                       hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
+                       hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5]
+                       );
+               Log("[NET  ] to %02x:%02x:%02x:%02x:%02x:%02x",
+                       hdr->Dest.B[0], hdr->Dest.B[1], hdr->Dest.B[2],
+                       hdr->Dest.B[3], hdr->Dest.B[4], hdr->Dest.B[5]
+                       );
+               checksum = *(Uint32*)&hdr->Data[ret-sizeof(tEthernetHeader)-4];
+               Log("[NET  ] Checksum 0x%08x", checksum);
+               
                // Check if there is a registered callback for this packet type
                for( i = giRegisteredTypes; i--; )
                {
-                       if(gaRegisteredTypes[i].Type == hdr->Type)      continue;
+                       if(gaRegisteredTypes[i].Type == hdr->Type)      break;
                }
                // No? Ignore it
-               if( i == -1 )   continue;
+               if( i == -1 ) {
+                       Log("[NET  ] Unregistered type 0x%x", ntohs(hdr->Type));
+                       continue;
+               }
                
                // Call the callback
                gaRegisteredTypes[i].Callback(
@@ -120,3 +161,50 @@ void Link_WatchDevice(tAdapter *Adapter)
        
        Log("[NET  ] Watcher terminated (file closed)");
 }
+
+// From http://www.cl.cam.ac.uk/research/srg/bluebook/21/crc/node6.html
+#define        QUOTIENT        0x04c11db7
+void Link_InitCRC(void)
+{
+     int       i, j;
+    Uint32     crc;
+
+    for (i = 0; i < 256; i++)
+    {
+        crc = i << 24;
+        for (j = 0; j < 8; j++)
+        {
+            if (crc & 0x80000000)
+                crc = (crc << 1) ^ QUOTIENT;
+            else
+                crc = crc << 1;
+        }
+        gaiLink_CRCTable[i] = crc;
+    }
+       
+       gbLink_CRCTableGenerated = 1;
+}
+
+Uint32 Link_CalculateCRC(void *Data, int Length)
+{
+       // x32 + x26 + x23 + x22 + x16 + x12 + x11 + x10 + x8 + x7 + x5 + x4 + x2 + x + 1
+       Uint32  result;
+     int       i;
+       Uint32  *data = Data;
+    
+    if(Length < 4)     return 0;
+
+    result = *data++ << 24;
+    result |= *data++ << 16;
+    result |= *data++ << 8;
+    result |= *data++;
+    result = ~ result;
+    Length -= 4;
+    
+    for( i = 0; i < Length; i++ )
+    {
+        result = (result << 8 | *data++) ^ gaiLink_CRCTable[result >> 24];
+    }
+    
+    return ~result;
+}