X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2FIPStack%2Flink.c;h=8bca51fccb694328a1f3f9ab50a7438923cab477;hb=a2495c6ea4f4cab16b5d339ae511428e92e89e73;hp=4489b5225caed5464bd1de7dac232bc22a8b0b62;hpb=47e9dfd89189fc6b150bd6b20229cb047c7e0858;p=tpg%2Facess2.git diff --git a/Modules/IPStack/link.c b/Modules/IPStack/link.c index 4489b522..8bca51fc 100644 --- a/Modules/IPStack/link.c +++ b/Modules/IPStack/link.c @@ -8,12 +8,22 @@ // === 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; + int giRegisteredTypeSpace = 0; struct { Uint16 Type; tPacketCallback Callback; } *gaRegisteredTypes; + int gbLink_CRCTableGenerated = 0; +Uint32 gaiLink_CRCTable[256]; // === CODE === /** @@ -26,10 +36,11 @@ void Link_RegisterType(Uint16 Type, tPacketCallback Callback) { int i; void *tmp; + for( i = giRegisteredTypes; i -- ; ) { if(gaRegisteredTypes[i].Type == Type) { - Warning("[NET ] Attempt to register 0x%x twice", Type); + Log_Warning("Net Link", "Attempt to register 0x%x twice", Type); return ; } // Ooh! Free slot! @@ -38,14 +49,18 @@ void Link_RegisterType(Uint16 Type, tPacketCallback Callback) if(i == -1) { - tmp = realloc(gaRegisteredTypes, (giRegisteredTypes+1)*sizeof(*gaRegisteredTypes)); + giRegisteredTypeSpace += 5; + tmp = realloc(gaRegisteredTypes, giRegisteredTypeSpace*sizeof(*gaRegisteredTypes)); if(!tmp) { - Warning("[NET ] Out of heap space!"); + Log_Warning("Net Link", + "Out of heap space! (Attempted to allocate %i)", + giRegisteredTypeSpace*sizeof(*gaRegisteredTypes) + ); return ; } + gaRegisteredTypes = tmp; i = giRegisteredTypes; giRegisteredTypes ++; - gaRegisteredTypes = tmp; } gaRegisteredTypes[i].Callback = Callback; @@ -58,60 +73,74 @@ 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+3)&~3) + 4; + Uint8 buf[bufSize]; // dynamic stack arrays ftw! tEthernetHeader *hdr = (void*)buf; + Log_Log("Net Link", "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[bufSize-4] = 0; + *(Uint32*) &hdr->Data[bufSize-4] = htonl( Link_CalculateCRC(buf, bufSize) ); + VFS_Write(Adapter->DeviceFD, bufSize, buf); } -/** - * \fn void Link_WatchDevice(tAdapter *Adapter) - * \brief Spawns a worker thread to watch the specified adapter - */ -void Link_WatchDevice(tAdapter *Adapter) +void Link_WorkerThread(void *Ptr) { - int tid = Proc_SpawnWorker(); // Create a new worker thread - - if(tid < 0) { - Warning("[NET ] Unable to create watcher thread for '%s'", Adapter->Device); - return ; - } - - if(tid > 0) { - Log("[NET ] Watching '%s' using tid %i", Adapter->Device, tid); - return ; - } + tAdapter *Adapter = Ptr; + Threads_SetName(Adapter->Device); + Log_Log("Net Link", "Thread %i watching '%s'", Threads_GetTID(), Adapter->Device); + // 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) + //Log_Debug("NET", "Waiting on adapter FD#0x%x", Adapter->DeviceFD); ret = VFS_Read(Adapter->DeviceFD, MAX_PACKET_SIZE, buf); if(ret == -1) break; - if(ret <= sizeof(tEthernetHeader)) { - Log("[NET ] Recieved an undersized packet"); + if(ret < sizeof(tEthernetHeader)) { + Log_Log("Net Link", "Recieved an undersized packet (%i < %i)", + ret, sizeof(tEthernetHeader)); continue; } + Log_Log("Net Link", + "Packet from %02x:%02x:%02x:%02x:%02x:%02x" + " to %02x:%02x:%02x:%02x:%02x:%02x (Type=%04x)", + 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->Dest.B[3], hdr->Dest.B[4], hdr->Dest.B[5], + ntohs(hdr->Type) + ); + checksum = *(Uint32*)&hdr->Data[ret-sizeof(tEthernetHeader)-4]; + //Log_Log("NET", "Checksum 0x%08x", checksum); + // TODO: Check 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 == ntohs(hdr->Type)) break; } // No? Ignore it - if( i == -1 ) continue; + if( i == -1 ) { + Log_Log("Net Link", "Unregistered type 0x%x", ntohs(hdr->Type)); + continue; + } // Call the callback gaRegisteredTypes[i].Callback( @@ -122,5 +151,75 @@ void Link_WatchDevice(tAdapter *Adapter) ); } - Log("[NET ] Watcher terminated (file closed)"); + Log_Log("Net Link", "Watcher terminated (file closed)"); + + Threads_Exit(0, 0); +} + +/** + * \fn void Link_WatchDevice(tAdapter *Adapter) + * \brief Spawns a worker thread to watch the specified adapter + */ +void Link_WatchDevice(tAdapter *Adapter) +{ + int tid; + + if( !gbLink_CRCTableGenerated ) + Link_InitCRC(); + + tid = Proc_SpawnWorker(Link_WorkerThread, Adapter); // Create a new worker thread + + if(tid < 0) { + Log_Warning("Net Link", "Unable to create watcher thread for '%s'", Adapter->Device); + return ; + } + + Log_Log("Net Link", "Watching '%s' using tid %i", Adapter->Device, tid); +} + +// 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; }