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