KernelLand/Modules/IPStack/link.c

   1 /*
   2  * Acess2 IP Stack
   3  * - Link/Media Layer Interface
   4  */
   5 #include "ipstack.h"
   6 #include "link.h"
   7
   8 // === CONSTANTS ===
   9 #define MAX_PACKET_SIZE 2048
  10
  11 // === PROTOTYPES ===
  12 void    Link_RegisterType(Uint16 Type, tPacketCallback Callback);
  13 void    Link_InitCRC();
  14 Uint32  Link_CalculateCRC(void *Data, int Length);
  15 void    Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, int Length, void *Buffer);
  16 void    Link_WatchDevice(tAdapter *Adapter);
  17
  18 // === GLOBALS ===
  19  int    giRegisteredTypes = 0;
  20  int    giRegisteredTypeSpace = 0;
  21 struct {
  22         Uint16  Type;
  23         tPacketCallback Callback;
  24 }       *gaRegisteredTypes;
  25  int    gbLink_CRCTableGenerated = 0;
  26 Uint32  gaiLink_CRCTable[256];
  27
  28 // === CODE ===
  29 /**
  30  * \fn void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
  31  * \brief Registers a callback for a specific packet type
  32  *
  33  * \todo Make thread safe (place a mutex on the list)
  34  */
  35 void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
  36 {
  37          int    i;
  38         void    *tmp;
  39
  40         for( i = giRegisteredTypes; i -- ; )
  41         {
  42                 if(gaRegisteredTypes[i].Type == Type) {
  43                         Log_Warning("Net Link", "Attempt to register 0x%x twice", Type);
  44                         return ;
  45                 }
  46                 // Ooh! Free slot!
  47                 if(gaRegisteredTypes[i].Callback == NULL)       break;
  48         }
  49
  50         if(i == -1)
  51         {
  52                 giRegisteredTypeSpace += 5;
  53                 tmp = realloc(gaRegisteredTypes, giRegisteredTypeSpace*sizeof(*gaRegisteredTypes));
  54                 if(!tmp) {
  55                         Log_Warning("Net Link",
  56                                 "Out of heap space! (Attempted to allocate %i)",
  57                                 giRegisteredTypeSpace*sizeof(*gaRegisteredTypes)
  58                                 );
  59                         return ;
  60                 }
  61                 gaRegisteredTypes = tmp;
  62                 i = giRegisteredTypes;
  63                 giRegisteredTypes ++;
  64         }
  65
  66         gaRegisteredTypes[i].Callback = Callback;
  67         gaRegisteredTypes[i].Type = Type;
  68 }
  69
  70 /**
  71  * \fn void Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, int Length, void *Buffer)
  72  * \brief Formats and sends a packet on the specified interface
  73  */
  74 void Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, int Length, void *Buffer)
  75 {
  76          int    bufSize = sizeof(tEthernetHeader) + ((Length+3)&~3) + 4;
  77         Uint8   buf[bufSize];   // dynamic stack arrays ftw!
  78         tEthernetHeader *hdr = (void*)buf;
  79
  80         Log_Log("Net Link", "Sending %i bytes to %02x:%02x:%02x:%02x:%02x:%02x (Type 0x%x)",
  81                 Length, To.B[0], To.B[1], To.B[2], To.B[3], To.B[4], To.B[5], Type);
  82
  83         hdr->Dest = To;
  84         hdr->Src = Adapter->MacAddr;
  85         hdr->Type = htons(Type);
  86
  87         memcpy(hdr->Data, Buffer, Length);
  88
  89         *(Uint32*) &hdr->Data[bufSize-4] = 0;
  90         *(Uint32*) &hdr->Data[bufSize-4] = htonl( Link_CalculateCRC(buf, bufSize) );
  91
  92         VFS_Write(Adapter->DeviceFD, bufSize, buf);
  93 }
  94
  95 void Link_WorkerThread(void *Ptr)
  96 {
  97         tAdapter        *Adapter = Ptr;
  98
  99         Threads_SetName(Adapter->Device);
 100         Log_Log("Net Link", "Thread %i watching '%s'", Threads_GetTID(), Adapter->Device);
 101
 102         // Child Thread
 103         while(Adapter->DeviceFD != -1)
 104         {
 105                 Uint8   buf[MAX_PACKET_SIZE];
 106                 tEthernetHeader *hdr = (void*)buf;
 107                  int    ret, i;
 108                 Uint32  checksum;
 109
 110                 // Wait for a packet (Read on a network device is blocking)
 111                 //Log_Debug("NET", "Waiting on adapter FD#0x%x", Adapter->DeviceFD);
 112                 ret = VFS_Read(Adapter->DeviceFD, MAX_PACKET_SIZE, buf);
 113                 if(ret == -1)   break;
 114
 115                 if(ret < sizeof(tEthernetHeader)) {
 116                         Log_Log("Net Link", "Recieved an undersized packet (%i < %i)",
 117                                 ret, sizeof(tEthernetHeader));
 118                         continue;
 119                 }
 120
 121                 Log_Log("Net Link",
 122                         "Packet from %02x:%02x:%02x:%02x:%02x:%02x"
 123                         " to %02x:%02x:%02x:%02x:%02x:%02x (Type=%04x)",
 124                         hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
 125                         hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5],
 126                         hdr->Dest.B[0], hdr->Dest.B[1], hdr->Dest.B[2],
 127                         hdr->Dest.B[3], hdr->Dest.B[4], hdr->Dest.B[5],
 128                         ntohs(hdr->Type)
 129                         );
 130                 checksum = *(Uint32*)&hdr->Data[ret-sizeof(tEthernetHeader)-4];
 131                 //Log_Log("NET", "Checksum 0x%08x", checksum);
 132                 // TODO: Check checksum
 133
 134                 // Check if there is a registered callback for this packet type
 135                 for( i = giRegisteredTypes; i--; )
 136                 {
 137                         if(gaRegisteredTypes[i].Type == ntohs(hdr->Type))       break;
 138                 }
 139                 // No? Ignore it
 140                 if( i == -1 ) {
 141                         Log_Log("Net Link", "Unregistered type 0x%x", ntohs(hdr->Type));
 142                         continue;
 143                 }
 144
 145                 // Call the callback
 146                 gaRegisteredTypes[i].Callback(
 147                         Adapter,
 148                         hdr->Src,
 149                         ret - sizeof(tEthernetHeader),
 150                         hdr->Data
 151                         );
 152         }
 153
 154         Log_Log("Net Link", "Watcher terminated (file closed)");
 155
 156         Threads_Exit(0, 0);
 157 }
 158
 159 /**
 160  * \fn void Link_WatchDevice(tAdapter *Adapter)
 161  * \brief Spawns a worker thread to watch the specified adapter
 162  */
 163 void Link_WatchDevice(tAdapter *Adapter)
 164 {
 165          int    tid;
 166
 167         if( !gbLink_CRCTableGenerated )
 168                 Link_InitCRC();
 169
 170         tid = Proc_SpawnWorker(Link_WorkerThread, Adapter);     // Create a new worker thread
 171
 172         if(tid < 0) {
 173                 Log_Warning("Net Link", "Unable to create watcher thread for '%s'", Adapter->Device);
 174                 return ;
 175         }
 176
 177         Log_Log("Net Link", "Watching '%s' using tid %i", Adapter->Device, tid);
 178 }
 179
 180 // From http://www.cl.cam.ac.uk/research/srg/bluebook/21/crc/node6.html
 181 #define QUOTIENT        0x04c11db7
 182 void Link_InitCRC(void)
 183 {
 184      int        i, j;
 185     Uint32      crc;
 186
 187     for (i = 0; i < 256; i++)
 188     {
 189         crc = i << 24;
 190         for (j = 0; j < 8; j++)
 191         {
 192             if (crc & 0x80000000)
 193                 crc = (crc << 1) ^ QUOTIENT;
 194             else
 195                 crc = crc << 1;
 196         }
 197         gaiLink_CRCTable[i] = crc;
 198     }
 199
 200         gbLink_CRCTableGenerated = 1;
 201 }
 202
 203 Uint32 Link_CalculateCRC(void *Data, int Length)
 204 {
 205         // x32 + x26 + x23 + x22 + x16 + x12 + x11 + x10 + x8 + x7 + x5 + x4 + x2 + x + 1
 206         Uint32  result;
 207      int        i;
 208         Uint32  *data = Data;
 209
 210     if(Length < 4)      return 0;
 211
 212     result = *data++ << 24;
 213     result |= *data++ << 16;
 214     result |= *data++ << 8;
 215     result |= *data++;
 216     result = ~ result;
 217     Length -= 4;
 218
 219     for( i = 0; i < Length; i++ )
 220     {
 221         result = (result << 8 | *data++) ^ gaiLink_CRCTable[result >> 24];
 222     }
 223
 224     return ~result;
 225 }