KernelLand/Modules/IPStack/link.c

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