X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2FIPStack%2Flink.c;h=cb59f0f1782f3e41bf95792336fa9d12bdcc47b6;hb=0f48b41ce8edd3b6d549d641b35901e4b51a5132;hp=4489b5225caed5464bd1de7dac232bc22a8b0b62;hpb=47e9dfd89189fc6b150bd6b20229cb047c7e0858;p=tpg%2Facess2.git

diff --git a/Modules/IPStack/link.c b/Modules/IPStack/link.c
index 4489b522..cb59f0f1 100644
--- 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) {
@@ -58,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);
 }
 
@@ -89,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);
@@ -105,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(
@@ -124,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;
+}