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