X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2FIPStack%2Flink.c;h=8de872923db567310c1dd5ff106141b6ca866912;hb=37eb4d555b07704870b7dc87591d72b5b2539dec;hp=fb219847a9dc177fe307465a796a2d44124cdd9e;hpb=6c5a509b5e14e097ca537c539bc9babe3b8f0c4c;p=tpg%2Facess2.git diff --git a/Modules/IPStack/link.c b/Modules/IPStack/link.c index fb219847..8de87292 100644 --- a/Modules/IPStack/link.c +++ b/Modules/IPStack/link.c @@ -8,39 +8,59 @@ // === 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 === /** * \fn void Link_RegisterType(Uint16 Type, tPacketCallback Callback) * \brief Registers a callback for a specific packet type + * + * \todo Make thread safe (place a mutex on the list) */ 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! if(gaRegisteredTypes[i].Callback == NULL) break; } - if(i + 1 == 0) + if(i == -1) { - tmp = realloc(gaRegisteredTypes, (giRegisteredTypes+1)*sizeof(*gaRegisteredTypes)); - if(!tmp) Panic("[NET ] Out of heap space!"); + giRegisteredTypeSpace += 5; + tmp = realloc(gaRegisteredTypes, giRegisteredTypeSpace*sizeof(*gaRegisteredTypes)); + if(!tmp) { + 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; @@ -53,16 +73,22 @@ 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); } @@ -75,37 +101,62 @@ 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); + Log_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); + Log_Log("Net Link", "Watching '%s' using tid %i", Adapter->Device, tid); return ; } + if( !gbLink_CRCTableGenerated ) + Link_InitCRC(); + + 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 + // 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 <= (int)sizeof(tEthernetHeader)) { + Log_Log("NET", "Recieved an undersized packet"); 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 == 0 ) continue; + if( i == -1 ) { + Log_Log("Net Link", "Unregistered type 0x%x", ntohs(hdr->Type)); + continue; + } // Call the callback gaRegisteredTypes[i].Callback( @@ -115,4 +166,55 @@ void Link_WatchDevice(tAdapter *Adapter) hdr->Data ); } + + Log_Log("NET", "Watcher terminated (file closed)"); + + Threads_Exit(0, 0); +} + +// 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; }